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 <OgreRay.h>
00031 #include <OgreVector3.h>
00032
00033 #include "rviz/viewport_mouse_event.h"
00034 #include "rviz/load_resource.h"
00035 #include "rviz/render_panel.h"
00036 #include "rviz/display_context.h"
00037 #include "rviz/selection/selection_manager.h"
00038 #include "rviz/view_controller.h"
00039
00040 #include "rviz/default_plugin/tools/point_tool.h"
00041
00042 #include "rviz/properties/bool_property.h"
00043 #include "rviz/properties/string_property.h"
00044
00045 #include <geometry_msgs/PointStamped.h>
00046
00047 #include <sstream>
00048
00049 namespace rviz
00050 {
00051
00052 PointTool::PointTool()
00053 : Tool()
00054 {
00055 topic_property_ = new StringProperty( "Topic", "/clicked_point",
00056 "The topic on which to publish points.",
00057 getPropertyContainer(), SLOT( updateTopic() ), this );
00058
00059 auto_deactivate_property_ = new BoolProperty( "Single click", true,
00060 "Switch away from this tool after one click.",
00061 getPropertyContainer(), SLOT( updateAutoDeactivate() ), this );
00062
00063 updateTopic();
00064 }
00065
00066 PointTool::~PointTool()
00067 {
00068 }
00069
00070 void PointTool::onInitialize()
00071 {
00072 hit_cursor_ = cursor_;
00073 std_cursor_ = getDefaultCursor();
00074 }
00075
00076 void PointTool::activate()
00077 {
00078 }
00079
00080 void PointTool::deactivate()
00081 {
00082 }
00083
00084 void PointTool::updateTopic()
00085 {
00086 pub_ = nh_.advertise<geometry_msgs::PointStamped>( topic_property_->getStdString(), 1 );
00087 }
00088
00089 void PointTool::updateAutoDeactivate()
00090 {
00091 }
00092
00093 int PointTool::processMouseEvent( ViewportMouseEvent& event )
00094 {
00095 int flags = 0;
00096
00097 Ogre::Vector3 pos;
00098 bool success = context_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, pos );
00099 setCursor( success ? hit_cursor_ : std_cursor_ );
00100
00101 if ( success )
00102 {
00103 std::ostringstream s;
00104 s << "<b>Left-Click:</b> Select this point.";
00105 s.precision(3);
00106 s << " [" << pos.x << "," << pos.y << "," << pos.z << "]";
00107 setStatus( s.str().c_str() );
00108
00109 if( event.leftUp() )
00110 {
00111 geometry_msgs::PointStamped ps;
00112 ps.point.x = pos.x;
00113 ps.point.y = pos.y;
00114 ps.point.z = pos.z;
00115 ps.header.frame_id = context_->getFixedFrame().toStdString();
00116 ps.header.stamp = ros::Time::now();
00117 pub_.publish( ps );
00118
00119 if ( auto_deactivate_property_->getBool() )
00120 {
00121 flags |= Finished;
00122 }
00123 }
00124 }
00125 else
00126 {
00127 setStatus( "Move over an object to select the target point." );
00128 }
00129
00130 return flags;
00131 }
00132
00133 }
00134
00135 #include <pluginlib/class_list_macros.h>
00136 PLUGINLIB_EXPORT_CLASS( rviz::PointTool, rviz::Tool )