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 "goal_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
00041 #include <OGRE/OgreRay.h>
00042 #include <OGRE/OgrePlane.h>
00043 #include <OGRE/OgreCamera.h>
00044 #include <OGRE/OgreSceneNode.h>
00045 #include <OGRE/OgreViewport.h>
00046
00047 #include <tf/transform_listener.h>
00048
00049 namespace rviz
00050 {
00051
00052 GoalTool::GoalTool( const std::string& name, char shortcut_key, VisualizationManager* manager )
00053 : PoseTool( name, shortcut_key, manager )
00054 {
00055 setTopic("/move_base_simple/goal");
00056 }
00057
00058 GoalTool::~GoalTool()
00059 {
00060 }
00061
00062 void GoalTool::setTopic(const std::string& topic)
00063 {
00064 topic_ = topic;
00065 pub_ = nh_.advertise<geometry_msgs::PoseStamped>(topic, 1);
00066 }
00067
00068 void GoalTool::onPoseSet(double x, double y, double theta)
00069 {
00070 std::string fixed_frame = manager_->getFixedFrame();
00071 tf::Quaternion quat;
00072 quat.setRPY(0.0, 0.0, theta);
00073 tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(quat, tf::Point(x, y, 0.0)), ros::Time::now(), fixed_frame);
00074 geometry_msgs::PoseStamped goal;
00075 tf::poseStampedTFToMsg(p, goal);
00076 ROS_INFO("Setting goal: Frame:%s, Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", fixed_frame.c_str(),
00077 goal.pose.position.x, goal.pose.position.y, goal.pose.position.z,
00078 goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w, theta);
00079 pub_.publish(goal);
00080 }
00081
00082 void GoalTool::enumerateProperties(PropertyManager* property_manager, const CategoryPropertyWPtr& parent)
00083 {
00084 topic_property_ = property_manager->createProperty<StringProperty>("Topic", "Tool " + getName(), boost::bind(&GoalTool::getTopic, this), boost::bind(&GoalTool::setTopic, this, _1), parent, this);
00085 }
00086
00087 }
00088