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/PoseStamped.h>
00033
00034 #include "rviz/display_context.h"
00035 #include "rviz/properties/string_property.h"
00036
00037 #include "rviz/default_plugin/tools/goal_tool.h"
00038
00039 namespace rviz
00040 {
00041
00042 GoalTool::GoalTool()
00043 {
00044 shortcut_key_ = 'g';
00045
00046 topic_property_ = new StringProperty( "Topic", "goal",
00047 "The topic on which to publish navigation goals.",
00048 getPropertyContainer(), SLOT( updateTopic() ), this );
00049 }
00050
00051 void GoalTool::onInitialize()
00052 {
00053 PoseTool::onInitialize();
00054 setName( "2D Nav Goal" );
00055 updateTopic();
00056 }
00057
00058 void GoalTool::updateTopic()
00059 {
00060 pub_ = nh_.advertise<geometry_msgs::PoseStamped>( topic_property_->getStdString(), 1 );
00061 }
00062
00063 void GoalTool::onPoseSet(double x, double y, double theta)
00064 {
00065 std::string fixed_frame = context_->getFixedFrame().toStdString();
00066 tf::Quaternion quat;
00067 quat.setRPY(0.0, 0.0, theta);
00068 tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(quat, tf::Point(x, y, 0.0)), ros::Time::now(), fixed_frame);
00069 geometry_msgs::PoseStamped goal;
00070 tf::poseStampedTFToMsg(p, goal);
00071 ROS_INFO("Setting goal: Frame:%s, Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", fixed_frame.c_str(),
00072 goal.pose.position.x, goal.pose.position.y, goal.pose.position.z,
00073 goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w, theta);
00074 pub_.publish(goal);
00075 }
00076
00077 }
00078
00079 #include <pluginlib/class_list_macros.h>
00080 PLUGINLIB_EXPORT_CLASS( rviz::GoalTool, rviz::Tool )