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