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
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
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()
00053 {
00054 name_ = "2D Nav Goal";
00055 shortcut_key_ = 'g';
00056 }
00057
00058 void GoalTool::onInitialize()
00059 {
00060 PoseTool::onInitialize();
00061 setTopic("/move_base_simple/goal");
00062 }
00063
00064 void GoalTool::setTopic(const std::string& topic)
00065 {
00066 topic_ = topic;
00067 pub_ = nh_.advertise<geometry_msgs::PoseStamped>(topic, 1);
00068 }
00069
00070 void GoalTool::onPoseSet(double x, double y, double theta)
00071 {
00072 std::string fixed_frame = manager_->getFixedFrame();
00073 tf::Quaternion quat;
00074 quat.setRPY(0.0, 0.0, theta);
00075 tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(quat, tf::Point(x, y, 0.0)), ros::Time::now(), fixed_frame);
00076 geometry_msgs::PoseStamped goal;
00077 tf::poseStampedTFToMsg(p, goal);
00078 ROS_INFO("Setting goal: Frame:%s, Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", fixed_frame.c_str(),
00079 goal.pose.position.x, goal.pose.position.y, goal.pose.position.z,
00080 goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w, theta);
00081 pub_.publish(goal);
00082 }
00083
00084 void GoalTool::enumerateProperties(PropertyManager* property_manager, const Property*& parent)
00085 {
00086 topic_property_ = property_manager->createProperty<StringProperty>("Topic", "Tool " + getName(), boost::bind(&GoalTool::getTopic, this), boost::bind(&GoalTool::setTopic, this, _1), parent, this);
00087 }
00088
00089 }
00090