pose_tool.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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_tools/camera_base.h"
00037 #include "ogre_tools/arrow.h"
00038 #include "ogre_tools/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 ogre_tools::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       //compute angle in x-y plane
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       //we need base_orient, since the arrow goes along the -z axis by default (for historical reasons)
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       //compute angle in x-y plane
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 


rviz_qt
Author(s): Dave Hershberger
autogenerated on Fri Dec 6 2013 20:56:53