conversions.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, Ioan A. Sucan
00005 *  Copyright (c) 2011-2013, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *********************************************************************/
00035 
00036 /* Author: Ioan Sucan, Dave Coleman */
00037 
00038 #include <moveit/robot_state/conversions.h>
00039 #include <geometric_shapes/shape_operations.h>
00040 #include <eigen_conversions/eigen_msg.h>
00041 #include <boost/lexical_cast.hpp>
00042 
00043 namespace moveit
00044 {
00045 namespace core
00046 {
00047 // ********************************************
00048 // * Internal (hidden) functions
00049 // ********************************************
00050 
00051 namespace
00052 {
00053 static bool _jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state)
00054 {
00055   if (joint_state.name.size() != joint_state.position.size())
00056   {
00057     logError("Different number of names and positions in JointState message: %u, %u",
00058              (unsigned int)joint_state.name.size(), (unsigned int)joint_state.position.size());
00059     return false;
00060   }
00061 
00062   state.setVariableValues(joint_state);
00063 
00064   return true;
00065 }
00066 
00067 static bool _multiDOFJointsToRobotState(const sensor_msgs::MultiDOFJointState& mjs, RobotState& state,
00068                                         const Transforms* tf)
00069 {
00070   std::size_t nj = mjs.joint_names.size();
00071   if (nj != mjs.transforms.size())
00072   {
00073     logError("Different number of names, values or frames in MultiDOFJointState message.");
00074     return false;
00075   }
00076 
00077   bool error = false;
00078   Eigen::Affine3d inv_t;
00079   bool use_inv_t = false;
00080 
00081   if (nj > 0 && !Transforms::sameFrame(mjs.header.frame_id, state.getRobotModel()->getModelFrame()))
00082   {
00083     if (tf)
00084       try
00085       {
00086         // find the transform that takes the given frame_id to the desired fixed frame
00087         const Eigen::Affine3d& t2fixed_frame = tf->getTransform(mjs.header.frame_id);
00088         // we update the value of the transform so that it transforms from the known fixed frame to the desired child
00089         // link
00090         inv_t = t2fixed_frame.inverse();
00091         use_inv_t = true;
00092       }
00093       catch (std::runtime_error&)
00094       {
00095         error = true;
00096       }
00097     else
00098       error = true;
00099 
00100     if (error)
00101       logWarn("The transform for multi-dof joints was specified in frame '%s' but it was not possible to transform "
00102               "that to frame '%s'",
00103               mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str());
00104   }
00105 
00106   for (std::size_t i = 0; i < nj; ++i)
00107   {
00108     const std::string& joint_name = mjs.joint_names[i];
00109     if (!state.getRobotModel()->hasJointModel(joint_name))
00110     {
00111       logWarn("No joint matching multi-dof joint '%s'", joint_name.c_str());
00112       error = true;
00113       continue;
00114     }
00115     Eigen::Affine3d transf;
00116     tf::transformMsgToEigen(mjs.transforms[i], transf);
00117     // if frames do not mach, attempt to transform
00118     if (use_inv_t)
00119       transf = transf * inv_t;
00120 
00121     state.setJointPositions(joint_name, transf);
00122   }
00123 
00124   return !error;
00125 }
00126 
00127 static inline void _robotStateToMultiDOFJointState(const RobotState& state, sensor_msgs::MultiDOFJointState& mjs)
00128 {
00129   const std::vector<const JointModel*>& js = state.getRobotModel()->getMultiDOFJointModels();
00130   mjs.joint_names.clear();
00131   mjs.transforms.clear();
00132   for (std::size_t i = 0; i < js.size(); ++i)
00133   {
00134     geometry_msgs::Transform p;
00135     if (state.dirtyJointTransform(js[i]))
00136     {
00137       Eigen::Affine3d t;
00138       t.setIdentity();
00139       js[i]->computeTransform(state.getJointPositions(js[i]), t);
00140       tf::transformEigenToMsg(t, p);
00141     }
00142     else
00143       tf::transformEigenToMsg(state.getJointTransform(js[i]), p);
00144     mjs.joint_names.push_back(js[i]->getName());
00145     mjs.transforms.push_back(p);
00146   }
00147   mjs.header.frame_id = state.getRobotModel()->getModelFrame();
00148 }
00149 
00150 class ShapeVisitorAddToCollisionObject : public boost::static_visitor<void>
00151 {
00152 public:
00153   ShapeVisitorAddToCollisionObject(moveit_msgs::CollisionObject* obj) : boost::static_visitor<void>(), obj_(obj)
00154   {
00155   }
00156 
00157   void addToObject(const shapes::ShapeMsg& sm, const geometry_msgs::Pose& pose)
00158   {
00159     pose_ = &pose;
00160     boost::apply_visitor(*this, sm);
00161   }
00162 
00163   void operator()(const shape_msgs::Plane& shape_msg) const
00164   {
00165     obj_->planes.push_back(shape_msg);
00166     obj_->plane_poses.push_back(*pose_);
00167   }
00168 
00169   void operator()(const shape_msgs::Mesh& shape_msg) const
00170   {
00171     obj_->meshes.push_back(shape_msg);
00172     obj_->mesh_poses.push_back(*pose_);
00173   }
00174 
00175   void operator()(const shape_msgs::SolidPrimitive& shape_msg) const
00176   {
00177     obj_->primitives.push_back(shape_msg);
00178     obj_->primitive_poses.push_back(*pose_);
00179   }
00180 
00181 private:
00182   moveit_msgs::CollisionObject* obj_;
00183   const geometry_msgs::Pose* pose_;
00184 };
00185 
00186 static void _attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::AttachedCollisionObject& aco)
00187 {
00188   aco.link_name = attached_body.getAttachedLinkName();
00189   aco.detach_posture = attached_body.getDetachPosture();
00190   const std::set<std::string>& touch_links = attached_body.getTouchLinks();
00191   aco.touch_links.clear();
00192   for (std::set<std::string>::const_iterator it = touch_links.begin(); it != touch_links.end(); ++it)
00193     aco.touch_links.push_back(*it);
00194   aco.object.header.frame_id = aco.link_name;
00195   aco.object.id = attached_body.getName();
00196 
00197   aco.object.operation = moveit_msgs::CollisionObject::ADD;
00198   const std::vector<shapes::ShapeConstPtr>& ab_shapes = attached_body.getShapes();
00199   const EigenSTL::vector_Affine3d& ab_tf = attached_body.getFixedTransforms();
00200   ShapeVisitorAddToCollisionObject sv(&aco.object);
00201   aco.object.primitives.clear();
00202   aco.object.meshes.clear();
00203   aco.object.planes.clear();
00204   aco.object.primitive_poses.clear();
00205   aco.object.mesh_poses.clear();
00206   aco.object.plane_poses.clear();
00207   for (std::size_t j = 0; j < ab_shapes.size(); ++j)
00208   {
00209     shapes::ShapeMsg sm;
00210     if (shapes::constructMsgFromShape(ab_shapes[j].get(), sm))
00211     {
00212       geometry_msgs::Pose p;
00213       tf::poseEigenToMsg(ab_tf[j], p);
00214       sv.addToObject(sm, p);
00215     }
00216   }
00217 }
00218 
00219 static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::AttachedCollisionObject& aco, RobotState& state)
00220 {
00221   if (aco.object.operation == moveit_msgs::CollisionObject::ADD)
00222   {
00223     if (!aco.object.primitives.empty() || !aco.object.meshes.empty() || !aco.object.planes.empty())
00224     {
00225       if (aco.object.primitives.size() != aco.object.primitive_poses.size())
00226       {
00227         logError("Number of primitive shapes does not match number of poses in collision object message");
00228         return;
00229       }
00230 
00231       if (aco.object.meshes.size() != aco.object.mesh_poses.size())
00232       {
00233         logError("Number of meshes does not match number of poses in collision object message");
00234         return;
00235       }
00236 
00237       if (aco.object.planes.size() != aco.object.plane_poses.size())
00238       {
00239         logError("Number of planes does not match number of poses in collision object message");
00240         return;
00241       }
00242 
00243       const LinkModel* lm = state.getLinkModel(aco.link_name);
00244       if (lm)
00245       {
00246         std::vector<shapes::ShapeConstPtr> shapes;
00247         EigenSTL::vector_Affine3d poses;
00248 
00249         for (std::size_t i = 0; i < aco.object.primitives.size(); ++i)
00250         {
00251           shapes::Shape* s = shapes::constructShapeFromMsg(aco.object.primitives[i]);
00252           if (s)
00253           {
00254             Eigen::Affine3d p;
00255             tf::poseMsgToEigen(aco.object.primitive_poses[i], p);
00256             shapes.push_back(shapes::ShapeConstPtr(s));
00257             poses.push_back(p);
00258           }
00259         }
00260         for (std::size_t i = 0; i < aco.object.meshes.size(); ++i)
00261         {
00262           shapes::Shape* s = shapes::constructShapeFromMsg(aco.object.meshes[i]);
00263           if (s)
00264           {
00265             Eigen::Affine3d p;
00266             tf::poseMsgToEigen(aco.object.mesh_poses[i], p);
00267             shapes.push_back(shapes::ShapeConstPtr(s));
00268             poses.push_back(p);
00269           }
00270         }
00271         for (std::size_t i = 0; i < aco.object.planes.size(); ++i)
00272         {
00273           shapes::Shape* s = shapes::constructShapeFromMsg(aco.object.planes[i]);
00274           if (s)
00275           {
00276             Eigen::Affine3d p;
00277             tf::poseMsgToEigen(aco.object.plane_poses[i], p);
00278 
00279             shapes.push_back(shapes::ShapeConstPtr(s));
00280             poses.push_back(p);
00281           }
00282         }
00283 
00284         // transform poses to link frame
00285         if (!Transforms::sameFrame(aco.object.header.frame_id, aco.link_name))
00286         {
00287           Eigen::Affine3d t0;
00288           if (state.knowsFrameTransform(aco.object.header.frame_id))
00289             t0 = state.getFrameTransform(aco.object.header.frame_id);
00290           else if (tf && tf->canTransform(aco.object.header.frame_id))
00291             t0 = tf->getTransform(aco.object.header.frame_id);
00292           else
00293           {
00294             t0.setIdentity();
00295             logError("Cannot properly transform from frame '%s'. The pose of the attached body may be incorrect",
00296                      aco.object.header.frame_id.c_str());
00297           }
00298           Eigen::Affine3d t = state.getGlobalLinkTransform(lm).inverse() * t0;
00299           for (std::size_t i = 0; i < poses.size(); ++i)
00300             poses[i] = t * poses[i];
00301         }
00302 
00303         if (shapes.empty())
00304           logError("There is no geometry to attach to link '%s' as part of attached body '%s'", aco.link_name.c_str(),
00305                    aco.object.id.c_str());
00306         else
00307         {
00308           if (state.clearAttachedBody(aco.object.id))
00309             logDebug("The robot state already had an object named '%s' attached to link '%s'. The object was replaced.",
00310                      aco.object.id.c_str(), aco.link_name.c_str());
00311           state.attachBody(aco.object.id, shapes, poses, aco.touch_links, aco.link_name, aco.detach_posture);
00312           logDebug("Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str());
00313         }
00314       }
00315     }
00316     else
00317       logError("The attached body for link '%s' has no geometry", aco.link_name.c_str());
00318   }
00319   else if (aco.object.operation == moveit_msgs::CollisionObject::REMOVE)
00320   {
00321     if (!state.clearAttachedBody(aco.object.id))
00322       logError("The attached body '%s' can not be removed because it does not exist", aco.link_name.c_str());
00323   }
00324   else
00325     logError("Unknown collision object operation: %d", aco.object.operation);
00326 }
00327 
00328 static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::RobotState& robot_state,
00329                                              RobotState& state, bool copy_attached_bodies)
00330 {
00331   bool valid;
00332   const moveit_msgs::RobotState& rs = robot_state;
00333 
00334   if (!rs.is_diff && rs.joint_state.name.empty() && rs.multi_dof_joint_state.joint_names.empty())
00335   {
00336     logError("Found empty JointState message");
00337     return false;
00338   }
00339 
00340   bool result1 = _jointStateToRobotState(robot_state.joint_state, state);
00341   bool result2 = _multiDOFJointsToRobotState(robot_state.multi_dof_joint_state, state, tf);
00342   valid = result1 || result2;
00343 
00344   if (valid && copy_attached_bodies)
00345   {
00346     if (!robot_state.is_diff)
00347       state.clearAttachedBodies();
00348     for (std::size_t i = 0; i < robot_state.attached_collision_objects.size(); ++i)
00349       _msgToAttachedBody(tf, robot_state.attached_collision_objects[i], state);
00350   }
00351 
00352   return valid;
00353 }
00354 }
00355 }
00356 }
00357 
00358 // ********************************************
00359 
00360 // ********************************************
00361 // * Exposed functions
00362 // ********************************************
00363 
00364 bool moveit::core::jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state)
00365 {
00366   bool result = _jointStateToRobotState(joint_state, state);
00367   state.update();
00368   return result;
00369 }
00370 
00371 bool moveit::core::robotStateMsgToRobotState(const moveit_msgs::RobotState& robot_state, RobotState& state,
00372                                              bool copy_attached_bodies)
00373 {
00374   bool result = _robotStateMsgToRobotStateHelper(NULL, robot_state, state, copy_attached_bodies);
00375   state.update();
00376   return result;
00377 }
00378 
00379 bool moveit::core::robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::RobotState& robot_state,
00380                                              RobotState& state, bool copy_attached_bodies)
00381 {
00382   bool result = _robotStateMsgToRobotStateHelper(&tf, robot_state, state, copy_attached_bodies);
00383   state.update();
00384   return result;
00385 }
00386 
00387 void moveit::core::robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::RobotState& robot_state,
00388                                              bool copy_attached_bodies)
00389 {
00390   robotStateToJointStateMsg(state, robot_state.joint_state);
00391   _robotStateToMultiDOFJointState(state, robot_state.multi_dof_joint_state);
00392 
00393   if (copy_attached_bodies)
00394   {
00395     std::vector<const AttachedBody*> attached_bodies;
00396     state.getAttachedBodies(attached_bodies);
00397     robot_state.attached_collision_objects.resize(attached_bodies.size());
00398     for (std::size_t i = 0; i < attached_bodies.size(); ++i)
00399       _attachedBodyToMsg(*attached_bodies[i], robot_state.attached_collision_objects[i]);
00400   }
00401 }
00402 
00403 void moveit::core::robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState& joint_state)
00404 {
00405   const std::vector<const JointModel*>& js = state.getRobotModel()->getSingleDOFJointModels();
00406   joint_state = sensor_msgs::JointState();
00407 
00408   for (std::size_t i = 0; i < js.size(); ++i)
00409   {
00410     joint_state.name.push_back(js[i]->getName());
00411     joint_state.position.push_back(state.getVariablePosition(js[i]->getFirstVariableIndex()));
00412     if (state.hasVelocities())
00413       joint_state.velocity.push_back(state.getVariableVelocity(js[i]->getFirstVariableIndex()));
00414   }
00415 
00416   // if inconsistent number of velocities are specified, discard them
00417   if (joint_state.velocity.size() != joint_state.position.size())
00418     joint_state.velocity.clear();
00419 
00420   joint_state.header.frame_id = state.getRobotModel()->getModelFrame();
00421 }
00422 
00423 bool moveit::core::jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory& trajectory, std::size_t point_id,
00424                                               RobotState& state)
00425 {
00426   if (trajectory.points.empty() || point_id > trajectory.points.size() - 1)
00427   {
00428     logError("Invalid point_id");
00429     return false;
00430   }
00431   if (trajectory.joint_names.empty())
00432   {
00433     logError("No joint names specified");
00434     return false;
00435   }
00436 
00437   state.setVariablePositions(trajectory.joint_names, trajectory.points[point_id].positions);
00438   if (!trajectory.points[point_id].velocities.empty())
00439     state.setVariableVelocities(trajectory.joint_names, trajectory.points[point_id].velocities);
00440   if (!trajectory.points[point_id].accelerations.empty())
00441     state.setVariableAccelerations(trajectory.joint_names, trajectory.points[point_id].accelerations);
00442   if (!trajectory.points[point_id].effort.empty())
00443     state.setVariableEffort(trajectory.joint_names, trajectory.points[point_id].effort);
00444 
00445   return true;
00446 }
00447 
00448 void moveit::core::robotStateToStream(const RobotState& state, std::ostream& out, bool include_header,
00449                                       const std::string& separator)
00450 {
00451   // Output name of variables
00452   if (include_header)
00453   {
00454     for (std::size_t i = 0; i < state.getVariableCount(); ++i)
00455     {
00456       out << state.getVariableNames()[i];
00457 
00458       // Output comma except at end
00459       if (i < state.getVariableCount() - 1)
00460         out << separator;
00461     }
00462     out << std::endl;
00463   }
00464 
00465   // Output values of joints
00466   for (std::size_t i = 0; i < state.getVariableCount(); ++i)
00467   {
00468     out << state.getVariablePositions()[i];
00469 
00470     // Output comma except at end
00471     if (i < state.getVariableCount() - 1)
00472       out << separator;
00473   }
00474   out << std::endl;
00475 }
00476 
00477 void moveit::core::robotStateToStream(const RobotState& state, std::ostream& out,
00478                                       const std::vector<std::string>& joint_groups_ordering, bool include_header,
00479                                       const std::string& separator)
00480 {
00481   std::stringstream headers;
00482   std::stringstream joints;
00483 
00484   for (std::size_t j = 0; j < joint_groups_ordering.size(); ++j)
00485   {
00486     const JointModelGroup* jmg = state.getRobotModel()->getJointModelGroup(joint_groups_ordering[j]);
00487 
00488     // Output name of variables
00489     if (include_header)
00490     {
00491       for (std::size_t i = 0; i < jmg->getVariableCount(); ++i)
00492       {
00493         headers << jmg->getVariableNames()[i] << separator;
00494       }
00495     }
00496 
00497     // Copy the joint positions for each joint model group
00498     std::vector<double> group_variable_positions;
00499     state.copyJointGroupPositions(jmg, group_variable_positions);
00500 
00501     // Output values of joints
00502     for (std::size_t i = 0; i < jmg->getVariableCount(); ++i)
00503     {
00504       joints << group_variable_positions[i] << separator;
00505     }
00506   }
00507 
00508   // Push all headers and joints to our output stream
00509   if (include_header)
00510     out << headers.str() << std::endl;
00511   out << joints.str() << std::endl;
00512 }
00513 
00514 void moveit::core::streamToRobotState(RobotState& state, const std::string& line, const std::string& separator)
00515 {
00516   std::stringstream lineStream(line);
00517   std::string cell;
00518 
00519   // For each item/column
00520   for (std::size_t i = 0; i < state.getVariableCount(); ++i)
00521   {
00522     // Get a variable
00523     if (!std::getline(lineStream, cell, separator[0]))
00524       logError("Missing variable %i", i);
00525 
00526     state.getVariablePositions()[i] = boost::lexical_cast<double>(cell.c_str());
00527   }
00528 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Apr 20 2018 03:31:10