Go to the documentation of this file.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 #include "visualization_manager.h"
00032 #include "properties/property.h"
00033 #include "properties/property_manager.h"
00034
00035 #include "ogre_helpers/camera_base.h"
00036 #include "ogre_helpers/arrow.h"
00037 #include "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( const std::string& name, char shortcut_key, VisualizationManager* manager )
00054 : PoseTool( name, shortcut_key, manager )
00055 {
00056 setTopic("initialpose");
00057 }
00058
00059 InitialPoseTool::~InitialPoseTool()
00060 {
00061 }
00062
00063 void InitialPoseTool::setTopic(const std::string& topic)
00064 {
00065 topic_ = topic;
00066 pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(topic, 1);
00067 }
00068
00069 void InitialPoseTool::onPoseSet(double x, double y, double theta)
00070 {
00071 std::string fixed_frame = manager_->getFixedFrame();
00072 geometry_msgs::PoseWithCovarianceStamped pose;
00073 pose.header.frame_id = fixed_frame;
00074 pose.pose.pose.position.x = x;
00075 pose.pose.pose.position.y = y;
00076
00077 tf::Quaternion quat;
00078 quat.setRPY(0.0, 0.0, theta);
00079 tf::quaternionTFToMsg(quat,
00080 pose.pose.pose.orientation);
00081 pose.pose.covariance[6*0+0] = 0.5 * 0.5;
00082 pose.pose.covariance[6*1+1] = 0.5 * 0.5;
00083 pose.pose.covariance[6*5+5] = M_PI/12.0 * M_PI/12.0;
00084 ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]", x, y, theta, fixed_frame.c_str());
00085 pub_.publish(pose);
00086 }
00087
00088 void InitialPoseTool::enumerateProperties(PropertyManager* property_manager, const CategoryPropertyWPtr& parent)
00089 {
00090 topic_property_ = property_manager->createProperty<StringProperty>("Topic", "Tool " + getName(), boost::bind(&InitialPoseTool::getTopic, this), boost::bind(&InitialPoseTool::setTopic, this, _1), parent, this);
00091 }
00092
00093 }
00094