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 <tf/transform_listener.h>
00031
00032 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00033
00034 #include "rviz/display_context.h"
00035 #include "rviz/properties/string_property.h"
00036
00037 #include "rviz/default_plugin/tools/initial_pose_tool.h"
00038
00039 namespace rviz
00040 {
00041
00042 InitialPoseTool::InitialPoseTool()
00043 {
00044 shortcut_key_ = 'p';
00045
00046 topic_property_ = new StringProperty( "Topic", "initialpose",
00047 "The topic on which to publish initial pose estimates.",
00048 getPropertyContainer(), SLOT( updateTopic() ), this );
00049 }
00050
00051 void InitialPoseTool::onInitialize()
00052 {
00053 PoseTool::onInitialize();
00054 setName( "2D Pose Estimate" );
00055 updateTopic();
00056 }
00057
00058 void InitialPoseTool::updateTopic()
00059 {
00060 pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>( topic_property_->getStdString(), 1 );
00061 }
00062
00063 void InitialPoseTool::onPoseSet(double x, double y, double theta)
00064 {
00065 std::string fixed_frame = context_->getFixedFrame().toStdString();
00066 geometry_msgs::PoseWithCovarianceStamped pose;
00067 pose.header.frame_id = fixed_frame;
00068 pose.pose.pose.position.x = x;
00069 pose.pose.pose.position.y = y;
00070
00071 tf::Quaternion quat;
00072 quat.setRPY(0.0, 0.0, theta);
00073 tf::quaternionTFToMsg(quat,
00074 pose.pose.pose.orientation);
00075 pose.pose.covariance[6*0+0] = 0.5 * 0.5;
00076 pose.pose.covariance[6*1+1] = 0.5 * 0.5;
00077 pose.pose.covariance[6*5+5] = M_PI/12.0 * M_PI/12.0;
00078 ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]", x, y, theta, fixed_frame.c_str());
00079 pub_.publish(pose);
00080 }
00081
00082 }
00083
00084 #include <pluginlib/class_list_macros.h>
00085 PLUGINLIB_EXPORT_CLASS( rviz::InitialPoseTool, rviz::Tool )