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)
104  ROS_WARN_NAMED(LOGNAME, "The transform for multi-dof joints was specified in frame '%s' "
105  "but it was not possible to transform that to frame '%s'",
106  mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str());
107  }
108 
109  for (std::size_t i = 0; i < nj; ++i)
110  {
111  const std::string& joint_name = mjs.joint_names[i];
112  if (!state.getRobotModel()->hasJointModel(joint_name))
113  {
114  ROS_WARN_NAMED(LOGNAME, "No joint matching multi-dof joint '%s'", joint_name.c_str());
115  error = true;
116  continue;
117  }
118  Eigen::Isometry3d transf = tf2::transformToEigen(mjs.transforms[i]);
119  // if frames do not mach, attempt to transform
120  if (use_inv_t)
121  transf = transf * inv_t;
122 
123  state.setJointPositions(joint_name, transf);
124  }
125 
126  return !error;
127 }
128 
129 static inline void _robotStateToMultiDOFJointState(const RobotState& state, sensor_msgs::MultiDOFJointState& mjs)
130 {
131  const std::vector<const JointModel*>& js = state.getRobotModel()->getMultiDOFJointModels();
132  mjs.joint_names.clear();
133  mjs.transforms.clear();
134  for (std::size_t i = 0; i < js.size(); ++i)
135  {
136  geometry_msgs::TransformStamped p;
137  if (state.dirtyJointTransform(js[i]))
138  {
139  Eigen::Isometry3d t;
140  t.setIdentity();
141  js[i]->computeTransform(state.getJointPositions(js[i]), t);
142  p = tf2::eigenToTransform(t);
143  }
144  else
145  p = tf2::eigenToTransform(state.getJointTransform(js[i]));
146  mjs.joint_names.push_back(js[i]->getName());
147  mjs.transforms.push_back(p.transform);
148  }
149  mjs.header.frame_id = state.getRobotModel()->getModelFrame();
150 }
151 
152 class ShapeVisitorAddToCollisionObject : public boost::static_visitor<void>
153 {
154 public:
155  ShapeVisitorAddToCollisionObject(moveit_msgs::CollisionObject* obj) : boost::static_visitor<void>(), obj_(obj)
156  {
157  }
158 
159  void addToObject(const shapes::ShapeMsg& sm, const geometry_msgs::Pose& pose)
160  {
161  pose_ = &pose;
162  boost::apply_visitor(*this, sm);
163  }
164 
165  void operator()(const shape_msgs::Plane& shape_msg) const
166  {
167  obj_->planes.push_back(shape_msg);
168  obj_->plane_poses.push_back(*pose_);
169  }
170 
171  void operator()(const shape_msgs::Mesh& shape_msg) const
172  {
173  obj_->meshes.push_back(shape_msg);
174  obj_->mesh_poses.push_back(*pose_);
175  }
176 
177  void operator()(const shape_msgs::SolidPrimitive& shape_msg) const
178  {
179  obj_->primitives.push_back(shape_msg);
180  obj_->primitive_poses.push_back(*pose_);
181  }
182 
183 private:
184  moveit_msgs::CollisionObject* obj_;
185  const geometry_msgs::Pose* pose_;
186 };
187 
188 static void _attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::AttachedCollisionObject& aco)
189 {
190  aco.link_name = attached_body.getAttachedLinkName();
191  aco.detach_posture = attached_body.getDetachPosture();
192  const std::set<std::string>& touch_links = attached_body.getTouchLinks();
193  aco.touch_links.clear();
194  for (std::set<std::string>::const_iterator it = touch_links.begin(); it != touch_links.end(); ++it)
195  aco.touch_links.push_back(*it);
196  aco.object.header.frame_id = aco.link_name;
197  aco.object.id = attached_body.getName();
198 
199  aco.object.operation = moveit_msgs::CollisionObject::ADD;
200  const std::vector<shapes::ShapeConstPtr>& ab_shapes = attached_body.getShapes();
201  const EigenSTL::vector_Isometry3d& ab_tf = attached_body.getFixedTransforms();
202  ShapeVisitorAddToCollisionObject sv(&aco.object);
203  aco.object.primitives.clear();
204  aco.object.meshes.clear();
205  aco.object.planes.clear();
206  aco.object.primitive_poses.clear();
207  aco.object.mesh_poses.clear();
208  aco.object.plane_poses.clear();
209  for (std::size_t j = 0; j < ab_shapes.size(); ++j)
210  {
211  shapes::ShapeMsg sm;
212  if (shapes::constructMsgFromShape(ab_shapes[j].get(), sm))
213  {
214  geometry_msgs::Pose p;
215  p = tf2::toMsg(ab_tf[j]);
216  sv.addToObject(sm, p);
217  }
218  }
219 }
220 
221 static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::AttachedCollisionObject& aco, RobotState& state)
222 {
223  if (aco.object.operation == moveit_msgs::CollisionObject::ADD)
224  {
225  if (!aco.object.primitives.empty() || !aco.object.meshes.empty() || !aco.object.planes.empty())
226  {
227  if (aco.object.primitives.size() != aco.object.primitive_poses.size())
228  {
229  ROS_ERROR_NAMED(LOGNAME, "Number of primitive shapes does not match "
230  "number of poses in collision object message");
231  return;
232  }
233 
234  if (aco.object.meshes.size() != aco.object.mesh_poses.size())
235  {
236  ROS_ERROR_NAMED(LOGNAME, "Number of meshes does not match number of poses in collision object message");
237  return;
238  }
239 
240  if (aco.object.planes.size() != aco.object.plane_poses.size())
241  {
242  ROS_ERROR_NAMED(LOGNAME, "Number of planes does not match number of poses in collision object message");
243  return;
244  }
245 
246  const LinkModel* lm = state.getLinkModel(aco.link_name);
247  if (lm)
248  {
249  std::vector<shapes::ShapeConstPtr> shapes;
251 
252  for (std::size_t i = 0; i < aco.object.primitives.size(); ++i)
253  {
254  shapes::Shape* s = shapes::constructShapeFromMsg(aco.object.primitives[i]);
255  if (s)
256  {
257  Eigen::Isometry3d p;
258  tf2::fromMsg(aco.object.primitive_poses[i], p);
259  shapes.push_back(shapes::ShapeConstPtr(s));
260  poses.push_back(p);
261  }
262  }
263  for (std::size_t i = 0; i < aco.object.meshes.size(); ++i)
264  {
265  shapes::Shape* s = shapes::constructShapeFromMsg(aco.object.meshes[i]);
266  if (s)
267  {
268  Eigen::Isometry3d p;
269  tf2::fromMsg(aco.object.mesh_poses[i], p);
270  shapes.push_back(shapes::ShapeConstPtr(s));
271  poses.push_back(p);
272  }
273  }
274  for (std::size_t i = 0; i < aco.object.planes.size(); ++i)
275  {
276  shapes::Shape* s = shapes::constructShapeFromMsg(aco.object.planes[i]);
277  if (s)
278  {
279  Eigen::Isometry3d p;
280  tf2::fromMsg(aco.object.plane_poses[i], p);
281 
282  shapes.push_back(shapes::ShapeConstPtr(s));
283  poses.push_back(p);
284  }
285  }
286 
287  // transform poses to link frame
288  if (!Transforms::sameFrame(aco.object.header.frame_id, aco.link_name))
289  {
290  Eigen::Isometry3d t0;
291  if (state.knowsFrameTransform(aco.object.header.frame_id))
292  t0 = state.getFrameTransform(aco.object.header.frame_id);
293  else if (tf && tf->canTransform(aco.object.header.frame_id))
294  t0 = tf->getTransform(aco.object.header.frame_id);
295  else
296  {
297  t0.setIdentity();
298  ROS_ERROR_NAMED(LOGNAME, "Cannot properly transform from frame '%s'. "
299  "The pose of the attached body may be incorrect",
300  aco.object.header.frame_id.c_str());
301  }
302  Eigen::Isometry3d t = state.getGlobalLinkTransform(lm).inverse() * t0;
303  for (std::size_t i = 0; i < poses.size(); ++i)
304  poses[i] = t * poses[i];
305  }
306 
307  if (shapes.empty())
308  ROS_ERROR_NAMED(LOGNAME, "There is no geometry to attach to link '%s' as part of attached body '%s'",
309  aco.link_name.c_str(), aco.object.id.c_str());
310  else
311  {
312  if (state.clearAttachedBody(aco.object.id))
313  ROS_DEBUG_NAMED(LOGNAME, "The robot state already had an object named '%s' attached to link '%s'. "
314  "The object was replaced.",
315  aco.object.id.c_str(), aco.link_name.c_str());
316  state.attachBody(aco.object.id, shapes, poses, aco.touch_links, aco.link_name, aco.detach_posture);
317  ROS_DEBUG_NAMED(LOGNAME, "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str());
318  }
319  }
320  }
321  else
322  ROS_ERROR_NAMED(LOGNAME, "The attached body for link '%s' has no geometry", aco.link_name.c_str());
323  }
324  else if (aco.object.operation == moveit_msgs::CollisionObject::REMOVE)
325  {
326  if (!state.clearAttachedBody(aco.object.id))
327  ROS_ERROR_NAMED(LOGNAME, "The attached body '%s' can not be removed because it does not exist",
328  aco.link_name.c_str());
329  }
330  else
331  ROS_ERROR_NAMED(LOGNAME, "Unknown collision object operation: %d", aco.object.operation);
332 }
333 
334 static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::RobotState& robot_state,
335  RobotState& state, bool copy_attached_bodies)
336 {
337  bool valid;
338  const moveit_msgs::RobotState& rs = robot_state;
339 
340  if (!rs.is_diff && rs.joint_state.name.empty() && rs.multi_dof_joint_state.joint_names.empty())
341  {
342  ROS_ERROR_NAMED(LOGNAME, "Found empty JointState message");
343  return false;
344  }
345 
346  bool result1 = _jointStateToRobotState(robot_state.joint_state, state);
347  bool result2 = _multiDOFJointsToRobotState(robot_state.multi_dof_joint_state, state, tf);
348  valid = result1 || result2;
349 
350  if (valid && copy_attached_bodies)
351  {
352  if (!robot_state.is_diff)
353  state.clearAttachedBodies();
354  for (std::size_t i = 0; i < robot_state.attached_collision_objects.size(); ++i)
355  _msgToAttachedBody(tf, robot_state.attached_collision_objects[i], state);
356  }
357 
358  return valid;
359 }
360 } // namespace
361 
362 // ********************************************
363 
364 // ********************************************
365 // * Exposed functions
366 // ********************************************
367 
368 bool jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state)
369 {
370  bool result = _jointStateToRobotState(joint_state, state);
371  state.update();
372  return result;
373 }
374 
375 bool robotStateMsgToRobotState(const moveit_msgs::RobotState& robot_state, RobotState& state, bool copy_attached_bodies)
376 {
377  bool result = _robotStateMsgToRobotStateHelper(nullptr, robot_state, state, copy_attached_bodies);
378  state.update();
379  return result;
380 }
381 
382 bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::RobotState& robot_state, RobotState& state,
383  bool copy_attached_bodies)
384 {
385  bool result = _robotStateMsgToRobotStateHelper(&tf, robot_state, state, copy_attached_bodies);
386  state.update();
387  return result;
388 }
389 
390 void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::RobotState& robot_state, bool copy_attached_bodies)
391 {
392  robot_state.is_diff = false;
393  robotStateToJointStateMsg(state, robot_state.joint_state);
394  _robotStateToMultiDOFJointState(state, robot_state.multi_dof_joint_state);
395 
396  if (copy_attached_bodies)
397  {
398  std::vector<const AttachedBody*> attached_bodies;
399  state.getAttachedBodies(attached_bodies);
400  attachedBodiesToAttachedCollisionObjectMsgs(attached_bodies, robot_state.attached_collision_objects);
401  }
402 }
403 
405  const std::vector<const AttachedBody*>& attached_bodies,
406  std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs)
407 {
408  attached_collision_objs.resize(attached_bodies.size());
409  for (std::size_t i = 0; i < attached_bodies.size(); ++i)
410  _attachedBodyToMsg(*attached_bodies[i], attached_collision_objs[i]);
411 }
412 
413 void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState& joint_state)
414 {
415  const std::vector<const JointModel*>& js = state.getRobotModel()->getSingleDOFJointModels();
416  joint_state = sensor_msgs::JointState();
417 
418  for (std::size_t i = 0; i < js.size(); ++i)
419  {
420  joint_state.name.push_back(js[i]->getName());
421  joint_state.position.push_back(state.getVariablePosition(js[i]->getFirstVariableIndex()));
422  if (state.hasVelocities())
423  joint_state.velocity.push_back(state.getVariableVelocity(js[i]->getFirstVariableIndex()));
424  }
425 
426  // if inconsistent number of velocities are specified, discard them
427  if (joint_state.velocity.size() != joint_state.position.size())
428  joint_state.velocity.clear();
429 
430  joint_state.header.frame_id = state.getRobotModel()->getModelFrame();
431 }
432 
433 bool jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory& trajectory, std::size_t point_id,
434  RobotState& state)
435 {
436  if (trajectory.points.empty() || point_id > trajectory.points.size() - 1)
437  {
438  ROS_ERROR_NAMED(LOGNAME, "Invalid point_id");
439  return false;
440  }
441  if (trajectory.joint_names.empty())
442  {
443  ROS_ERROR_NAMED(LOGNAME, "No joint names specified");
444  return false;
445  }
446 
447  state.setVariablePositions(trajectory.joint_names, trajectory.points[point_id].positions);
448  if (!trajectory.points[point_id].velocities.empty())
449  state.setVariableVelocities(trajectory.joint_names, trajectory.points[point_id].velocities);
450  if (!trajectory.points[point_id].accelerations.empty())
451  state.setVariableAccelerations(trajectory.joint_names, trajectory.points[point_id].accelerations);
452  if (!trajectory.points[point_id].effort.empty())
453  state.setVariableEffort(trajectory.joint_names, trajectory.points[point_id].effort);
454 
455  return true;
456 }
457 
458 void robotStateToStream(const RobotState& state, std::ostream& out, bool include_header, const std::string& separator)
459 {
460  // Output name of variables
461  if (include_header)
462  {
463  for (std::size_t i = 0; i < state.getVariableCount(); ++i)
464  {
465  out << state.getVariableNames()[i];
466 
467  // Output comma except at end
468  if (i < state.getVariableCount() - 1)
469  out << separator;
470  }
471  out << std::endl;
472  }
473 
474  // Output values of joints
475  for (std::size_t i = 0; i < state.getVariableCount(); ++i)
476  {
477  out << state.getVariablePositions()[i];
478 
479  // Output comma except at end
480  if (i < state.getVariableCount() - 1)
481  out << separator;
482  }
483  out << std::endl;
484 }
485 
486 void robotStateToStream(const RobotState& state, std::ostream& out,
487  const std::vector<std::string>& joint_groups_ordering, bool include_header,
488  const std::string& separator)
489 {
490  std::stringstream headers;
491  std::stringstream joints;
492 
493  for (std::size_t j = 0; j < joint_groups_ordering.size(); ++j)
494  {
495  const JointModelGroup* jmg = state.getRobotModel()->getJointModelGroup(joint_groups_ordering[j]);
496 
497  // Output name of variables
498  if (include_header)
499  {
500  for (std::size_t i = 0; i < jmg->getVariableCount(); ++i)
501  {
502  headers << jmg->getVariableNames()[i] << separator;
503  }
504  }
505 
506  // Copy the joint positions for each joint model group
507  std::vector<double> group_variable_positions;
508  state.copyJointGroupPositions(jmg, group_variable_positions);
509 
510  // Output values of joints
511  for (std::size_t i = 0; i < jmg->getVariableCount(); ++i)
512  {
513  joints << group_variable_positions[i] << separator;
514  }
515  }
516 
517  // Push all headers and joints to our output stream
518  if (include_header)
519  out << headers.str() << std::endl;
520  out << joints.str() << std::endl;
521 }
522 
523 void streamToRobotState(RobotState& state, const std::string& line, const std::string& separator)
524 {
525  std::stringstream line_stream(line);
526  std::string cell;
527 
528  // For each item/column
529  for (std::size_t i = 0; i < state.getVariableCount(); ++i)
530  {
531  // Get a variable
532  if (!std::getline(line_stream, cell, separator[0]))
533  ROS_ERROR_STREAM_NAMED(LOGNAME, "Missing variable " << state.getVariableNames()[i]);
534 
535  state.getVariablePositions()[i] = boost::lexical_cast<double>(cell.c_str());
536  }
537 }
538 
539 } // end of namespace core
540 } // end of namespace moveit
double getVariableVelocity(const std::string &variable) const
Get the velocity of a particular variable. An exception is thrown if the variable is not known...
Definition: robot_state.h:326
Core components of MoveIt!
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known...
Definition: robot_state.h:240
#define ROS_ERROR_STREAM_NAMED(name, args)
#define ROS_WARN_NAMED(name,...)
bool hasVelocities() const
By default, if velocities are never set or initialized, the state remembers that there are no velocit...
Definition: robot_state.h:262
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
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 ...
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:283
XmlRpcServer s
bool jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory &trajectory, std::size_t point_id, RobotState &state)
Convert a joint trajectory point to a MoveIt! robot state.
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:57
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
void robotStateToJointStateMsg(const RobotState &state, sensor_msgs::JointState &joint_state)
Convert a MoveIt! robot state to a joint state message.
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up the joints included in this group. The number of returned...
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully. If you change these values externally you need to make sure you trigger a forced update for the state by calling update(true).
Definition: robot_state.h:180
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state...
geometry_msgs::TransformStamped t
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:61
robot_trajectory::RobotTrajectory trajectory(rmodel, "right_arm")
#define ROS_DEBUG_NAMED(name,...)
void getAttachedBodies(std::vector< const AttachedBody *> &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
void update(bool force=false)
Update all transforms.
void fromMsg(const A &, B &b)
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:137
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...
Shape * constructShapeFromMsg(const shape_msgs::SolidPrimitive &shape_msg)
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:665
const std::string LOGNAME
Definition: robot_model.cpp:53
B toMsg(const A &a)
const geometry_msgs::Pose * pose_
void attachedBodiesToAttachedCollisionObjectMsgs(const std::vector< const AttachedBody *> &attached_bodies, std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objs)
Convert AttachedBodies to AttachedCollisionObjects.
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.
bool constructMsgFromShape(const Shape *shape, ShapeMsg &shape_msg)
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:143
bool jointStateToRobotState(const sensor_msgs::JointState &joint_state, RobotState &state)
Convert a joint state to a MoveIt! robot state.
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:372
#define ROS_ERROR_NAMED(name,...)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:122
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
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:463
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
Main namespace for MoveIt!
moveit_msgs::CollisionObject * obj_
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.
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up this state, in the order they are stored in memory...
Definition: robot_state.h:149
boost::variant< shape_msgs::SolidPrimitive, shape_msgs::Mesh, shape_msgs::Plane > ShapeMsg
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Feb 17 2020 03:50:49