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 "pose_tool.h"
00031
00032 #include "rviz/visualization_manager.h"
00033 #include "rviz/viewport_mouse_event.h"
00034 #include "rviz/properties/property.h"
00035 #include "rviz/properties/property_manager.h"
00036 #include "rviz/ogre_helpers/camera_base.h"
00037 #include "rviz/ogre_helpers/arrow.h"
00038 #include "rviz/ogre_helpers/qt_ogre_render_window.h"
00039 #include "rviz/geometry.h"
00040
00041 #include <geometry_msgs/PoseStamped.h>
00042 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00043
00044 #include <OGRE/OgreRay.h>
00045 #include <OGRE/OgrePlane.h>
00046 #include <OGRE/OgreCamera.h>
00047 #include <OGRE/OgreSceneNode.h>
00048 #include <OGRE/OgreViewport.h>
00049
00050 #include <tf/transform_listener.h>
00051
00052 namespace rviz
00053 {
00054
00055 PoseTool::PoseTool()
00056 : Tool()
00057 , arrow_( NULL )
00058 {
00059 }
00060
00061 PoseTool::~PoseTool()
00062 {
00063 delete arrow_;
00064 }
00065
00066 void PoseTool::onInitialize()
00067 {
00068 arrow_ = new Arrow( scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f );
00069 arrow_->setColor( 0.0f, 1.0f, 0.0f, 1.0f );
00070 arrow_->getSceneNode()->setVisible( false );
00071 }
00072
00073 void PoseTool::activate()
00074 {
00075 state_ = Position;
00076 }
00077
00078 void PoseTool::deactivate()
00079 {
00080 arrow_->getSceneNode()->setVisible( false );
00081 }
00082
00083 int PoseTool::processMouseEvent( ViewportMouseEvent& event )
00084 {
00085 int flags = 0;
00086
00087 if( event.leftDown() )
00088 {
00089 ROS_ASSERT( state_ == Position );
00090
00091 Ogre::Vector3 intersection;
00092 Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
00093 if( getPointOnPlaneFromWindowXY( event.viewport,
00094 ground_plane,
00095 event.x, event.y, intersection ))
00096 {
00097 pos_ = intersection;
00098 arrow_->setPosition( pos_ );
00099
00100 state_ = Orientation;
00101 flags |= Render;
00102 }
00103 }
00104 else if( event.type == QEvent::MouseMove && event.left() )
00105 {
00106 if( state_ == Orientation )
00107 {
00108
00109 Ogre::Vector3 cur_pos;
00110 Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
00111 if( getPointOnPlaneFromWindowXY( event.viewport,
00112 ground_plane,
00113 event.x, event.y, cur_pos ))
00114 {
00115 double angle = atan2( cur_pos.y - pos_.y, cur_pos.x - pos_.x );
00116
00117 arrow_->getSceneNode()->setVisible( true );
00118
00119
00120 Ogre::Quaternion orient_x = Ogre::Quaternion( Ogre::Radian(-Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Y );
00121
00122 arrow_->setOrientation( Ogre::Quaternion( Ogre::Radian(angle), Ogre::Vector3::UNIT_Z ) * orient_x );
00123
00124 flags |= Render;
00125 }
00126 }
00127 }
00128 else if( event.leftUp() )
00129 {
00130 if( state_ == Orientation )
00131 {
00132
00133 Ogre::Vector3 cur_pos;
00134 Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f );
00135 if( getPointOnPlaneFromWindowXY( event.viewport,
00136 ground_plane,
00137 event.x, event.y, cur_pos ))
00138 {
00139 double angle = atan2( cur_pos.y - pos_.y, cur_pos.x - pos_.x );
00140
00141 onPoseSet(pos_.x, pos_.y, angle);
00142
00143 flags |= (Finished|Render);
00144 }
00145 }
00146 }
00147
00148 return flags;
00149 }
00150
00151 }
00152