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 <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       //compute angle in x-y plane
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         //we need base_orient, since the arrow goes along the -z axis by default (for historical reasons)
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       //compute angle in x-y plane
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 


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:31