rviz_interaction_tools: rviz_interaction_tools: cartesian

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 #include "rviz_interaction_tools/cartesian_control.h"
00031 
00032 #include <visualization_msgs/Marker.h>
00033 
00034 #include <rviz/visualization_manager.h>
00035 #include <rviz/frame_manager.h>
00036 #include <rviz/selection/selection_manager.h>
00037 #include <rviz/common.h>
00038 
00039 #include <OGRE/OgreSceneNode.h>
00040 #include <OGRE/OgreSceneManager.h>
00041 #include <OGRE/OgreVector3.h>
00042 #include <OGRE/OgreMaterialManager.h>
00043 #include <OGRE/OgreResourceGroupManager.h>
00044 #include <OGRE/OgreSubEntity.h>
00045 #include <OGRE/OgreMath.h>
00046 
00047 #include "rviz_interaction_tools/unique_string_manager.h"
00048 
00049 #include <math.h>
00050 
00051 namespace rviz_interaction_tools
00052 {
00053 
00054 
00055 CartesianControl::CartesianControl( Ogre::SceneNode *scene_node, rviz::VisualizationManager *vis_manager ) :
00056     vis_manager_( vis_manager ),
00057     fixed_controls_orientation_(false),
00058     status_(HIDDEN),
00059     position_(Ogre::Vector3::ZERO),
00060     orientation_(Ogre::Quaternion::IDENTITY)
00061 {
00062   main_node_ = scene_node->createChildSceneNode();
00063   main_node_->setVisible(false);
00064 
00065   controls_node_ = main_node_->createChildSceneNode();
00066 
00067   addControls(0);
00068   addControls(1);
00069   addControls(2);
00070 
00071   marker_ = new ogre_tools::Shape( ogre_tools::Shape::Sphere, vis_manager->getSceneManager(), main_node_ );
00072   marker_->setScale( Ogre::Vector3( 0.01,0.01,0.01 ) );
00073 }
00074 
00075 CartesianControl::~CartesianControl()
00076 {
00077   vis_manager_->getSceneManager()->destroySceneNode(main_node_);
00078   vis_manager_->getSceneManager()->destroySceneNode(controls_node_);
00079 
00080   for ( unsigned i=0; i<3; i++ )
00081   {
00082     delete drag_boxes_[i][POS];
00083     delete drag_boxes_[i][NEG];
00084   }
00085 }
00086 
00087 void CartesianControl::addControls( unsigned axis )
00088 {
00089   std::vector<Ogre::Vector3> axes;
00090   axes.push_back(Ogre::Vector3::UNIT_X);
00091   axes.push_back(Ogre::Vector3::UNIT_Y);
00092   axes.push_back(Ogre::Vector3::UNIT_Z);
00093 
00094   drag_boxes_[axis][POS] = addDragBox( axes[axis] );
00095   drag_boxes_[axis][NEG] = addDragBox( -1 * axes[axis] );
00096 
00097   Ogre::MaterialPtr material = drag_boxes_[axis][POS]->getEntity()->getSubEntity(0)->getMaterial();
00098   material->getBestTechnique()->setCullingMode( Ogre::CULL_NONE );
00099   material->getBestTechnique()->setDepthWriteEnabled( true );
00100   material->getBestTechnique()->setDepthCheckEnabled( true );
00101 
00102   addRing( axes[(axis+1)%3], axes[(axis+2)%3],
00103       material, drag_boxes_[axis][POS]->getEntity()->getRenderQueueGroup() );
00104 }
00105 
00106 ogre_tools::Shape* CartesianControl::addDragBox( Ogre::Vector3 axis )
00107 {
00108   ogre_tools::Shape *shape =
00109       new ogre_tools::Shape( ogre_tools::Shape::Cube, vis_manager_->getSceneManager(), controls_node_ );
00110 
00111   shape->setPosition( axis * (HANDLE_RADIUS_OUTER + HANDLE_SIZE) );
00112   shape->setScale( Ogre::Vector3(HANDLE_SIZE,HANDLE_SIZE,HANDLE_SIZE) );
00113   shape->setColor( fabs(axis.x), fabs(axis.y), fabs(axis.z), HANDLE_ALPHA );
00114 
00115   //put everything into a higher render queue to transparency will work
00116   shape->getEntity()->setRenderQueueGroup( Ogre::RENDER_QUEUE_6 );
00117 
00118   Ogre::MaterialPtr material = shape->getEntity()->getSubEntity(0)->getMaterial();
00119   material->getBestTechnique()->setCullingMode( Ogre::CULL_NONE );
00120   material->getBestTechnique()->setDepthWriteEnabled( true );
00121   material->getBestTechnique()->setDepthCheckEnabled( true );
00122 //  material->getBestTechnique()->setLightingEnabled(false);
00123 
00124   return shape;
00125 }
00126 
00127 void CartesianControl::addRing(
00128     Ogre::Vector3 axis1, Ogre::Vector3 axis2,
00129     Ogre::MaterialPtr material, unsigned render_queue_group)
00130 {
00131   static const float pi = 3.14159265;
00132   for ( float a=0; a<2.0*pi; a+=0.5*pi )
00133   {
00134     addRingSegment( axis1, axis2, material, render_queue_group, a, a+pi*0.5 );
00135   }
00136 }
00137 
00138 void CartesianControl::addRingSegment(
00139     Ogre::Vector3 axis1, Ogre::Vector3 axis2,
00140     Ogre::MaterialPtr material, unsigned render_queue_group,
00141     float a_min, float a_max )
00142 {
00143   rviz_interaction_tools::MeshObject *mesh_object =
00144       new rviz_interaction_tools::MeshObject( vis_manager_->getSceneManager(), controls_node_ );
00145 
00146   Ogre::Vector3 axis0 = Ogre::Vector3(1,1,1) - axis1 - axis2;
00147 
00148   std::vector< rviz_interaction_tools::MeshObject::Point > points;
00149 
00150   int steps = 20;
00151   for ( int i=0; i<=steps; i++ )
00152   {
00153     float a = a_min + (float(i)/float(steps) * (a_max-a_min) );
00154     Ogre::Vector3 p = (sin(a)*axis1 + cos(a)*axis2);
00155     Ogre::Vector3 p1 = p * (HANDLE_RADIUS_INNER);
00156     Ogre::Vector3 p2 = p * (HANDLE_RADIUS_OUTER);
00157     rviz_interaction_tools::MeshObject::Point p_inner = { p1.x, p1.y, p1.z, axis0.x, axis0.y, axis0.z, HANDLE_ALPHA };
00158     rviz_interaction_tools::MeshObject::Point p_outer = { p2.x, p2.y, p2.z, axis0.x, axis0.y, axis0.z, HANDLE_ALPHA };
00159     points.push_back(p_inner);
00160     points.push_back(p_outer);
00161   }
00162 
00163   std::vector< unsigned > triangles;
00164 
00165   for ( unsigned i=0; i<points.size()-2; i++ )
00166   {
00167     triangles.push_back(i);
00168     triangles.push_back(i+1);
00169     triangles.push_back(i+2);
00170   }
00171 
00172   rviz_interaction_tools::UniqueStringManager usm;
00173   mesh_object->loadMesh( usm.unique("CartesianControl_disc"), points, triangles );
00174 
00175   mesh_object->getEntity()->setMaterial( material );
00176   mesh_object->getEntity()->setRenderQueueGroup( render_queue_group );
00177 }
00178 
00179 void CartesianControl::show()
00180 {
00181   status_ = IDLE;
00182   main_node_->setVisible(true);
00183 }
00184 
00185 void CartesianControl::hide()
00186 {
00187   main_node_->setVisible(false);
00188 
00189   if ( status_ == HIDDEN )
00190   {
00191     return;
00192   }
00193 
00194   status_ = HIDDEN;
00195 }
00196 
00197 void CartesianControl::setPosition( Ogre::Vector3 position )
00198 {
00199   position_ = position;
00200 }
00201 
00202 void CartesianControl::setOrientation( Ogre::Quaternion orientation )
00203 {
00204   orientation_ = orientation;
00205 }
00206 
00207 Ogre::Vector3 CartesianControl::getPosition()
00208 {
00209   return position_;
00210 }
00211 
00212 Ogre::Quaternion CartesianControl::getOrientation()
00213 {
00214   return orientation_;
00215 }
00216 
00217 void CartesianControl::setControlsPosition( Ogre::Vector3 position )
00218 {
00219   controls_node_->setPosition( position );
00220 }
00221 
00222 void CartesianControl::setControlsOrientation( Ogre::Quaternion orientation )
00223 {
00224   controls_node_->setOrientation( orientation );
00225 }
00226 
00227 Ogre::Vector3 CartesianControl::getControlsPosition()
00228 {
00229   return controls_node_->getPosition();
00230 }
00231 
00232 Ogre::Quaternion CartesianControl::getControlsOrientation()
00233 {
00234   return controls_node_->getOrientation();
00235 }
00236 
00237 void CartesianControl::update()
00238 {
00239 }
00240 
00241 float CartesianControl::getClosestIntersection( Ogre::Ray mouse_ray )
00242 {
00243   Ogre::Vector3 disc_intersection_3d;
00244   Ogre::Vector2 disc_intersection_2d;
00245   float disc_ray_t;
00246   unsigned disc_axis;
00247 
00248   Ogre::Vector3 box_intersection_3d;
00249   float box_intersection_1d;
00250   float box_ray_t;
00251   unsigned box_axis;
00252   SideT box_side;
00253 
00254   if ( !getClosestRing( mouse_ray, disc_intersection_3d, disc_intersection_2d, disc_ray_t, disc_axis )
00255       && !getClosestBox( mouse_ray, box_intersection_3d, box_intersection_1d, box_ray_t, box_axis, box_side ) )
00256     return NO_INTERSECTION;
00257 
00258   //choose what's closer to camera, box or ring
00259   return disc_ray_t < box_ray_t ? disc_ray_t : box_ray_t;
00260 }
00261 
00262 
00263 void CartesianControl::mouseMove( Ogre::Ray mouse_ray )
00264 {
00265   Ogre::Vector3 intersection_3d;
00266   Ogre::Vector2 intersection_2d;
00267   float intersection_1d;
00268   float ray_t;
00269 
00270   switch ( status_ )
00271   {
00272     case HIDDEN:
00273     case IDLE:
00274     {
00275       unsigned axis;
00276       getClosestRing( mouse_ray, intersection_3d, intersection_2d, ray_t, axis );
00277 //      getClosestBox( mouse_ray, intersection_3d, intersection_1d, ray_t, axis, side );
00278       marker_->setPosition( intersection_3d );
00279       break;
00280     }
00281 
00282     case ROTATING:
00283     {
00284       if ( intersectRing( mouse_ray, last_axis_, intersection_3d, intersection_2d, ray_t,
00285           HANDLE_RADIUS_INNER*0.5, HANDLE_RADIUS_OUTER*100 ) )
00286       {
00287         double angle = atan2( intersection_2d.x, intersection_2d.y );
00288         Ogre::Radian delta_angle( (last_angle_-angle) );
00289         Ogre::Vector3 axis_vec = getControlsOrientation() * getAxis( last_axis_ );
00290         Ogre::Quaternion delta_orientation( delta_angle, axis_vec );
00291 
00292         Ogre::Vector3 offset = getPosition() - getControlsPosition();
00293 
00294         //rotate controlled frame around controls
00295         orientation_ = delta_orientation * orientation_;
00296         position_ = getControlsPosition() + delta_orientation * offset;
00297 
00298         if ( fixed_controls_orientation_ )
00299         {
00300           last_angle_ = angle;
00301         }
00302         else
00303         {
00304           controls_node_->setOrientation( delta_orientation * controls_node_->getOrientation() );
00305         }
00306 
00307         update();
00308         ROS_DEBUG( "delta angle for axis %d = %f (%f - %f)", last_axis_, angle-last_angle_, angle, last_angle_ );
00309 
00310         Ogre::Vector3 diff = intersection_3d - getControlsPosition();
00311         diff.normalise();
00312         diff *= (HANDLE_RADIUS_OUTER+HANDLE_RADIUS_INNER) / 2.0;
00313         marker_->setPosition( getControlsPosition() + diff );
00314       }
00315       else
00316       {
00317         //make sure the angle doesn't jump when we get back to the ring
00318         last_angle_ = atan2( intersection_2d.x, intersection_2d.y );
00319       }
00320       break;
00321     }
00322 
00323     case DRAGGING:
00324     {
00325       getClosestPosition( mouse_ray, last_axis_, intersection_1d );
00326       {
00327         float delta = intersection_1d - last_drag_pos_;
00328         Ogre::Vector3 translate_delta = getControlsOrientation() * getAxis(last_axis_) * delta;
00329         position_ = position_ + translate_delta;
00330         controls_node_->setPosition( controls_node_->getPosition() + translate_delta );
00331 
00332         update();
00333 
00334         marker_->setPosition( getControlsPosition() + getControlsOrientation() * getAxis(last_axis_) * intersection_1d );
00335       }
00336       break;
00337     }
00338   }
00339 }
00340 
00341 void CartesianControl::mouseDown( Ogre::Ray mouse_ray )
00342 {
00343   Ogre::Vector3 disc_intersection_3d;
00344   Ogre::Vector2 disc_intersection_2d;
00345   float disc_ray_t;
00346   unsigned disc_axis;
00347 
00348   Ogre::Vector3 box_intersection_3d;
00349   float box_intersection_1d;
00350   float box_ray_t;
00351   unsigned box_axis;
00352   SideT box_side;
00353 
00354   getClosestRing( mouse_ray, disc_intersection_3d, disc_intersection_2d, disc_ray_t, disc_axis );
00355   getClosestBox( mouse_ray, box_intersection_3d, box_intersection_1d, box_ray_t, box_axis, box_side );
00356 
00357   if ( disc_ray_t == NO_INTERSECTION && box_ray_t == NO_INTERSECTION )
00358     return;
00359 
00360   //choose what's closer to camera, box or ring
00361   if ( disc_ray_t < box_ray_t )
00362   {
00363     //choose disc
00364     if ( fabs( (getControlsOrientation()*getAxis(disc_axis)).dotProduct( mouse_ray.getDirection() )) < 0.01 )
00365     {
00366       //if the ray points at the plane at a steep angle, it's too dangerous
00367       ROS_DEBUG("the ray points at the plane at a steep angle, that's too dangerous");
00368       return;
00369     }
00370 
00371     ROS_DEBUG("Rotating around axis %d", disc_axis);
00372     marker_->setPosition( disc_intersection_3d );
00373     status_ = ROTATING;
00374     last_angle_ = atan2( disc_intersection_2d.x, disc_intersection_2d.y );
00375     last_axis_ = disc_axis;
00376   }
00377   else
00378   {
00379     //choose box
00380     if ( fabs( (getControlsOrientation()*getAxis(box_axis)).dotProduct( mouse_ray.getDirection() )) > 0.99 )
00381     {
00382       ROS_DEBUG("the ray points at the line at a steep angle, that's too dangerous");
00383       return;
00384     }
00385 
00386     marker_->setPosition( box_intersection_3d );
00387     status_ = DRAGGING;
00388     last_drag_pos_ = box_intersection_1d;
00389     last_drag_side_ = box_side;
00390     last_axis_ = box_axis;
00391     ROS_DEBUG("Dragging axis %d t=%f", box_axis, last_drag_pos_);
00392   }
00393 
00394 }
00395 
00396 void CartesianControl::mouseUp( Ogre::Ray mouse_ray )
00397 {
00398   switch ( status_ )
00399   {
00400     case IDLE:
00401     case HIDDEN:
00402 
00403       break;
00404 
00405     case ROTATING:
00406     case DRAGGING:
00407 
00408       status_ = IDLE;
00409       break;
00410   }
00411 }
00412 
00413 
00414 bool CartesianControl::getClosestRing( Ogre::Ray mouse_ray,
00415     Ogre::Vector3 &nearest_intersection_3d, Ogre::Vector2 &nearest_intersection_2d,
00416     float &nearest_t, unsigned &nearest_axis )
00417 {
00418   nearest_t = NO_INTERSECTION;
00419   bool found = false;
00420 
00421   for ( int axis = 0; axis < 3; axis++ )
00422   {
00423     Ogre::Vector3 intersection_3d;
00424     Ogre::Vector2 intersection_2d;
00425     float ray_t;
00426     if ( intersectRing( mouse_ray, axis, intersection_3d, intersection_2d, ray_t ) && ray_t < nearest_t )
00427     {
00428       nearest_axis = axis;
00429       nearest_t = ray_t;
00430       nearest_intersection_3d = intersection_3d;
00431       nearest_intersection_2d = intersection_2d;
00432       found = true;
00433     }
00434   }
00435 
00436 //  marker_->setPosition( nearest_intersection_3d );
00437   return found;
00438 }
00439 
00440 
00441 bool CartesianControl::intersectRing( Ogre::Ray mouse_ray, unsigned axis,
00442     Ogre::Vector3 &intersection_3d, Ogre::Vector2 &intersection_2d, float &ray_t,
00443     float inner_radius, float outer_radius )
00444 {
00445   return ( intersectPlane( mouse_ray, axis, intersection_3d, intersection_2d, ray_t )
00446       && intersection_2d.length() > inner_radius
00447       && intersection_2d.length() < outer_radius );
00448 }
00449 
00450 
00451 bool CartesianControl::intersectPlane( Ogre::Ray mouse_ray, unsigned axis,
00452     Ogre::Vector3 &intersection_3d, Ogre::Vector2 &intersection_2d, float &ray_t )
00453 {
00454   Ogre::Vector3 position = getControlsPosition();
00455   Ogre::Quaternion orientation = getControlsOrientation();
00456 
00457   Ogre::Vector3 normal = orientation * getAxis(axis);
00458   Ogre::Vector3 x_axis = orientation * getAxis(axis+1);
00459   Ogre::Vector3 y_axis = orientation * getAxis(axis+2);
00460 
00461   Ogre::Plane plane( normal, position );
00462 
00463 
00464   Ogre::Vector2 origin_2d( position.dotProduct( x_axis ), position.dotProduct( y_axis ) );
00465 
00466   std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects( plane );
00467   if ( intersection.first )
00468   {
00469     intersection_3d = mouse_ray.getPoint( intersection.second );
00470     intersection_2d = Ogre::Vector2( intersection_3d.dotProduct(x_axis), intersection_3d.dotProduct(y_axis) );
00471     intersection_2d -= origin_2d;
00472 
00473     ray_t = intersection.second;
00474     return true;
00475   }
00476 
00477   ray_t = 0;
00478   return false;
00479 }
00480 
00481 
00482 bool CartesianControl::getClosestBox( Ogre::Ray mouse_ray,
00483     Ogre::Vector3 &nearest_intersection_3d, float &nearest_intersection_1d,
00484     float &nearest_t, unsigned &nearest_axis, SideT &nearest_side )
00485 {
00486   nearest_t = NO_INTERSECTION;
00487   bool found = false;
00488 
00489   for ( int axis = 0; axis < 3; axis++ )
00490   {
00491     SideT sides[] = {POS,NEG};
00492     for ( unsigned s = 0; s < 2; s++ )
00493     {
00494       SideT side = sides[s];
00495       Ogre::Vector3 intersection_3d;
00496       float ray_t;
00497       float intersection_1d;
00498       if ( intersectBox( mouse_ray, axis, side, intersection_3d, intersection_1d, ray_t ) && ray_t < nearest_t )
00499       {
00500         nearest_intersection_1d = intersection_1d;
00501         nearest_intersection_3d = intersection_3d;
00502         nearest_t = ray_t;
00503         nearest_side = side;
00504         nearest_axis = axis;
00505         found = true;
00506       }
00507     }
00508   }
00509 
00510   return found;
00511 }
00512 
00513 
00514 bool CartesianControl::intersectBox( Ogre::Ray mouse_ray, unsigned axis, SideT side,
00515     Ogre::Vector3 &intersection_3d, float &intersection_1d, float &ray_t )
00516 {
00517   ogre_tools::Shape* shape = drag_boxes_[axis][side];
00518 
00519   Ogre::Vector3 position = shape->getRootNode()->convertLocalToWorldPosition(Ogre::Vector3(0,0,0));
00520   Ogre::Quaternion orientation = shape->getRootNode()->convertLocalToWorldOrientation(Ogre::Quaternion());
00521 
00522   ROS_DEBUG_STREAM( "intersectBox axis " << axis << " side " << side << " position " << position << " orientation " << orientation );
00523 
00524   Ogre::PlaneBoundedVolume volume;
00525 
00526   float box_size = HANDLE_SIZE*0.5;
00527 
00528   for ( unsigned i = 0; i<3; i++ )
00529   {
00530     Ogre::Vector3 axis_vec = orientation * getAxis(i);
00531     volume.planes.push_back( Ogre::Plane( -1 * axis_vec, position + (axis_vec*box_size) ) );
00532     volume.planes.push_back( Ogre::Plane( axis_vec, position - (axis_vec*box_size) ) );
00533   }
00534 
00535   std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects( volume );
00536   if ( intersection.first )
00537   {
00538     intersection_3d = mouse_ray.getPoint( intersection.second );
00539     ray_t = intersection.second;
00540     getClosestPosition( mouse_ray, axis, intersection_1d );
00541     return true;
00542   }
00543 
00544   ray_t = 0;
00545   return false;
00546 }
00547 
00548 
00549 
00550 Ogre::Vector3 CartesianControl::getAxis( unsigned axis )
00551 {
00552   axis = axis % 3;
00553   switch ( axis )
00554   {
00555     case 0:
00556       return Ogre::Vector3::UNIT_X;
00557     case 1:
00558       return Ogre::Vector3::UNIT_Y;
00559     case 2:
00560       return Ogre::Vector3::UNIT_Z;
00561     default:
00562       break;
00563   }
00564   return Ogre::Vector3();
00565 }
00566 
00567 
00568 bool CartesianControl::getClosestPosition( Ogre::Ray mouse_ray, unsigned axis, float &pos )
00569 {
00570   Ogre::Vector3 axis_vec = getControlsOrientation()*getAxis(axis);
00571 
00572   //axis2 is perpendicular to mouse ray and axis ray
00573   Ogre::Vector3 axis2 = mouse_ray.getDirection().crossProduct( axis_vec );
00574 
00575   //axis3 is perpendicular to axis and axis2, thus the normal of the plane
00576   //that contains the shortest connection line
00577   Ogre::Vector3 normal = axis2.crossProduct( mouse_ray.getDirection() );
00578 
00579   Ogre::Plane mouse_plane( normal, mouse_ray.getOrigin() );
00580   Ogre::Ray axis_ray( getControlsPosition(), axis_vec );
00581 
00582   std::pair<bool,float> result = axis_ray.intersects( mouse_plane );
00583 
00584   pos = result.second;
00585   return result.first;
00586 }
00587 
00588 
00589 geometry_msgs::PoseStamped CartesianControl::getPose()
00590 {
00591   geometry_msgs::PoseStamped desired_pose;
00592 
00593   desired_pose.header.frame_id = vis_manager_->getFrameManager()->getFixedFrame();
00594   desired_pose.header.stamp = ros::Time::now();
00595 
00596   Ogre::Vector3 position = getPosition();
00597   Ogre::Quaternion orientation = getOrientation();
00598 
00599   //swap axes
00600   rviz::ogreToRobot( position );
00601   rviz::ogreToRobot( orientation );
00602 
00603   desired_pose.pose.position.x = position.x;
00604   desired_pose.pose.position.y = position.y;
00605   desired_pose.pose.position.z = position.z;
00606 
00607   desired_pose.pose.orientation.w = orientation.w;
00608   desired_pose.pose.orientation.x = orientation.x;
00609   desired_pose.pose.orientation.y = orientation.y;
00610   desired_pose.pose.orientation.z = orientation.z;
00611 
00612   return desired_pose;
00613 }
00614 
00615 }
cartesian_control.cpp
