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


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