cartesian_control.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, 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 #ifndef CARTESIAN_CONTROL_H_
00031 #define CARTESIAN_CONTROL_H_
00032 
00033 #include "rviz_interaction_tools/gripper.h"
00034 
00035 #include "rviz/ogre_helpers/shape.h"
00036 #include "rviz_interaction_tools/mesh_object.h"
00037 
00038 #include <OGRE/OgreMaterial.h>
00039 #include <OGRE/OgreRay.h>
00040 
00041 #include <string>
00042 
00043 namespace rviz {
00044 class VisualizationManager;
00045 }
00046 
00047 namespace Ogre {
00048 class SceneNode;
00049 }
00050 
00051 namespace rviz_interaction_tools
00052 {
00053 
00054 // Displays a 6-dof control that can be dragged and rotated using the mouse
00055 // - the position/orientation of the controls being displayed and the
00056 //   position/orientation being controlled do not necessarily have to be identical
00057 // - the orientation of the controls can be fixed
00058 class CartesianControl
00059 {
00060 public:
00061 
00062   static const float NO_INTERSECTION = 99999;
00063 
00064   enum StatusT
00065   {
00066     HIDDEN,
00067     IDLE,
00068     ROTATING,
00069     DRAGGING
00070   };
00071 
00072   CartesianControl( Ogre::SceneNode *scene_node, rviz::VisualizationManager *vis_manager );
00073 
00074   virtual ~CartesianControl();
00075 
00076   //set the position and orientation of the frame being controlled
00077   void setPosition( Ogre::Vector3 position );
00078   void setOrientation( Ogre::Quaternion orientation );
00079 
00080   //get the position and orientation of the frame being controlled
00081   Ogre::Vector3 getPosition();
00082   Ogre::Quaternion getOrientation();
00083 
00084   //set the position and orientation of the controls
00085   void setControlsPosition( Ogre::Vector3 position );
00086   void setControlsOrientation( Ogre::Quaternion orientation );
00087 
00088   void setFixedControlsOrientation( bool use ) { fixed_controls_orientation_ = use; }
00089   bool getFixedControlsOrientation() { return fixed_controls_orientation_; }
00090 
00091   //get the position and orientation of the controls
00092   Ogre::Vector3 getControlsPosition();
00093   Ogre::Quaternion getControlsOrientation( );
00094 
00095   //get the pose of the controlled frame in ROS format
00096   geometry_msgs::PoseStamped getPose();
00097 
00098   void show();
00099   void hide();
00100 
00101   void update();
00102 
00103   void mouseMove( Ogre::Ray mouse_ray );
00104   void mouseDown( Ogre::Ray mouse_ray );
00105   void mouseUp( Ogre::Ray mouse_ray );
00106 
00107   //get the smallest distance between the ray's origin and any of the controls, -1 if there's no intersection
00108   float getClosestIntersection( Ogre::Ray mouse_ray );
00109 
00110 protected:
00111 
00112   static const float HANDLE_RADIUS_OUTER = 0.13;
00113   static const float HANDLE_RADIUS_INNER = 0.10;
00114   static const float HANDLE_SIZE = 0.03;
00115   static const float HANDLE_ALPHA = 0.5;
00116 
00117   enum SideT
00118   {
00119     POS,
00120     NEG
00121   };
00122 
00123   void addControls( unsigned axis );
00124 
00125   rviz::Shape* addDragBox( Ogre::Vector3 axis );
00126 
00127   void addRing(
00128       Ogre::Vector3 axis1, Ogre::Vector3 axis2,
00129       Ogre::MaterialPtr material, unsigned render_queue_group );
00130 
00131   //we need to split the discs into segments because of the z ordering for alpha blending
00132   void addRingSegment(
00133       Ogre::Vector3 axis1, Ogre::Vector3 axis2,
00134       Ogre::MaterialPtr material, unsigned render_queue_group,
00135       float a_min, float a_max );
00136 
00137   bool intersectRing( Ogre::Ray mouse_ray, unsigned axis,
00138       Ogre::Vector3 &intersection_3d, Ogre::Vector2 &intersection_2d, float &ray_t,
00139       float inner_radius=HANDLE_RADIUS_INNER, float outer_radius=HANDLE_RADIUS_OUTER );
00140 
00141   bool intersectPlane( Ogre::Ray mouse_ray, unsigned axis,
00142       Ogre::Vector3 &intersection_3d, Ogre::Vector2 &intersection_2d, float &ray_t );
00143 
00144   bool getClosestRing( Ogre::Ray mouse_ray,
00145       Ogre::Vector3 &nearest_intersection_3d, Ogre::Vector2 &nearest_intersection_2d,
00146       float &nearest_t, unsigned &nearest_axis );
00147 
00148   bool intersectBox( Ogre::Ray mouse_ray, unsigned axis, SideT side,
00149       Ogre::Vector3 &intersection_3d, float &intersection_1d, float &ray_t );
00150 
00151   bool getClosestBox( Ogre::Ray mouse_ray,
00152       Ogre::Vector3 &nearest_intersection_3d, float &nearest_intersection_1d,
00153       float &nearest_t, unsigned &nearest_axis, SideT &nearest_side );
00154 
00155   Ogre::Vector3 getAxis( unsigned axis );
00156 
00157   bool getClosestPosition( Ogre::Ray mouse_ray, unsigned axis, float &pos );
00158 
00159   rviz::VisualizationManager *vis_manager_;
00160 
00161   bool fixed_controls_orientation_;
00162 
00163   Ogre::SceneNode *main_node_;
00164   Ogre::SceneNode *controls_node_;
00165 
00166   // the first key is the axis, the second is the side (1 or -1)
00167   std::map< unsigned, std::map<SideT, rviz::Shape*> > drag_boxes_;
00168 
00169   std::vector<rviz_interaction_tools::MeshObject*> ring_segments_;
00170 
00171   rviz::Shape* marker_;
00172 
00173   StatusT status_;
00174 
00175   double last_angle_;
00176   double last_drag_pos_;
00177   SideT last_drag_side_;
00178   int last_axis_;
00179 
00180   Ogre::Vector3 position_;
00181   Ogre::Quaternion orientation_;
00182 };
00183 
00184 }
00185 
00186 #endif /* CARTESIAN_GRIPPER_CONTROL_H_ */


rviz_interaction_tools
Author(s): David Gossow
autogenerated on Mon Oct 6 2014 03:03:25