conversions.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Ioan A. Sucan
5  * Copyright (c) 2011-2013, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Ioan Sucan, Dave Coleman */
37 
40 #include <tf2_eigen/tf2_eigen.h>
41 #include <boost/lexical_cast.hpp>
42 
43 namespace moveit
44 {
45 namespace core
46 {
47 const std::string LOGNAME = "robot_state";
48 
49 // ********************************************
50 // * Internal (hidden) functions
51 // ********************************************
52 
53 namespace
54 {
55 static bool _jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state)
56 {
57  if (joint_state.name.size() != joint_state.position.size())
58  {
59  ROS_ERROR_NAMED(LOGNAME, "Different number of names and positions in JointState message: %zu, %zu",
60  joint_state.name.size(), joint_state.position.size());
61  return false;
62  }
63 
64  state.setVariableValues(joint_state);
65 
66  return true;
67 }
68 
69 static bool _multiDOFJointsToRobotState(const sensor_msgs::MultiDOFJointState& mjs, RobotState& state,
70  const Transforms* tf)
71 {
72  std::size_t nj = mjs.joint_names.size();
73  if (nj != mjs.transforms.size())
74  {
75  ROS_ERROR_NAMED(LOGNAME, "Different number of names, values or frames in MultiDOFJointState message.");
76  return false;
77  }
78 
79  bool error = false;
80  Eigen::Isometry3d inv_t;
81  bool use_inv_t = false;
82 
83  if (nj > 0 && !Transforms::sameFrame(mjs.header.frame_id, state.getRobotModel()->getModelFrame()))
84  {
85  if (tf)
86  try
87  {
88  // find the transform that takes the given frame_id to the desired fixed frame
89  const Eigen::Isometry3d& t2fixed_frame = tf->getTransform(mjs.header.frame_id);
90  // we update the value of the transform so that it transforms from the known fixed frame to the desired child
91  // link
92  inv_t = t2fixed_frame.inverse();
93  use_inv_t = true;
94  }
95  catch (std::exception& ex)
96  {
97  ROS_ERROR_NAMED(LOGNAME, "Caught %s", ex.what());
98  error = true;
99  }
100  else
101  error = true;
102 
103  if (error)
105  "The transform for multi-dof joints was specified in frame '%s' "
106  "but it was not possible to transform that to frame '%s'",
107  mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str());
108  }
109 
110  for (std::size_t i = 0; i < nj; ++i)
111  {
112  const std::string& joint_name = mjs.joint_names[i];
113  if (!state.getRobotModel()->hasJointModel(joint_name))
114  {
115  ROS_WARN_NAMED(LOGNAME, "No joint matching multi-dof joint '%s'", joint_name.c_str());
116  error = true;
117  continue;
118  }
119  Eigen::Isometry3d transf = tf2::transformToEigen(mjs.transforms[i]);
120  // if frames do not mach, attempt to transform
121  if (use_inv_t)
122  transf = transf * inv_t;
123 
124  state.setJointPositions(joint_name, transf);
125  }
126 
127  return !error;
128 }
129 
130 static inline void _robotStateToMultiDOFJointState(const RobotState& state, sensor_msgs::MultiDOFJointState& mjs)
131 {
132  const std::vector<const JointModel*>& js = state.getRobotModel()->getMultiDOFJointModels();
133  mjs.joint_names.clear();
134  mjs.transforms.clear();
135  for (const JointModel* joint_model : js)
136  {
137  geometry_msgs::TransformStamped p;
138  if (state.dirtyJointTransform(joint_model))
139  {
140  Eigen::Isometry3d t;
141  t.setIdentity();
142  joint_model->computeTransform(state.getJointPositions(joint_model), t);
143  p = tf2::eigenToTransform(t);
144  }
145  else
146  p = tf2::eigenToTransform(state.getJointTransform(joint_model));
147  mjs.joint_names.push_back(joint_model->getName());
148  mjs.transforms.push_back(p.transform);
149  }
150  mjs.header.frame_id = state.getRobotModel()->getModelFrame();
151 }
152 
153 class ShapeVisitorAddToCollisionObject : public boost::static_visitor<void>
154 {
155 public:
156  ShapeVisitorAddToCollisionObject(moveit_msgs::CollisionObject* obj) : boost::static_visitor<void>(), obj_(obj)
157  {
158  }
159 
160  void addToObject(const shapes::ShapeMsg& sm, const geometry_msgs::Pose& pose)
161  {
162  pose_ = &pose;
163  boost::apply_visitor(*this, sm);
164  }
165 
166  void operator()(const shape_msgs::Plane& shape_msg) const
167  {
168  obj_->planes.push_back(shape_msg);
169  obj_->plane_poses.push_back(*pose_);
170  }
171 
172  void operator()(const shape_msgs::Mesh& shape_msg) const
173  {
174  obj_->meshes.push_back(shape_msg);
175  obj_->mesh_poses.push_back(*pose_);
176  }
177 
178  void operator()(const shape_msgs::SolidPrimitive& shape_msg) const
179  {
180  obj_->primitives.push_back(shape_msg);
181  obj_->primitive_poses.push_back(*pose_);
182  }
183 
184 private:
185  moveit_msgs::CollisionObject* obj_;
186  const geometry_msgs::Pose* pose_;
187 };
188 
189 static void _attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::AttachedCollisionObject& aco)
190 {
191  aco.link_name = attached_body.getAttachedLinkName();
192  aco.detach_posture = attached_body.getDetachPosture();
193  const std::set<std::string>& touch_links = attached_body.getTouchLinks();
194  aco.touch_links.clear();
195  for (const std::string& touch_link : touch_links)
196  aco.touch_links.push_back(touch_link);
197  aco.object.header.frame_id = aco.link_name;
198  aco.object.id = attached_body.getName();
199 
200  aco.object.operation = moveit_msgs::CollisionObject::ADD;
201  const std::vector<shapes::ShapeConstPtr>& ab_shapes = attached_body.getShapes();
202  const EigenSTL::vector_Isometry3d& ab_tf = attached_body.getFixedTransforms();
203  ShapeVisitorAddToCollisionObject sv(&aco.object);
204  aco.object.primitives.clear();
205  aco.object.meshes.clear();
206  aco.object.planes.clear();
207  aco.object.primitive_poses.clear();
208  aco.object.mesh_poses.clear();
209  aco.object.plane_poses.clear();
210  for (std::size_t j = 0; j < ab_shapes.size(); ++j)
211  {
212  shapes::ShapeMsg sm;
213  if (shapes::constructMsgFromShape(ab_shapes[j].get(), sm))
214  {
215  geometry_msgs::Pose p;
216  p = tf2::toMsg(ab_tf[j]);
217  sv.addToObject(sm, p);
218  }
219  }
220  aco.object.subframe_names.clear();
221  aco.object.subframe_poses.clear();
222  for (const auto& frame_pair : attached_body.getSubframeTransforms())
223  {
224  aco.object.subframe_names.push_back(frame_pair.first);
225  geometry_msgs::Pose pose;
226  pose = tf2::toMsg(frame_pair.second);
227  aco.object.subframe_poses.push_back(pose);
228  }
229 }
230 
231 static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::AttachedCollisionObject& aco, RobotState& state)
232 {
233  if (aco.object.operation == moveit_msgs::CollisionObject::ADD)
234  {
235  if (!aco.object.primitives.empty() || !aco.object.meshes.empty() || !aco.object.planes.empty())
236  {
237  if (aco.object.primitives.size() != aco.object.primitive_poses.size())
238  {
239  ROS_ERROR_NAMED(LOGNAME, "Number of primitive shapes does not match "
240  "number of poses in collision object message");
241  return;
242  }
243 
244  if (aco.object.meshes.size() != aco.object.mesh_poses.size())
245  {
246  ROS_ERROR_NAMED(LOGNAME, "Number of meshes does not match number of poses in collision object message");
247  return;
248  }
249 
250  if (aco.object.planes.size() != aco.object.plane_poses.size())
251  {
252  ROS_ERROR_NAMED(LOGNAME, "Number of planes does not match number of poses in collision object message");
253  return;
254  }
255 
256  if (aco.object.subframe_poses.size() != aco.object.subframe_names.size())
257  {
258  ROS_ERROR_NAMED(LOGNAME, "Number of subframe poses does not match number of subframe names in message");
259  return;
260  }
261 
262  const LinkModel* lm = state.getLinkModel(aco.link_name);
263  if (lm)
264  {
265  std::vector<shapes::ShapeConstPtr> shapes;
266  EigenSTL::vector_Isometry3d poses;
267 
268  for (std::size_t i = 0; i < aco.object.primitives.size(); ++i)
269  {
270  shapes::Shape* s = shapes::constructShapeFromMsg(aco.object.primitives[i]);
271  if (s)
272  {
273  Eigen::Isometry3d p;
274  tf2::fromMsg(aco.object.primitive_poses[i], p);
275  shapes.push_back(shapes::ShapeConstPtr(s));
276  poses.push_back(p);
277  }
278  }
279  for (std::size_t i = 0; i < aco.object.meshes.size(); ++i)
280  {
281  shapes::Shape* s = shapes::constructShapeFromMsg(aco.object.meshes[i]);
282  if (s)
283  {
284  Eigen::Isometry3d p;
285  tf2::fromMsg(aco.object.mesh_poses[i], p);
286  shapes.push_back(shapes::ShapeConstPtr(s));
287  poses.push_back(p);
288  }
289  }
290  for (std::size_t i = 0; i < aco.object.planes.size(); ++i)
291  {
292  shapes::Shape* s = shapes::constructShapeFromMsg(aco.object.planes[i]);
293  if (s)
294  {
295  Eigen::Isometry3d p;
296  tf2::fromMsg(aco.object.plane_poses[i], p);
297 
298  shapes.push_back(shapes::ShapeConstPtr(s));
299  poses.push_back(p);
300  }
301  }
302 
303  moveit::core::FixedTransformsMap subframe_poses;
304  for (std::size_t i = 0; i < aco.object.subframe_poses.size(); ++i)
305  {
306  Eigen::Isometry3d p;
307  tf2::fromMsg(aco.object.subframe_poses[i], p);
308  std::string name = aco.object.subframe_names[i];
309  subframe_poses[name] = p;
310  }
311 
312  // Transform shape poses and subframes to link frame
313  if (!Transforms::sameFrame(aco.object.header.frame_id, aco.link_name))
314  {
315  bool frame_found = false;
316  Eigen::Isometry3d t0;
317  t0 = state.getFrameTransform(aco.object.header.frame_id, &frame_found);
318  if (!frame_found)
319  {
320  if (tf && tf->canTransform(aco.object.header.frame_id))
321  t0 = tf->getTransform(aco.object.header.frame_id);
322  else
323  {
324  t0.setIdentity();
326  "Cannot properly transform from frame '%s'. "
327  "The pose of the attached body may be incorrect",
328  aco.object.header.frame_id.c_str());
329  }
330  }
331  Eigen::Isometry3d t = state.getGlobalLinkTransform(lm).inverse() * t0;
332  for (Eigen::Isometry3d& pose : poses)
333  pose = t * pose;
334  for (auto& subframe_pose : subframe_poses)
335  subframe_pose.second = t * subframe_pose.second;
336  }
337 
338  if (shapes.empty())
339  ROS_ERROR_NAMED(LOGNAME, "There is no geometry to attach to link '%s' as part of attached body '%s'",
340  aco.link_name.c_str(), aco.object.id.c_str());
341  else
342  {
343  if (state.clearAttachedBody(aco.object.id))
345  "The robot state already had an object named '%s' attached to link '%s'. "
346  "The object was replaced.",
347  aco.object.id.c_str(), aco.link_name.c_str());
348  state.attachBody(aco.object.id, shapes, poses, aco.touch_links, aco.link_name, aco.detach_posture,
349  subframe_poses);
350  ROS_DEBUG_NAMED(LOGNAME, "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str());
351  }
352  }
353  }
354  else
355  ROS_ERROR_NAMED(LOGNAME, "The attached body for link '%s' has no geometry", aco.link_name.c_str());
356  }
357  else if (aco.object.operation == moveit_msgs::CollisionObject::REMOVE)
358  {
359  if (!state.clearAttachedBody(aco.object.id))
360  ROS_ERROR_NAMED(LOGNAME, "The attached body '%s' can not be removed because it does not exist",
361  aco.link_name.c_str());
362  }
363  else
364  ROS_ERROR_NAMED(LOGNAME, "Unknown collision object operation: %d", aco.object.operation);
365 }
366 
367 static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::RobotState& robot_state,
368  RobotState& state, bool copy_attached_bodies)
369 {
370  bool valid;
371  const moveit_msgs::RobotState& rs = robot_state;
372 
373  if (!rs.is_diff && rs.joint_state.name.empty() && rs.multi_dof_joint_state.joint_names.empty())
374  {
375  ROS_ERROR_NAMED(LOGNAME, "Found empty JointState message");
376  return false;
377  }
378 
379  bool result1 = _jointStateToRobotState(robot_state.joint_state, state);
380  bool result2 = _multiDOFJointsToRobotState(robot_state.multi_dof_joint_state, state, tf);
381  valid = result1 || result2;
382 
383  if (valid && copy_attached_bodies)
384  {
385  if (!robot_state.is_diff)
386  state.clearAttachedBodies();
387  for (const moveit_msgs::AttachedCollisionObject& attached_collision_object : robot_state.attached_collision_objects)
388  _msgToAttachedBody(tf, attached_collision_object, state);
389  }
390 
391  return valid;
392 }
393 } // namespace
394 
395 // ********************************************
396 
397 // ********************************************
398 // * Exposed functions
399 // ********************************************
400 
401 bool jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state)
402 {
403  bool result = _jointStateToRobotState(joint_state, state);
404  state.update();
405  return result;
406 }
407 
408 bool robotStateMsgToRobotState(const moveit_msgs::RobotState& robot_state, RobotState& state, bool copy_attached_bodies)
409 {
410  bool result = _robotStateMsgToRobotStateHelper(nullptr, robot_state, state, copy_attached_bodies);
411  state.update();
412  return result;
413 }
414 
415 bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::RobotState& robot_state, RobotState& state,
416  bool copy_attached_bodies)
417 {
418  bool result = _robotStateMsgToRobotStateHelper(&tf, robot_state, state, copy_attached_bodies);
419  state.update();
420  return result;
421 }
422 
423 void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::RobotState& robot_state, bool copy_attached_bodies)
424 {
425  robot_state.is_diff = false;
426  robotStateToJointStateMsg(state, robot_state.joint_state);
427  _robotStateToMultiDOFJointState(state, robot_state.multi_dof_joint_state);
428 
429  if (copy_attached_bodies)
430  {
431  std::vector<const AttachedBody*> attached_bodies;
432  state.getAttachedBodies(attached_bodies);
433  attachedBodiesToAttachedCollisionObjectMsgs(attached_bodies, robot_state.attached_collision_objects);
434  }
435 }
436 
438  const std::vector<const AttachedBody*>& attached_bodies,
439  std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs)
440 {
441  attached_collision_objs.resize(attached_bodies.size());
442  for (std::size_t i = 0; i < attached_bodies.size(); ++i)
443  _attachedBodyToMsg(*attached_bodies[i], attached_collision_objs[i]);
444 }
445 
446 void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState& joint_state)
447 {
448  const std::vector<const JointModel*>& js = state.getRobotModel()->getSingleDOFJointModels();
449  joint_state = sensor_msgs::JointState();
450 
451  for (const JointModel* joint_model : js)
452  {
453  joint_state.name.push_back(joint_model->getName());
454  joint_state.position.push_back(state.getVariablePosition(joint_model->getFirstVariableIndex()));
455  if (state.hasVelocities())
456  joint_state.velocity.push_back(state.getVariableVelocity(joint_model->getFirstVariableIndex()));
457  }
458 
459  // if inconsistent number of velocities are specified, discard them
460  if (joint_state.velocity.size() != joint_state.position.size())
461  joint_state.velocity.clear();
462 
463  joint_state.header.frame_id = state.getRobotModel()->getModelFrame();
464 }
465 
466 bool jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory& trajectory, std::size_t point_id,
467  RobotState& state)
468 {
469  if (trajectory.points.empty() || point_id > trajectory.points.size() - 1)
470  {
471  ROS_ERROR_NAMED(LOGNAME, "Invalid point_id");
472  return false;
473  }
474  if (trajectory.joint_names.empty())
475  {
476  ROS_ERROR_NAMED(LOGNAME, "No joint names specified");
477  return false;
478  }
479 
480  state.setVariablePositions(trajectory.joint_names, trajectory.points[point_id].positions);
481  if (!trajectory.points[point_id].velocities.empty())
482  state.setVariableVelocities(trajectory.joint_names, trajectory.points[point_id].velocities);
483  if (!trajectory.points[point_id].accelerations.empty())
484  state.setVariableAccelerations(trajectory.joint_names, trajectory.points[point_id].accelerations);
485  if (!trajectory.points[point_id].effort.empty())
486  state.setVariableEffort(trajectory.joint_names, trajectory.points[point_id].effort);
487 
488  return true;
489 }
490 
491 void robotStateToStream(const RobotState& state, std::ostream& out, bool include_header, const std::string& separator)
492 {
493  // Output name of variables
494  if (include_header)
495  {
496  for (std::size_t i = 0; i < state.getVariableCount(); ++i)
497  {
498  out << state.getVariableNames()[i];
499 
500  // Output comma except at end
501  if (i < state.getVariableCount() - 1)
502  out << separator;
503  }
504  out << std::endl;
505  }
506 
507  // Output values of joints
508  for (std::size_t i = 0; i < state.getVariableCount(); ++i)
509  {
510  out << state.getVariablePositions()[i];
511 
512  // Output comma except at end
513  if (i < state.getVariableCount() - 1)
514  out << separator;
515  }
516  out << std::endl;
517 }
518 
519 void robotStateToStream(const RobotState& state, std::ostream& out,
520  const std::vector<std::string>& joint_groups_ordering, bool include_header,
521  const std::string& separator)
522 {
523  std::stringstream headers;
524  std::stringstream joints;
525 
526  for (const std::string& joint_group_id : joint_groups_ordering)
527  {
528  const JointModelGroup* jmg = state.getRobotModel()->getJointModelGroup(joint_group_id);
529 
530  // Output name of variables
531  if (include_header)
532  {
533  for (std::size_t i = 0; i < jmg->getVariableCount(); ++i)
534  {
535  headers << jmg->getVariableNames()[i] << separator;
536  }
537  }
538 
539  // Copy the joint positions for each joint model group
540  std::vector<double> group_variable_positions;
541  state.copyJointGroupPositions(jmg, group_variable_positions);
542 
543  // Output values of joints
544  for (std::size_t i = 0; i < jmg->getVariableCount(); ++i)
545  {
546  joints << group_variable_positions[i] << separator;
547  }
548  }
549 
550  // Push all headers and joints to our output stream
551  if (include_header)
552  out << headers.str() << std::endl;
553  out << joints.str() << std::endl;
554 }
555 
556 void streamToRobotState(RobotState& state, const std::string& line, const std::string& separator)
557 {
558  std::stringstream line_stream(line);
559  std::string cell;
560 
561  // For each item/column
562  for (std::size_t i = 0; i < state.getVariableCount(); ++i)
563  {
564  // Get a variable
565  if (!std::getline(line_stream, cell, separator[0]))
566  ROS_ERROR_STREAM_NAMED(LOGNAME, "Missing variable " << state.getVariableNames()[i]);
567 
568  state.getVariablePositions()[i] = boost::lexical_cast<double>(cell.c_str());
569  }
570 }
571 
572 } // end of namespace core
573 } // end of namespace moveit
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:81
shapes::ShapeMsg
boost::variant< shape_msgs::SolidPrimitive, shape_msgs::Mesh, shape_msgs::Plane > ShapeMsg
obj_
moveit_msgs::CollisionObject * obj_
Definition: conversions.cpp:251
moveit::core::RobotState::setVariablePositions
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
Definition: robot_state.cpp:414
shapes
get
ROSCPP_DECL bool get(const std::string &key, bool &b)
tf2_eigen.h
tf2::fromMsg
void fromMsg(const A &, B &b)
moveit::core::RobotState::setVariableVelocities
void setVariableVelocities(const double *velocity)
Given an array with velocity values for all variables, set those values as the velocities in this sta...
Definition: robot_state.h:319
s
XmlRpcServer s
moveit::core::attachedBodiesToAttachedCollisionObjectMsgs
void attachedBodiesToAttachedCollisionObjectMsgs(const std::vector< const AttachedBody * > &attached_bodies, std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objs)
Convert AttachedBodies to AttachedCollisionObjects.
Definition: conversions.cpp:503
shape_operations.h
boost
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
shapes::Shape
moveit::core::robotStateToStream
void robotStateToStream(const RobotState &state, std::ostream &out, bool include_header=true, const std::string &separator=",")
Convert a MoveIt robot state to common separated values (CSV) on a single line that is outputted to a...
Definition: conversions.cpp:557
conversions.h
obj
CollisionObject< S > * obj
tf2::eigenToTransform
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
name
std::string name
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
moveit::core::robotStateMsgToRobotState
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state.
Definition: conversions.cpp:481
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
moveit::core::robotStateToJointStateMsg
void robotStateToJointStateMsg(const RobotState &state, sensor_msgs::JointState &joint_state)
Convert a MoveIt robot state to a joint state message.
Definition: conversions.cpp:512
moveit::core::RobotState::setVariableEffort
void setVariableEffort(const double *effort)
Given an array with effort values for all variables, set those values as the effort in this state.
Definition: robot_state.h:511
moveit::core::FixedTransformsMap
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
Definition: transforms.h:117
moveit::core::LOGNAME
const std::string LOGNAME
Definition: joint_model_group.cpp:165
tf2::transformToEigen
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
pose_
const geometry_msgs::Pose * pose_
Definition: conversions.cpp:252
moveit::core::Transforms::sameFrame
static bool sameFrame(const std::string &frame1, const std::string &frame2)
Check if two frames end up being the same once the missing / are added as prefix (if they are missing...
Definition: transforms.cpp:122
shapes::constructMsgFromShape
bool constructMsgFromShape(const Shape *shape, ShapeMsg &shape_msg)
shapes::constructShapeFromMsg
Shape * constructShapeFromMsg(const shape_msgs::Mesh &shape_msg)
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
tf2::toMsg
B toMsg(const A &a)
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
moveit::core::RobotState::setVariableAccelerations
void setVariableAccelerations(const double *acceleration)
Given an array with acceleration values for all variables, set those values as the accelerations in t...
Definition: robot_state.h:414
tf
moveit::core::jointTrajPointToRobotState
bool jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory &trajectory, std::size_t point_id, RobotState &state)
Convert a joint trajectory point to a MoveIt robot state.
Definition: conversions.cpp:532
moveit::core::jointStateToRobotState
bool jointStateToRobotState(const sensor_msgs::JointState &joint_state, RobotState &state)
Convert a joint state to a MoveIt robot state.
Definition: conversions.cpp:467
moveit::core::streamToRobotState
void streamToRobotState(RobotState &state, const std::string &line, const std::string &separator=",")
Convert a string of joint values from a file (CSV) or input source into a RobotState.
Definition: conversions.cpp:622
moveit::core::robotStateToRobotStateMsg
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
Definition: conversions.cpp:489
t
geometry_msgs::TransformStamped t


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Dec 12 2020 03:25:44