00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "initial_pose_tool.h"
00031
00032 #include "rviz/visualization_manager.h"
00033 #include "rviz/properties/property.h"
00034 #include "rviz/properties/property_manager.h"
00035 #include "rviz/ogre_helpers/camera_base.h"
00036 #include "rviz/ogre_helpers/arrow.h"
00037 #include "rviz/ogre_helpers/qt_ogre_render_window.h"
00038
00039 #include <geometry_msgs/PoseStamped.h>
00040 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00041
00042 #include <OGRE/OgreRay.h>
00043 #include <OGRE/OgrePlane.h>
00044 #include <OGRE/OgreCamera.h>
00045 #include <OGRE/OgreSceneNode.h>
00046 #include <OGRE/OgreViewport.h>
00047
00048 #include <tf/transform_listener.h>
00049
00050 namespace rviz
00051 {
00052
00053 InitialPoseTool::InitialPoseTool()
00054 {
00055 name_ = "2D Pose Estimate";
00056 shortcut_key_ = 'p';
00057 }
00058
00059 void InitialPoseTool::onInitialize()
00060 {
00061 PoseTool::onInitialize();
00062 setTopic("initialpose");
00063 }
00064
00065 void InitialPoseTool::setTopic(const std::string& topic)
00066 {
00067 topic_ = topic;
00068 pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(topic, 1);
00069 }
00070
00071 void InitialPoseTool::onPoseSet(double x, double y, double theta)
00072 {
00073 std::string fixed_frame = manager_->getFixedFrame();
00074 geometry_msgs::PoseWithCovarianceStamped pose;
00075 pose.header.frame_id = fixed_frame;
00076 pose.pose.pose.position.x = x;
00077 pose.pose.pose.position.y = y;
00078
00079 tf::Quaternion quat;
00080 quat.setRPY(0.0, 0.0, theta);
00081 tf::quaternionTFToMsg(quat,
00082 pose.pose.pose.orientation);
00083 pose.pose.covariance[6*0+0] = 0.5 * 0.5;
00084 pose.pose.covariance[6*1+1] = 0.5 * 0.5;
00085 pose.pose.covariance[6*5+5] = M_PI/12.0 * M_PI/12.0;
00086 ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]", x, y, theta, fixed_frame.c_str());
00087 pub_.publish(pose);
00088 }
00089
00090 void InitialPoseTool::enumerateProperties(PropertyManager* property_manager, const Property*& parent)
00091 {
00092 topic_property_ = property_manager->createProperty<StringProperty>("Topic", "Tool " + getName(), boost::bind(&InitialPoseTool::getTopic, this), boost::bind(&InitialPoseTool::setTopic, this, _1), parent, this);
00093 }
00094
00095 }
00096