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  aco.object.pose = tf2::toMsg(attached_body.getPose());
200 
201  aco.object.operation = moveit_msgs::CollisionObject::ADD;
202  const std::vector<shapes::ShapeConstPtr>& ab_shapes = attached_body.getShapes();
203  const EigenSTL::vector_Isometry3d& shape_poses = attached_body.getShapePoses();
204  ShapeVisitorAddToCollisionObject sv(&aco.object);
205  aco.object.primitives.clear();
206  aco.object.meshes.clear();
207  aco.object.planes.clear();
208  aco.object.primitive_poses.clear();
209  aco.object.mesh_poses.clear();
210  aco.object.plane_poses.clear();
211  for (std::size_t j = 0; j < ab_shapes.size(); ++j)
212  {
213  shapes::ShapeMsg sm;
214  if (shapes::constructMsgFromShape(ab_shapes[j].get(), sm))
215  {
216  geometry_msgs::Pose p;
217  p = tf2::toMsg(shape_poses[j]);
218  sv.addToObject(sm, p);
219  }
220  }
221  aco.object.subframe_names.clear();
222  aco.object.subframe_poses.clear();
223  for (const auto& frame_pair : attached_body.getSubframes())
224  {
225  aco.object.subframe_names.push_back(frame_pair.first);
226  geometry_msgs::Pose pose;
227  pose = tf2::toMsg(frame_pair.second);
228  aco.object.subframe_poses.push_back(pose);
229  }
230 }
231 
232 static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::AttachedCollisionObject& aco, RobotState& state)
233 {
234  if (aco.object.operation == moveit_msgs::CollisionObject::ADD)
235  {
236  if (!aco.object.primitives.empty() || !aco.object.meshes.empty() || !aco.object.planes.empty())
237  {
238  if (aco.object.primitives.size() != aco.object.primitive_poses.size())
239  {
240  ROS_ERROR_NAMED(LOGNAME, "Number of primitive shapes does not match "
241  "number of poses in collision object message");
242  return;
243  }
244 
245  if (aco.object.meshes.size() != aco.object.mesh_poses.size())
246  {
247  ROS_ERROR_NAMED(LOGNAME, "Number of meshes does not match number of poses in collision object message");
248  return;
249  }
250 
251  if (aco.object.planes.size() != aco.object.plane_poses.size())
252  {
253  ROS_ERROR_NAMED(LOGNAME, "Number of planes does not match number of poses in collision object message");
254  return;
255  }
256 
257  if (aco.object.subframe_poses.size() != aco.object.subframe_names.size())
258  {
259  ROS_ERROR_NAMED(LOGNAME, "Number of subframe poses does not match number of subframe names in message");
260  return;
261  }
262 
263  const LinkModel* lm = state.getLinkModel(aco.link_name);
264  if (lm)
265  {
266  Eigen::Isometry3d object_pose;
267  tf2::fromMsg(aco.object.pose, object_pose);
268 
269  std::vector<shapes::ShapeConstPtr> shapes;
270  EigenSTL::vector_Isometry3d shape_poses;
271  const auto num_shapes = aco.object.primitives.size() + aco.object.meshes.size() + aco.object.planes.size();
272  shapes.reserve(num_shapes);
273  shape_poses.reserve(num_shapes);
274 
275  auto append = [&shapes, &shape_poses](shapes::Shape* s, const geometry_msgs::Pose& pose_msg) {
276  if (!s)
277  return;
278  Eigen::Isometry3d pose;
279  tf2::fromMsg(pose_msg, pose);
280  shapes.emplace_back(shapes::ShapeConstPtr(s));
281  shape_poses.emplace_back(std::move(pose));
282  };
283 
284  for (std::size_t i = 0; i < aco.object.primitives.size(); ++i)
285  append(shapes::constructShapeFromMsg(aco.object.primitives[i]), aco.object.primitive_poses[i]);
286  for (std::size_t i = 0; i < aco.object.meshes.size(); ++i)
287  append(shapes::constructShapeFromMsg(aco.object.meshes[i]), aco.object.mesh_poses[i]);
288  for (std::size_t i = 0; i < aco.object.planes.size(); ++i)
289  append(shapes::constructShapeFromMsg(aco.object.planes[i]), aco.object.plane_poses[i]);
290 
291  moveit::core::FixedTransformsMap subframe_poses;
292  for (std::size_t i = 0; i < aco.object.subframe_poses.size(); ++i)
293  {
294  Eigen::Isometry3d p;
295  tf2::fromMsg(aco.object.subframe_poses[i], p);
296  std::string name = aco.object.subframe_names[i];
297  subframe_poses[name] = p;
298  }
299 
300  // Transform shape pose to link frame
301  if (!Transforms::sameFrame(aco.object.header.frame_id, aco.link_name))
302  {
303  bool frame_found = false;
304  Eigen::Isometry3d world_to_header_frame;
305  world_to_header_frame = state.getFrameTransform(aco.object.header.frame_id, &frame_found);
306  if (!frame_found)
307  {
308  if (tf && tf->canTransform(aco.object.header.frame_id))
309  world_to_header_frame = tf->getTransform(aco.object.header.frame_id);
310  else
311  {
312  world_to_header_frame.setIdentity();
314  "Cannot properly transform from frame '%s'. "
315  "The pose of the attached body may be incorrect",
316  aco.object.header.frame_id.c_str());
317  }
318  }
319  object_pose = state.getGlobalLinkTransform(lm).inverse() * world_to_header_frame * object_pose;
320  }
321 
322  if (shapes.empty())
323  ROS_ERROR_NAMED(LOGNAME, "There is no geometry to attach to link '%s' as part of attached body '%s'",
324  aco.link_name.c_str(), aco.object.id.c_str());
325  else
326  {
327  if (state.clearAttachedBody(aco.object.id))
329  "The robot state already had an object named '%s' attached to link '%s'. "
330  "The object was replaced.",
331  aco.object.id.c_str(), aco.link_name.c_str());
332  state.attachBody(aco.object.id, object_pose, shapes, shape_poses, aco.touch_links, aco.link_name,
333  aco.detach_posture, subframe_poses);
334  ROS_DEBUG_NAMED(LOGNAME, "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str());
335  }
336  }
337  }
338  else
339  ROS_ERROR_NAMED(LOGNAME, "The attached body for link '%s' has no geometry", aco.link_name.c_str());
340  }
341  else if (aco.object.operation == moveit_msgs::CollisionObject::REMOVE)
342  {
343  if (!state.clearAttachedBody(aco.object.id))
344  ROS_ERROR_NAMED(LOGNAME, "The attached body '%s' can not be removed because it does not exist",
345  aco.link_name.c_str());
346  }
347  else
348  ROS_ERROR_NAMED(LOGNAME, "Unknown collision object operation: %d", aco.object.operation);
349 }
350 
351 static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::RobotState& robot_state,
352  RobotState& state, bool copy_attached_bodies)
353 {
354  bool valid;
355  const moveit_msgs::RobotState& rs = robot_state;
356 
357  if (!rs.is_diff && rs.joint_state.name.empty() && rs.multi_dof_joint_state.joint_names.empty())
358  {
359  ROS_ERROR_NAMED(LOGNAME, "Found empty JointState message");
360  return false;
361  }
362 
363  bool result1 = _jointStateToRobotState(robot_state.joint_state, state);
364  bool result2 = _multiDOFJointsToRobotState(robot_state.multi_dof_joint_state, state, tf);
365  valid = result1 || result2;
366 
367  if (valid && copy_attached_bodies)
368  {
369  if (!robot_state.is_diff)
370  state.clearAttachedBodies();
371  for (const moveit_msgs::AttachedCollisionObject& attached_collision_object : robot_state.attached_collision_objects)
372  _msgToAttachedBody(tf, attached_collision_object, state);
373  }
374 
375  return valid;
376 }
377 } // namespace
378 
379 // ********************************************
380 
381 // ********************************************
382 // * Exposed functions
383 // ********************************************
384 
385 bool jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state)
386 {
387  bool result = _jointStateToRobotState(joint_state, state);
388  state.update();
389  return result;
390 }
391 
392 bool robotStateMsgToRobotState(const moveit_msgs::RobotState& robot_state, RobotState& state, bool copy_attached_bodies)
393 {
394  bool result = _robotStateMsgToRobotStateHelper(nullptr, robot_state, state, copy_attached_bodies);
395  state.update();
396  return result;
397 }
398 
399 bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::RobotState& robot_state, RobotState& state,
400  bool copy_attached_bodies)
401 {
402  bool result = _robotStateMsgToRobotStateHelper(&tf, robot_state, state, copy_attached_bodies);
403  state.update();
404  return result;
405 }
406 
407 void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::RobotState& robot_state, bool copy_attached_bodies)
408 {
409  robot_state.is_diff = false;
410  robotStateToJointStateMsg(state, robot_state.joint_state);
411  _robotStateToMultiDOFJointState(state, robot_state.multi_dof_joint_state);
412 
413  if (copy_attached_bodies)
414  {
415  std::vector<const AttachedBody*> attached_bodies;
416  state.getAttachedBodies(attached_bodies);
417  attachedBodiesToAttachedCollisionObjectMsgs(attached_bodies, robot_state.attached_collision_objects);
418  }
419 }
420 
422  const std::vector<const AttachedBody*>& attached_bodies,
423  std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs)
424 {
425  attached_collision_objs.resize(attached_bodies.size());
426  for (std::size_t i = 0; i < attached_bodies.size(); ++i)
427  _attachedBodyToMsg(*attached_bodies[i], attached_collision_objs[i]);
428 }
429 
430 void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState& joint_state)
431 {
432  const std::vector<const JointModel*>& js = state.getRobotModel()->getSingleDOFJointModels();
433  joint_state = sensor_msgs::JointState();
434 
435  for (const JointModel* joint_model : js)
436  {
437  joint_state.name.push_back(joint_model->getName());
438  joint_state.position.push_back(state.getVariablePosition(joint_model->getFirstVariableIndex()));
439  if (state.hasVelocities())
440  joint_state.velocity.push_back(state.getVariableVelocity(joint_model->getFirstVariableIndex()));
441  }
442 
443  // if inconsistent number of velocities are specified, discard them
444  if (joint_state.velocity.size() != joint_state.position.size())
445  joint_state.velocity.clear();
446 
447  joint_state.header.frame_id = state.getRobotModel()->getModelFrame();
448 }
449 
450 bool jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory& trajectory, std::size_t point_id,
451  RobotState& state)
452 {
453  if (trajectory.points.empty() || point_id > trajectory.points.size() - 1)
454  {
455  ROS_ERROR_NAMED(LOGNAME, "Invalid point_id");
456  return false;
457  }
458  if (trajectory.joint_names.empty())
459  {
460  ROS_ERROR_NAMED(LOGNAME, "No joint names specified");
461  return false;
462  }
463 
464  state.setVariablePositions(trajectory.joint_names, trajectory.points[point_id].positions);
465  if (!trajectory.points[point_id].velocities.empty())
466  state.setVariableVelocities(trajectory.joint_names, trajectory.points[point_id].velocities);
467  if (!trajectory.points[point_id].accelerations.empty())
468  state.setVariableAccelerations(trajectory.joint_names, trajectory.points[point_id].accelerations);
469  if (!trajectory.points[point_id].effort.empty())
470  state.setVariableEffort(trajectory.joint_names, trajectory.points[point_id].effort);
471 
472  return true;
473 }
474 
475 void robotStateToStream(const RobotState& state, std::ostream& out, bool include_header, const std::string& separator)
476 {
477  // Output name of variables
478  if (include_header)
479  {
480  for (std::size_t i = 0; i < state.getVariableCount(); ++i)
481  {
482  out << state.getVariableNames()[i];
483 
484  // Output comma except at end
485  if (i < state.getVariableCount() - 1)
486  out << separator;
487  }
488  out << std::endl;
489  }
490 
491  // Output values of joints
492  for (std::size_t i = 0; i < state.getVariableCount(); ++i)
493  {
494  out << state.getVariablePositions()[i];
495 
496  // Output comma except at end
497  if (i < state.getVariableCount() - 1)
498  out << separator;
499  }
500  out << std::endl;
501 }
502 
503 void robotStateToStream(const RobotState& state, std::ostream& out,
504  const std::vector<std::string>& joint_groups_ordering, bool include_header,
505  const std::string& separator)
506 {
507  std::stringstream headers;
508  std::stringstream joints;
509 
510  for (const std::string& joint_group_id : joint_groups_ordering)
511  {
512  const JointModelGroup* jmg = state.getRobotModel()->getJointModelGroup(joint_group_id);
513 
514  // Output name of variables
515  if (include_header)
516  {
517  for (std::size_t i = 0; i < jmg->getVariableCount(); ++i)
518  {
519  headers << jmg->getVariableNames()[i] << separator;
520  }
521  }
522 
523  // Copy the joint positions for each joint model group
524  std::vector<double> group_variable_positions;
525  state.copyJointGroupPositions(jmg, group_variable_positions);
526 
527  // Output values of joints
528  for (std::size_t i = 0; i < jmg->getVariableCount(); ++i)
529  {
530  joints << group_variable_positions[i] << separator;
531  }
532  }
533 
534  // Push all headers and joints to our output stream
535  if (include_header)
536  out << headers.str() << std::endl;
537  out << joints.str() << std::endl;
538 }
539 
540 void streamToRobotState(RobotState& state, const std::string& line, const std::string& separator)
541 {
542  std::stringstream line_stream(line);
543  std::string cell;
544 
545  // For each item/column
546  for (std::size_t i = 0; i < state.getVariableCount(); ++i)
547  {
548  // Get a variable
549  if (!std::getline(line_stream, cell, separator[0]))
550  ROS_ERROR_STREAM_NAMED(LOGNAME, "Missing variable " << state.getVariableNames()[i]);
551 
552  state.getVariablePositions()[i] = boost::lexical_cast<double>(cell.c_str());
553  }
554 }
555 
556 } // end of namespace core
557 } // end of namespace moveit
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
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:434
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:487
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:541
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:465
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:496
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)
append
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
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
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:516
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:451
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:606
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:473
t
geometry_msgs::TransformStamped t


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Oct 12 2021 02:24:44