00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/robot_state/conversions.h>
00038 #include <geometric_shapes/shape_operations.h>
00039 #include <eigen_conversions/eigen_msg.h>
00040
00041 namespace robot_state
00042 {
00043
00044
00045
00046
00047
00048 namespace
00049 {
00050
00051 static bool _jointStateToRobotState(const sensor_msgs::JointState &joint_state, RobotState& state, std::set<std::string> *missing)
00052 {
00053 if (joint_state.name.size() != joint_state.position.size())
00054 {
00055 logError("Different number of names and positions in JointState message: %u, %u",
00056 (unsigned int)joint_state.name.size(), (unsigned int)joint_state.position.size());
00057 return false;
00058 }
00059
00060 std::map<std::string, double> joint_state_map;
00061 for (unsigned int i = 0 ; i < joint_state.name.size(); ++i)
00062 joint_state_map[joint_state.name[i]] = joint_state.position[i];
00063
00064 if (missing == NULL)
00065 state.setStateValues(joint_state_map);
00066 else
00067 {
00068 std::vector<std::string> missing_variables;
00069 state.setStateValues(joint_state_map, missing_variables);
00070 missing->clear();
00071 for (unsigned int i = 0; i < missing_variables.size(); ++i)
00072 missing->insert(missing_variables[i]);
00073 }
00074
00075
00076 if (!joint_state.velocity.empty())
00077 for (unsigned int i = 0 ; i < joint_state.name.size(); ++i)
00078 {
00079 JointState *js = state.getJointState(joint_state.name[i]);
00080 if (js)
00081 {
00082 js->getVelocities().resize(1);
00083 js->getVelocities()[0] = joint_state.velocity[i];
00084 }
00085 }
00086
00087 return true;
00088 }
00089
00090 static bool multiDOFJointsToRobotState(const moveit_msgs::MultiDOFJointState &mjs, RobotState& state, const Transforms *tf)
00091 {
00092 std::size_t nj = mjs.joint_names.size();
00093 if (nj != mjs.joint_transforms.size())
00094 {
00095 logError("Different number of names, values or frames in MultiDOFJointState message.");
00096 return false;
00097 }
00098
00099 bool error = false;
00100 Eigen::Affine3d inv_t;
00101 bool use_inv_t = false;
00102
00103 if (nj > 0 && mjs.header.frame_id != state.getRobotModel()->getModelFrame())
00104 {
00105 if (tf)
00106 try
00107 {
00108
00109 const Eigen::Affine3d &t2fixed_frame = tf->getTransform(mjs.header.frame_id);
00110
00111 inv_t = t2fixed_frame.inverse();
00112 use_inv_t = true;
00113 }
00114 catch (std::runtime_error&)
00115 {
00116 error = true;
00117 }
00118 else
00119 error = true;
00120 if (error)
00121 logWarn("The transform for multi-dof joints was specified in frame '%s' but it was not possible to transform that to frame '%s'",
00122 mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str());
00123 }
00124
00125 for (std::size_t i = 0 ; i < nj ; ++i)
00126 {
00127 const std::string &joint_name = mjs.joint_names[i];
00128 if (!state.hasJointState(joint_name))
00129 {
00130 logWarn("No joint matching multi-dof joint '%s'", joint_name.c_str());
00131 error = true;
00132 continue;
00133 }
00134 Eigen::Affine3d transf;
00135 tf::transformMsgToEigen(mjs.joint_transforms[i], transf);
00136
00137
00138 if (use_inv_t)
00139 transf = transf * inv_t;
00140
00141 JointState *joint_state = state.getJointState(joint_name);
00142 joint_state->setVariableValues(transf);
00143 }
00144
00145 return !error;
00146 }
00147
00148 static inline void robotStateToMultiDOFJointState(const RobotState& state, moveit_msgs::MultiDOFJointState &mjs)
00149 {
00150
00151 const std::vector<JointState*> &js = state.getJointStateVector();
00152 mjs.joint_names.clear();
00153 mjs.joint_transforms.clear();
00154 for (std::size_t i = 0 ; i < js.size() ; ++i)
00155 if (js[i]->getVariableCount() > 1)
00156 {
00157 geometry_msgs::Transform p;
00158 tf::transformEigenToMsg(js[i]->getVariableTransform(), p);
00159 mjs.joint_names.push_back(js[i]->getName());
00160 mjs.joint_transforms.push_back(p);
00161 }
00162 mjs.header.frame_id = state.getRobotModel()->getModelFrame();
00163 }
00164
00165 class ShapeVisitorAddToCollisionObject : public boost::static_visitor<void>
00166 {
00167 public:
00168
00169 ShapeVisitorAddToCollisionObject(moveit_msgs::CollisionObject *obj) :
00170 boost::static_visitor<void>(), obj_(obj)
00171 {
00172 }
00173
00174 void addToObject(const shapes::ShapeMsg &sm, const geometry_msgs::Pose &pose)
00175 {
00176 pose_ = &pose;
00177 boost::apply_visitor(*this, sm);
00178 }
00179
00180 void operator()(const shape_msgs::Plane &shape_msg) const
00181 {
00182 obj_->planes.push_back(shape_msg);
00183 obj_->plane_poses.push_back(*pose_);
00184 }
00185
00186 void operator()(const shape_msgs::Mesh &shape_msg) const
00187 {
00188 obj_->meshes.push_back(shape_msg);
00189 obj_->mesh_poses.push_back(*pose_);
00190 }
00191
00192 void operator()(const shape_msgs::SolidPrimitive &shape_msg) const
00193 {
00194 obj_->primitives.push_back(shape_msg);
00195 obj_->primitive_poses.push_back(*pose_);
00196 }
00197
00198 private:
00199
00200 moveit_msgs::CollisionObject *obj_;
00201 const geometry_msgs::Pose *pose_;
00202 };
00203
00204 static void attachedBodyToMsg(const AttachedBody &attached_body, moveit_msgs::AttachedCollisionObject &aco)
00205 {
00206 aco.link_name = attached_body.getAttachedLinkName();
00207 aco.detach_posture = attached_body.getDetachPosture();
00208 const std::set<std::string> &touch_links = attached_body.getTouchLinks();
00209 aco.touch_links.clear();
00210 for (std::set<std::string>::const_iterator it = touch_links.begin() ; it != touch_links.end() ; ++it)
00211 aco.touch_links.push_back(*it);
00212 aco.object.header.frame_id = aco.link_name;
00213 aco.object.id = attached_body.getName();
00214
00215 aco.object.operation = moveit_msgs::CollisionObject::ADD;
00216 const std::vector<shapes::ShapeConstPtr>& ab_shapes = attached_body.getShapes();
00217 const EigenSTL::vector_Affine3d& ab_tf = attached_body.getFixedTransforms();
00218 ShapeVisitorAddToCollisionObject sv(&aco.object);
00219 aco.object.primitives.clear();
00220 aco.object.meshes.clear();
00221 aco.object.planes.clear();
00222 aco.object.primitive_poses.clear();
00223 aco.object.mesh_poses.clear();
00224 aco.object.plane_poses.clear();
00225 for (std::size_t j = 0 ; j < ab_shapes.size() ; ++j)
00226 {
00227 shapes::ShapeMsg sm;
00228 if (shapes::constructMsgFromShape(ab_shapes[j].get(), sm))
00229 {
00230 geometry_msgs::Pose p;
00231 tf::poseEigenToMsg(ab_tf[j], p);
00232 sv.addToObject(sm, p);
00233 }
00234 }
00235 }
00236
00237 static void msgToAttachedBody(const Transforms *tf, const moveit_msgs::AttachedCollisionObject &aco, RobotState& state)
00238 {
00239 if (aco.object.operation == moveit_msgs::CollisionObject::ADD)
00240 {
00241 if (!aco.object.primitives.empty() || !aco.object.meshes.empty() || !aco.object.planes.empty())
00242 {
00243 if (aco.object.primitives.size() != aco.object.primitive_poses.size())
00244 {
00245 logError("Number of primitive shapes does not match number of poses in collision object message");
00246 return;
00247 }
00248
00249 if (aco.object.meshes.size() != aco.object.mesh_poses.size())
00250 {
00251 logError("Number of meshes does not match number of poses in collision object message");
00252 return;
00253 }
00254
00255 if (aco.object.planes.size() != aco.object.plane_poses.size())
00256 {
00257 logError("Number of planes does not match number of poses in collision object message");
00258 return;
00259 }
00260
00261 LinkState *ls = state.getLinkState(aco.link_name);
00262 if (ls)
00263 {
00264 std::vector<shapes::ShapeConstPtr> shapes;
00265 EigenSTL::vector_Affine3d poses;
00266
00267 for (std::size_t i = 0 ; i < aco.object.primitives.size() ; ++i)
00268 {
00269 shapes::Shape *s = shapes::constructShapeFromMsg(aco.object.primitives[i]);
00270 if (s)
00271 {
00272 Eigen::Affine3d p;
00273 tf::poseMsgToEigen(aco.object.primitive_poses[i], p);
00274 shapes.push_back(shapes::ShapeConstPtr(s));
00275 poses.push_back(p);
00276 }
00277 }
00278 for (std::size_t i = 0 ; i < aco.object.meshes.size() ; ++i)
00279 {
00280 shapes::Shape *s = shapes::constructShapeFromMsg(aco.object.meshes[i]);
00281 if (s)
00282 {
00283 Eigen::Affine3d p;
00284 tf::poseMsgToEigen(aco.object.mesh_poses[i], p);
00285 shapes.push_back(shapes::ShapeConstPtr(s));
00286 poses.push_back(p);
00287 }
00288 }
00289 for (std::size_t i = 0 ; i < aco.object.planes.size() ; ++i)
00290 {
00291 shapes::Shape *s = shapes::constructShapeFromMsg(aco.object.planes[i]);
00292 if (s)
00293 {
00294 Eigen::Affine3d p;
00295 tf::poseMsgToEigen(aco.object.plane_poses[i], p);
00296
00297 shapes.push_back(shapes::ShapeConstPtr(s));
00298 poses.push_back(p);
00299 }
00300 }
00301
00302
00303 if (aco.object.header.frame_id != aco.link_name)
00304 {
00305 Eigen::Affine3d t0;
00306 if (state.knowsFrameTransform(aco.object.header.frame_id))
00307 t0 = state.getFrameTransform(aco.object.header.frame_id);
00308 else
00309 if (tf && tf->canTransform(aco.object.header.frame_id))
00310 t0 = tf->getTransform(aco.object.header.frame_id);
00311 else
00312 {
00313 t0.setIdentity();
00314 logError("Cannot properly transform from frame '%s'. The pose of the attached body may be incorrect", aco.object.header.frame_id.c_str());
00315 }
00316 Eigen::Affine3d t = ls->getGlobalLinkTransform().inverse() * t0;
00317 for (std::size_t i = 0 ; i < poses.size() ; ++i)
00318 poses[i] = t * poses[i];
00319 }
00320
00321 if (shapes.empty())
00322 logError("There is no geometry to attach to link '%s' as part of attached body '%s'", aco.link_name.c_str(), aco.object.id.c_str());
00323 else
00324 {
00325 if (state.clearAttachedBody(aco.object.id))
00326 logInform("The robot state already had an object named '%s' attached to link '%s'. The object was replaced.",
00327 aco.object.id.c_str(), aco.link_name.c_str());
00328 std::set<std::string> touch_links(aco.touch_links.begin(), aco.touch_links.end());
00329 state.attachBody(aco.object.id, shapes, poses, touch_links, aco.link_name, aco.detach_posture);
00330 logDebug("Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str());
00331 }
00332 }
00333 }
00334 else
00335 logError("The attached body for link '%s' has no geometry", aco.link_name.c_str());
00336 }
00337 else
00338 if (aco.object.operation == moveit_msgs::CollisionObject::REMOVE)
00339 {
00340 state.clearAttachedBody(aco.object.id);
00341 }
00342 else
00343 logError("Unknown collision object operation: %d", aco.object.operation);
00344 }
00345
00346 static bool robotStateMsgToRobotStateHelper(const Transforms *tf, const moveit_msgs::RobotState &robot_state, RobotState& state, bool copy_attached_bodies)
00347 {
00348 std::set<std::string> missing;
00349 bool result1 = _jointStateToRobotState(robot_state.joint_state, state, &missing);
00350 bool result2 = multiDOFJointsToRobotState(robot_state.multi_dof_joint_state, state, tf);
00351 state.updateLinkTransforms();
00352
00353 if (copy_attached_bodies && !robot_state.attached_collision_objects.empty())
00354 {
00355 for (std::size_t i = 0 ; i < robot_state.attached_collision_objects.size() ; ++i)
00356 msgToAttachedBody(tf, robot_state.attached_collision_objects[i], state);
00357 state.updateLinkTransforms();
00358 }
00359
00360 if (result1 && result2)
00361 {
00362 if (!missing.empty())
00363 for (unsigned int i = 0 ; i < robot_state.multi_dof_joint_state.joint_names.size(); ++i)
00364 {
00365 const robot_model::JointModel *jm = state.getRobotModel()->getJointModel(robot_state.multi_dof_joint_state.joint_names[i]);
00366 if (jm)
00367 {
00368 const std::vector<std::string> &vnames = jm->getVariableNames();
00369 for (std::size_t i = 0 ; i < vnames.size(); ++i)
00370 missing.erase(vnames[i]);
00371 }
00372 }
00373
00374 return missing.empty();
00375 }
00376 else
00377 return false;
00378 }
00379
00380 }
00381 }
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392 bool robot_state::jointStateToRobotState(const sensor_msgs::JointState &joint_state, RobotState& state)
00393 {
00394 bool result = _jointStateToRobotState(joint_state, state, NULL);
00395 state.updateLinkTransforms();
00396 return result;
00397 }
00398
00399 bool robot_state::robotStateMsgToRobotState(const moveit_msgs::RobotState &robot_state, RobotState& state, bool copy_attached_bodies)
00400 {
00401 return robotStateMsgToRobotStateHelper(NULL, robot_state, state, copy_attached_bodies);
00402 }
00403
00404 bool robot_state::robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::RobotState &robot_state, RobotState& state, bool copy_attached_bodies)
00405 {
00406 return robotStateMsgToRobotStateHelper(&tf, robot_state, state, copy_attached_bodies);
00407 }
00408
00409 void robot_state::robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies)
00410 {
00411 robotStateToJointStateMsg(state, robot_state.joint_state);
00412 robotStateToMultiDOFJointState(state, robot_state.multi_dof_joint_state);
00413 if (copy_attached_bodies)
00414 {
00415 std::vector<const AttachedBody*> attached_bodies;
00416 state.getAttachedBodies(attached_bodies);
00417 robot_state.attached_collision_objects.resize(attached_bodies.size());
00418 for (std::size_t i = 0 ; i < attached_bodies.size() ; ++i)
00419 attachedBodyToMsg(*attached_bodies[i], robot_state.attached_collision_objects[i]);
00420 }
00421 }
00422
00423 void robot_state::robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState &joint_state)
00424 {
00425 const std::vector<JointState*> &js = state.getJointStateVector();
00426 joint_state = sensor_msgs::JointState();
00427
00428 for (std::size_t i = 0 ; i < js.size() ; ++i)
00429 if (js[i]->getVariableCount() == 1)
00430 {
00431 joint_state.name.push_back(js[i]->getName());
00432 joint_state.position.push_back(js[i]->getVariableValues()[0]);
00433 if (!js[i]->getVelocities().empty())
00434 joint_state.velocity.push_back(js[i]->getVelocities()[0]);
00435 }
00436
00437
00438 if (joint_state.velocity.size() != joint_state.position.size())
00439 joint_state.velocity.clear();
00440
00441 joint_state.header.frame_id = state.getRobotModel()->getModelFrame();
00442 }