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