planning_scene.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include <boost/algorithm/string.hpp>
49 #include <tf2_eigen/tf2_eigen.h>
50 #include <memory>
51 #include <set>
52 
53 namespace planning_scene
54 {
55 const std::string PlanningScene::OCTOMAP_NS = "<octomap>";
56 const std::string PlanningScene::DEFAULT_SCENE_NAME = "(noname)";
57 
58 const std::string LOGNAME = "planning_scene";
59 
60 class SceneTransforms : public moveit::core::Transforms
61 {
62 public:
63  SceneTransforms(const PlanningScene* scene) : Transforms(scene->getRobotModel()->getModelFrame()), scene_(scene)
64  {
65  }
66 
67  bool canTransform(const std::string& from_frame) const override
68  {
69  return scene_->knowsFrameTransform(from_frame);
70  }
71 
72  bool isFixedFrame(const std::string& frame) const override
73  {
74  if (frame.empty())
75  return false;
76  if (Transforms::isFixedFrame(frame))
77  return true;
78  if (frame[0] == '/')
79  return knowsObjectFrame(frame.substr(1));
80  else
81  return knowsObjectFrame(frame);
82  }
83 
84  const Eigen::Isometry3d& getTransform(const std::string& from_frame) const override
85  { // the call below also calls Transforms::getTransform() too
86  return scene_->getFrameTransform(from_frame);
87  }
88 
89 private:
90  // Returns true if frame_id is the name of an object or the name of a subframe on an object
91  bool knowsObjectFrame(const std::string& frame_id) const
92  {
93  return scene_->getWorld()->knowsTransform(frame_id);
94  }
95 
96  const PlanningScene* scene_;
97 };
98 
99 bool PlanningScene::isEmpty(const moveit_msgs::PlanningScene& msg)
100 {
101  return moveit::core::isEmpty(msg);
102 }
103 
104 bool PlanningScene::isEmpty(const moveit_msgs::RobotState& msg)
105 {
106  return moveit::core::isEmpty(msg);
107 }
108 
109 bool PlanningScene::isEmpty(const moveit_msgs::PlanningSceneWorld& msg)
110 {
111  return moveit::core::isEmpty(msg);
112 }
113 
114 PlanningScene::PlanningScene(const moveit::core::RobotModelConstPtr& robot_model,
115  const collision_detection::WorldPtr& world)
116  : robot_model_(robot_model), world_(world), world_const_(world)
117 {
118  initialize();
119 }
120 
121 PlanningScene::PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model,
122  const srdf::ModelConstSharedPtr& srdf_model, const collision_detection::WorldPtr& world)
123  : world_(world), world_const_(world)
124 {
125  if (!urdf_model)
126  throw moveit::ConstructException("The URDF model cannot be NULL");
127 
128  if (!srdf_model)
129  throw moveit::ConstructException("The SRDF model cannot be NULL");
130 
131  robot_model_ = createRobotModel(urdf_model, srdf_model);
132  if (!robot_model_)
133  throw moveit::ConstructException("Could not create RobotModel");
134 
135  initialize();
136 }
137 
139 {
142 }
143 
145 {
147 
148  scene_transforms_ = std::make_shared<SceneTransforms>(this);
149 
150  robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
151  robot_state_->setToDefaultValues();
152  robot_state_->update();
153 
154  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*getRobotModel()->getSRDF());
155 
157 }
158 
159 /* return NULL on failure */
160 moveit::core::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
161  const srdf::ModelConstSharedPtr& srdf_model)
162 {
163  moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model, srdf_model));
164  if (!robot_model || !robot_model->getRootJoint())
165  return moveit::core::RobotModelPtr();
166 
167  return robot_model;
168 }
169 
170 PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(parent)
171 {
172  if (!parent_)
173  throw moveit::ConstructException("NULL parent pointer for planning scene");
174 
175  if (!parent_->getName().empty())
176  name_ = parent_->getName() + "+";
177 
178  robot_model_ = parent_->robot_model_;
179 
180  // maintain a separate world. Copy on write ensures that most of the object
181  // info is shared until it is modified.
182  world_ = std::make_shared<collision_detection::World>(*parent_->world_);
184 
185  // record changes to the world
186  world_diff_ = std::make_shared<collision_detection::WorldDiff>(world_);
187 
188  // Set up the same collision detectors as the parent
189  for (const std::pair<const std::string, CollisionDetectorPtr>& it : parent_->collision_)
190  {
191  const CollisionDetectorPtr& parent_detector = it.second;
192  CollisionDetectorPtr& detector = collision_[it.first];
193  detector = std::make_shared<CollisionDetector>();
194  detector->alloc_ = parent_detector->alloc_;
195  detector->parent_ = parent_detector;
196 
197  detector->cenv_ = detector->alloc_->allocateEnv(parent_detector->cenv_, world_);
198  detector->cenv_const_ = detector->cenv_;
199 
200  detector->cenv_unpadded_ = detector->alloc_->allocateEnv(parent_detector->cenv_unpadded_, world_);
201  detector->cenv_unpadded_const_ = detector->cenv_unpadded_;
202  }
203  setActiveCollisionDetector(parent_->getActiveCollisionDetectorName());
204 }
205 
206 PlanningScenePtr PlanningScene::clone(const PlanningSceneConstPtr& scene)
207 {
208  PlanningScenePtr result = scene->diff();
209  result->decoupleParent();
210  result->setName(scene->getName());
211  return result;
212 }
213 
214 PlanningScenePtr PlanningScene::diff() const
215 {
216  return PlanningScenePtr(new PlanningScene(shared_from_this()));
217 }
218 
219 PlanningScenePtr PlanningScene::diff(const moveit_msgs::PlanningScene& msg) const
220 {
221  PlanningScenePtr result = diff();
222  result->setPlanningSceneDiffMsg(msg);
223  return result;
224 }
225 
227 {
228  cenv_->setLinkPadding(src.getCollisionEnv()->getLinkPadding());
229  cenv_->setLinkScale(src.getCollisionEnv()->getLinkScale());
230 }
231 
233 {
234  for (std::pair<const std::string, CollisionDetectorPtr>& it : collision_)
235  {
236  if (it.second != active_collision_)
237  it.second->copyPadding(*active_collision_);
238  }
239 }
240 
242 {
243  if (parent_ || !scene.parent_)
244  return;
245 
246  CollisionDetectorConstIterator it = scene.parent_->collision_.find(alloc_->getName());
247  if (it != scene.parent_->collision_.end())
248  parent_ = it->second->parent_;
249 }
250 
251 void PlanningScene::addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator)
252 {
253  const std::string& name = allocator->getName();
254  CollisionDetectorPtr& detector = collision_[name];
255 
256  if (detector) // already added this one
257  return;
258 
259  detector = std::make_shared<CollisionDetector>();
260 
261  detector->alloc_ = allocator;
262 
263  if (!active_collision_)
264  active_collision_ = detector;
265 
266  detector->findParent(*this);
267 
268  detector->cenv_ = detector->alloc_->allocateEnv(world_, getRobotModel());
269  detector->cenv_const_ = detector->cenv_;
270 
271  // if the current active detector is not the added one, copy its padding to the new one and allocate unpadded
272  if (detector != active_collision_)
273  {
274  detector->cenv_unpadded_ = detector->alloc_->allocateEnv(world_, getRobotModel());
275  detector->cenv_unpadded_const_ = detector->cenv_unpadded_;
276 
277  detector->copyPadding(*active_collision_);
278  }
279 
280  detector->cenv_unpadded_ = detector->alloc_->allocateEnv(world_, getRobotModel());
281  detector->cenv_unpadded_const_ = detector->cenv_unpadded_;
282 }
283 
284 void PlanningScene::setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
285  bool exclusive)
286 {
287  if (exclusive)
288  {
289  CollisionDetectorPtr p;
290  CollisionDetectorIterator it = collision_.find(allocator->getName());
291  if (it != collision_.end())
292  p = it->second;
293 
294  collision_.clear();
295  active_collision_.reset();
296 
297  if (p)
298  {
299  collision_[allocator->getName()] = p;
300  active_collision_ = p;
301  return;
302  }
303  }
304 
305  addCollisionDetector(allocator);
306  setActiveCollisionDetector(allocator->getName());
307 }
308 
309 bool PlanningScene::setActiveCollisionDetector(const std::string& collision_detector_name)
310 {
311  CollisionDetectorIterator it = collision_.find(collision_detector_name);
312  if (it != collision_.end())
313  {
314  active_collision_ = it->second;
315  return true;
316  }
317  else
318  {
320  "Cannot setActiveCollisionDetector to '%s' -- it has been added to PlanningScene. "
321  "Keeping existing active collision detector '%s'",
322  collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str());
323  return false;
324  }
325 }
326 
327 void PlanningScene::getCollisionDetectorNames(std::vector<std::string>& names) const
328 {
329  names.clear();
330  names.reserve(collision_.size());
331  for (const std::pair<const std::string, CollisionDetectorPtr>& it : collision_)
332  names.push_back(it.first);
333 }
334 
335 const collision_detection::CollisionEnvConstPtr&
336 PlanningScene::getCollisionEnv(const std::string& collision_detector_name) const
337 {
338  CollisionDetectorConstIterator it = collision_.find(collision_detector_name);
339  if (it == collision_.end())
340  {
341  ROS_ERROR_NAMED(LOGNAME, "Could not get CollisionRobot named '%s'. Returning active CollisionRobot '%s' instead",
342  collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str());
343  return active_collision_->getCollisionEnv();
344  }
345 
346  return it->second->getCollisionEnv();
347 }
348 
349 const collision_detection::CollisionEnvConstPtr&
350 PlanningScene::getCollisionEnvUnpadded(const std::string& collision_detector_name) const
351 {
352  CollisionDetectorConstIterator it = collision_.find(collision_detector_name);
353  if (it == collision_.end())
354  {
356  "Could not get CollisionRobotUnpadded named '%s'. "
357  "Returning active CollisionRobotUnpadded '%s' instead",
358  collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str());
359  return active_collision_->getCollisionEnvUnpadded();
360  }
361 
362  return it->second->getCollisionEnvUnpadded();
363 }
364 
366 {
367  if (!parent_)
368  return;
369 
370  // clear everything, reset the world, record diffs
371  world_ = std::make_shared<collision_detection::World>(*parent_->world_);
373  world_diff_ = std::make_shared<collision_detection::WorldDiff>(world_);
376 
377  // use parent crobot_ if it exists. Otherwise copy padding from parent.
378  for (std::pair<const std::string, CollisionDetectorPtr>& it : collision_)
379  {
380  if (!it.second->parent_)
381  it.second->findParent(*this);
382 
383  if (it.second->parent_)
384  {
385  it.second->cenv_unpadded_ = it.second->alloc_->allocateEnv(it.second->parent_->cenv_unpadded_, world_);
386  it.second->cenv_unpadded_const_ = it.second->cenv_unpadded_;
387 
388  it.second->cenv_ = it.second->alloc_->allocateEnv(it.second->parent_->cenv_, world_);
389  it.second->cenv_const_ = it.second->cenv_;
390  }
391  else // This is the parent CollisionDetector
392  {
393  it.second->copyPadding(*parent_->active_collision_);
394  it.second->cenv_const_ = it.second->cenv_;
395  }
396  }
397 
398  scene_transforms_.reset();
399  robot_state_.reset();
400  acm_.reset();
401  object_colors_.reset();
402  object_types_.reset();
403 }
404 
405 void PlanningScene::pushDiffs(const PlanningScenePtr& scene)
406 {
407  if (!parent_)
408  return;
409 
410  if (scene_transforms_)
411  scene->getTransformsNonConst().setAllTransforms(scene_transforms_->getAllTransforms());
412 
413  if (robot_state_)
414  {
415  scene->getCurrentStateNonConst() = *robot_state_;
416  // push colors and types for attached objects
417  std::vector<const moveit::core::AttachedBody*> attached_objs;
418  robot_state_->getAttachedBodies(attached_objs);
419  for (const moveit::core::AttachedBody* attached_obj : attached_objs)
420  {
421  if (hasObjectType(attached_obj->getName()))
422  scene->setObjectType(attached_obj->getName(), getObjectType(attached_obj->getName()));
423  if (hasObjectColor(attached_obj->getName()))
424  scene->setObjectColor(attached_obj->getName(), getObjectColor(attached_obj->getName()));
425  }
426  }
427 
428  if (acm_)
429  scene->getAllowedCollisionMatrixNonConst() = *acm_;
430 
431  collision_detection::CollisionEnvPtr active_cenv = scene->getCollisionEnvNonConst();
432  active_cenv->setLinkPadding(active_collision_->cenv_->getLinkPadding());
433  active_cenv->setLinkScale(active_collision_->cenv_->getLinkScale());
434  scene->propogateRobotPadding();
435 
436  if (world_diff_)
437  {
438  for (const std::pair<const std::string, collision_detection::World::Action>& it : *world_diff_)
439  {
440  if (it.second == collision_detection::World::DESTROY)
441  {
442  scene->world_->removeObject(it.first);
443  scene->removeObjectColor(it.first);
444  scene->removeObjectType(it.first);
445  }
446  else
447  {
448  const collision_detection::World::Object& obj = *world_->getObject(it.first);
449  scene->world_->removeObject(obj.id_);
450  scene->world_->addToObject(obj.id_, obj.pose_, obj.shapes_, obj.shape_poses_);
451  if (hasObjectColor(it.first))
452  scene->setObjectColor(it.first, getObjectColor(it.first));
453  if (hasObjectType(it.first))
454  scene->setObjectType(it.first, getObjectType(it.first));
455 
456  scene->world_->setSubframesOfObject(obj.id_, obj.subframe_poses_);
457  }
458  }
459  }
460 }
461 
464 {
465  if (getCurrentState().dirtyCollisionBodyTransforms())
467  else
468  checkCollision(req, res, getCurrentState());
469 }
470 
474 {
476 }
477 
480 {
481  if (getCurrentState().dirtyCollisionBodyTransforms())
483  else
484  checkSelfCollision(req, res, getCurrentState());
485 }
486 
491 {
492  // check collision with the world using the padded version
493  getCollisionEnv()->checkRobotCollision(req, res, robot_state, acm);
494 
495  // do self-collision checking with the unpadded version of the robot
496  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
497  getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm);
498 }
499 
502 {
503  if (getCurrentState().dirtyCollisionBodyTransforms())
505  else
507 }
508 
513 {
514  // check collision with the world using the unpadded version
515  getCollisionEnvUnpadded()->checkRobotCollision(req, res, robot_state, acm);
516 
517  // do self-collision checking with the unpadded version of the robot
518  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
519  {
520  getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm);
521  }
522 }
523 
525 {
526  if (getCurrentState().dirtyCollisionBodyTransforms())
528  else
530 }
531 
535  const std::string& group_name) const
536 {
538  req.contacts = true;
539  req.max_contacts = getRobotModel()->getLinkModelsWithCollisionGeometry().size() + 1;
540  req.max_contacts_per_pair = 1;
541  req.group_name = group_name;
543  checkCollision(req, res, robot_state, acm);
544  res.contacts.swap(contacts);
545 }
546 
547 void PlanningScene::getCollidingLinks(std::vector<std::string>& links)
548 {
549  if (getCurrentState().dirtyCollisionBodyTransforms())
551  else
553 }
554 
555 void PlanningScene::getCollidingLinks(std::vector<std::string>& links, const moveit::core::RobotState& robot_state,
557 {
559  getCollidingPairs(contacts, robot_state, acm);
560  links.clear();
561  for (collision_detection::CollisionResult::ContactMap::const_iterator it = contacts.begin(); it != contacts.end();
562  ++it)
563  for (const collision_detection::Contact& contact : it->second)
564  {
566  links.push_back(contact.body_name_1);
568  links.push_back(contact.body_name_2);
569  }
570 }
571 
572 const collision_detection::CollisionEnvPtr& PlanningScene::getCollisionEnvNonConst()
573 {
574  return active_collision_->cenv_;
575 }
576 
578 {
580  {
581  robot_state_ = std::make_shared<moveit::core::RobotState>(parent_->getCurrentState());
582  robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
583  }
584  robot_state_->update();
585  return *robot_state_;
586 }
587 
588 moveit::core::RobotStatePtr PlanningScene::getCurrentStateUpdated(const moveit_msgs::RobotState& update) const
589 {
590  moveit::core::RobotStatePtr state(new moveit::core::RobotState(getCurrentState()));
592  return state;
593 }
594 
596 {
598  if (robot_state_)
599  robot_state_->setAttachedBodyUpdateCallback(callback);
600 }
601 
603 {
606  if (callback)
607  current_world_object_update_observer_handle_ = world_->addObserver(callback);
609 }
610 
612 {
613  if (!acm_)
614  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(parent_->getAllowedCollisionMatrix());
615  return *acm_;
616 }
617 
619 {
620  // Trigger an update of the robot transforms
622  return static_cast<const PlanningScene*>(this)->getTransforms();
623 }
624 
626 {
627  // Trigger an update of the robot transforms
629  if (!scene_transforms_)
630  {
631  // The only case when there are no transforms is if this planning scene has a parent. When a non-const version of
632  // the planning scene is requested, a copy of the parent's transforms is forced
633  scene_transforms_ = std::make_shared<SceneTransforms>(this);
634  scene_transforms_->setAllTransforms(parent_->getTransforms().getAllTransforms());
635  }
636  return *scene_transforms_;
637 }
638 
639 void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene_msg) const
640 {
641  scene_msg.name = name_;
642  scene_msg.robot_model_name = getRobotModel()->getName();
643  scene_msg.is_diff = true;
644 
645  if (scene_transforms_)
646  scene_transforms_->copyTransforms(scene_msg.fixed_frame_transforms);
647  else
648  scene_msg.fixed_frame_transforms.clear();
649 
650  if (robot_state_)
651  moveit::core::robotStateToRobotStateMsg(*robot_state_, scene_msg.robot_state);
652  else
653  scene_msg.robot_state = moveit_msgs::RobotState();
654  scene_msg.robot_state.is_diff = true;
655 
656  if (acm_)
657  acm_->getMessage(scene_msg.allowed_collision_matrix);
658  else
659  scene_msg.allowed_collision_matrix = moveit_msgs::AllowedCollisionMatrix();
660 
661  active_collision_->cenv_->getPadding(scene_msg.link_padding);
662  active_collision_->cenv_->getScale(scene_msg.link_scale);
663 
664  scene_msg.object_colors.clear();
665  if (object_colors_)
666  {
667  unsigned int i = 0;
668  scene_msg.object_colors.resize(object_colors_->size());
669  for (ObjectColorMap::const_iterator it = object_colors_->begin(); it != object_colors_->end(); ++it, ++i)
670  {
671  scene_msg.object_colors[i].id = it->first;
672  scene_msg.object_colors[i].color = it->second;
673  }
674  }
675 
676  scene_msg.world.collision_objects.clear();
677  scene_msg.world.octomap = octomap_msgs::OctomapWithPose();
678 
679  if (world_diff_)
680  {
681  bool do_omap = false;
682  for (const std::pair<const std::string, collision_detection::World::Action>& it : *world_diff_)
683  {
684  if (it.first == OCTOMAP_NS)
685  {
686  if (it.second == collision_detection::World::DESTROY)
687  scene_msg.world.octomap.octomap.id = "cleared"; // indicate cleared octomap
688  else
689  do_omap = true;
690  }
691  else if (it.second == collision_detection::World::DESTROY)
692  {
693  // if object became attached, it should not be recorded as removed here
694  if (!std::count_if(scene_msg.robot_state.attached_collision_objects.cbegin(),
695  scene_msg.robot_state.attached_collision_objects.cend(),
696  [&it](const moveit_msgs::AttachedCollisionObject& aco) {
697  return aco.object.id == it.first &&
698  aco.object.operation == moveit_msgs::CollisionObject::ADD;
699  }))
700  {
701  moveit_msgs::CollisionObject co;
702  co.header.frame_id = getPlanningFrame();
703  co.id = it.first;
704  co.operation = moveit_msgs::CollisionObject::REMOVE;
705  scene_msg.world.collision_objects.push_back(co);
706  }
707  }
708  else
709  {
710  scene_msg.world.collision_objects.emplace_back();
711  getCollisionObjectMsg(scene_msg.world.collision_objects.back(), it.first);
712  }
713  }
714  if (do_omap)
715  getOctomapMsg(scene_msg.world.octomap);
716  }
717 
718  // Ensure all detached collision objects actually get removed when applying the diff
719  // Because RobotState doesn't handle diffs (yet), we explicitly declare attached objects
720  // as removed, if they show up as "normal" collision objects but were attached in parent
721  for (const auto& collision_object : scene_msg.world.collision_objects)
722  {
723  if (parent_ && parent_->getCurrentState().hasAttachedBody(collision_object.id))
724  {
725  moveit_msgs::AttachedCollisionObject aco;
726  aco.object.id = collision_object.id;
727  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
728  scene_msg.robot_state.attached_collision_objects.push_back(aco);
729  }
730  }
731 }
732 
733 namespace
734 {
735 class ShapeVisitorAddToCollisionObject : public boost::static_visitor<void>
736 {
737 public:
738  ShapeVisitorAddToCollisionObject(moveit_msgs::CollisionObject* obj) : boost::static_visitor<void>(), obj_(obj)
739  {
740  }
741 
742  void setPoseMessage(const geometry_msgs::Pose* pose)
743  {
744  pose_ = pose;
745  }
746 
747  void operator()(const shape_msgs::Plane& shape_msg) const
748  {
749  obj_->planes.push_back(shape_msg);
750  obj_->plane_poses.push_back(*pose_);
751  }
752 
753  void operator()(const shape_msgs::Mesh& shape_msg) const
754  {
755  obj_->meshes.push_back(shape_msg);
756  obj_->mesh_poses.push_back(*pose_);
757  }
758 
759  void operator()(const shape_msgs::SolidPrimitive& shape_msg) const
760  {
761  obj_->primitives.push_back(shape_msg);
762  obj_->primitive_poses.push_back(*pose_);
763  }
764 
765 private:
766  moveit_msgs::CollisionObject* obj_;
767  const geometry_msgs::Pose* pose_;
768 };
769 } // namespace
770 
771 bool PlanningScene::getCollisionObjectMsg(moveit_msgs::CollisionObject& collision_obj, const std::string& ns) const
772 {
774  if (!obj)
775  return false;
776  collision_obj.header.frame_id = getPlanningFrame();
777  collision_obj.pose = tf2::toMsg(obj->pose_);
778  collision_obj.id = ns;
779  collision_obj.operation = moveit_msgs::CollisionObject::ADD;
780  ShapeVisitorAddToCollisionObject sv(&collision_obj);
781  for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
782  {
783  shapes::ShapeMsg sm;
784  if (constructMsgFromShape(obj->shapes_[j].get(), sm))
785  {
786  geometry_msgs::Pose p = tf2::toMsg(obj->shape_poses_[j]);
787  sv.setPoseMessage(&p);
788  boost::apply_visitor(sv, sm);
789  }
790  }
791 
792  if (!collision_obj.primitives.empty() || !collision_obj.meshes.empty() || !collision_obj.planes.empty())
793  {
794  if (hasObjectType(collision_obj.id))
795  collision_obj.type = getObjectType(collision_obj.id);
796  }
797  for (const auto& frame_pair : obj->subframe_poses_)
798  {
799  collision_obj.subframe_names.push_back(frame_pair.first);
800  geometry_msgs::Pose p;
801  p = tf2::toMsg(frame_pair.second);
802  collision_obj.subframe_poses.push_back(p);
803  }
804 
805  return true;
806 }
807 
808 void PlanningScene::getCollisionObjectMsgs(std::vector<moveit_msgs::CollisionObject>& collision_objs) const
809 {
810  collision_objs.clear();
811  const std::vector<std::string>& ids = world_->getObjectIds();
812  for (const std::string& id : ids)
813  if (id != OCTOMAP_NS)
814  {
815  collision_objs.emplace_back();
816  getCollisionObjectMsg(collision_objs.back(), id);
817  }
818 }
819 
820 bool PlanningScene::getAttachedCollisionObjectMsg(moveit_msgs::AttachedCollisionObject& attached_collision_obj,
821  const std::string& ns) const
822 {
823  std::vector<moveit_msgs::AttachedCollisionObject> attached_collision_objs;
824  getAttachedCollisionObjectMsgs(attached_collision_objs);
825  for (moveit_msgs::AttachedCollisionObject& it : attached_collision_objs)
826  {
827  if (it.object.id == ns)
828  {
829  attached_collision_obj = it;
830  return true;
831  }
832  }
833  return false;
834 }
835 
837  std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs) const
838 {
839  std::vector<const moveit::core::AttachedBody*> attached_bodies;
840  getCurrentState().getAttachedBodies(attached_bodies);
841  attachedBodiesToAttachedCollisionObjectMsgs(attached_bodies, attached_collision_objs);
842 }
843 
844 bool PlanningScene::getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const
845 {
846  octomap.header.frame_id = getPlanningFrame();
847  octomap.octomap = octomap_msgs::Octomap();
848 
850  if (map)
851  {
852  if (map->shapes_.size() == 1)
853  {
854  const shapes::OcTree* o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
856  octomap.origin = tf2::toMsg(map->shape_poses_[0]);
857  return true;
858  }
859  ROS_ERROR_NAMED(LOGNAME, "Unexpected number of shapes in octomap collision object. Not including '%s' object",
860  OCTOMAP_NS.c_str());
861  }
862  return false;
863 }
864 
865 void PlanningScene::getObjectColorMsgs(std::vector<moveit_msgs::ObjectColor>& object_colors) const
866 {
867  object_colors.clear();
868 
869  unsigned int i = 0;
870  ObjectColorMap cmap;
871  getKnownObjectColors(cmap);
872  object_colors.resize(cmap.size());
873  for (ObjectColorMap::const_iterator it = cmap.begin(); it != cmap.end(); ++it, ++i)
874  {
875  object_colors[i].id = it->first;
876  object_colors[i].color = it->second;
877  }
878 }
879 
880 void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg) const
881 {
882  scene_msg.name = name_;
883  scene_msg.is_diff = false;
884  scene_msg.robot_model_name = getRobotModel()->getName();
885  getTransforms().copyTransforms(scene_msg.fixed_frame_transforms);
886 
888  getAllowedCollisionMatrix().getMessage(scene_msg.allowed_collision_matrix);
889  getCollisionEnv()->getPadding(scene_msg.link_padding);
890  getCollisionEnv()->getScale(scene_msg.link_scale);
891 
892  getObjectColorMsgs(scene_msg.object_colors);
893 
894  // add collision objects
895  getCollisionObjectMsgs(scene_msg.world.collision_objects);
896 
897  // get the octomap
898  getOctomapMsg(scene_msg.world.octomap);
899 }
900 
901 void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg,
902  const moveit_msgs::PlanningSceneComponents& comp) const
903 {
904  scene_msg.is_diff = false;
905  if (comp.components & moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS)
906  {
907  scene_msg.name = name_;
908  scene_msg.robot_model_name = getRobotModel()->getName();
909  }
910 
911  if (comp.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
912  getTransforms().copyTransforms(scene_msg.fixed_frame_transforms);
913 
914  if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS)
915  {
916  moveit::core::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, true);
917  for (moveit_msgs::AttachedCollisionObject& attached_collision_object :
918  scene_msg.robot_state.attached_collision_objects)
919  {
920  if (hasObjectType(attached_collision_object.object.id))
921  {
922  attached_collision_object.object.type = getObjectType(attached_collision_object.object.id);
923  }
924  }
925  }
926  else if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE)
927  {
928  moveit::core::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, false);
929  }
930 
931  if (comp.components & moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX)
932  getAllowedCollisionMatrix().getMessage(scene_msg.allowed_collision_matrix);
933 
934  if (comp.components & moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING)
935  {
936  getCollisionEnv()->getPadding(scene_msg.link_padding);
937  getCollisionEnv()->getScale(scene_msg.link_scale);
938  }
939 
940  if (comp.components & moveit_msgs::PlanningSceneComponents::OBJECT_COLORS)
941  getObjectColorMsgs(scene_msg.object_colors);
942 
943  // add collision objects
944  if (comp.components & moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY)
945  getCollisionObjectMsgs(scene_msg.world.collision_objects);
946  else if (comp.components & moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES)
947  {
948  const std::vector<std::string>& ids = world_->getObjectIds();
949  scene_msg.world.collision_objects.clear();
950  scene_msg.world.collision_objects.reserve(ids.size());
951  for (const std::string& id : ids)
952  if (id != OCTOMAP_NS)
953  {
954  moveit_msgs::CollisionObject co;
955  co.id = id;
956  if (hasObjectType(co.id))
957  co.type = getObjectType(co.id);
958  scene_msg.world.collision_objects.push_back(co);
959  }
960  }
961 
962  // get the octomap
963  if (comp.components & moveit_msgs::PlanningSceneComponents::OCTOMAP)
964  getOctomapMsg(scene_msg.world.octomap);
965 }
966 
967 void PlanningScene::saveGeometryToStream(std::ostream& out) const
968 {
969  out << name_ << std::endl;
970  const std::vector<std::string>& ids = world_->getObjectIds();
971  for (const std::string& id : ids)
972  if (id != OCTOMAP_NS)
973  {
975  if (obj)
976  {
977  out << "* " << id << std::endl; // New object start
978  // Write object pose
979  writePoseToText(out, obj->pose_);
980 
981  // Write shapes and shape poses
982  out << obj->shapes_.size() << std::endl; // Number of shapes
983  for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
984  {
985  shapes::saveAsText(obj->shapes_[j].get(), out);
986  // shape_poses_ is valid isometry by contract
987  writePoseToText(out, obj->shape_poses_[j]);
988  if (hasObjectColor(id))
989  {
990  const std_msgs::ColorRGBA& c = getObjectColor(id);
991  out << c.r << " " << c.g << " " << c.b << " " << c.a << std::endl;
992  }
993  else
994  out << "0 0 0 0" << std::endl;
995  }
996 
997  // Write subframes
998  out << obj->subframe_poses_.size() << std::endl; // Number of subframes
999  for (auto& pose_pair : obj->subframe_poses_)
1000  {
1001  out << pose_pair.first << std::endl; // Subframe name
1002  writePoseToText(out, pose_pair.second); // Subframe pose
1003  }
1004  }
1005  }
1006  out << "." << std::endl;
1007 }
1008 
1009 bool PlanningScene::loadGeometryFromStream(std::istream& in)
1010 {
1011  return loadGeometryFromStream(in, Eigen::Isometry3d::Identity()); // Use no offset
1012 }
1013 
1014 bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isometry3d& offset)
1015 {
1016  if (!in.good() || in.eof())
1017  {
1018  ROS_ERROR_NAMED(LOGNAME, "Bad input stream when loading scene geometry");
1019  return false;
1020  }
1021  // Read scene name
1022  std::getline(in, name_);
1023 
1024  // Identify scene format version for backwards compatibility of parser
1025  auto pos = in.tellg(); // remember current stream position
1026  std::string line;
1027  do
1028  {
1029  std::getline(in, line);
1030  } while (in.good() && !in.eof() && (line.empty() || line[0] != '*')); // read * marker
1031  std::getline(in, line); // next line determines format
1032  boost::algorithm::trim(line);
1033  // new format: line specifies position of object, with spaces as delimiter -> spaces indicate new format
1034  // old format: line specifies number of shapes
1035  bool uses_new_scene_format = line.find(' ') != std::string::npos;
1036  in.seekg(pos);
1037 
1038  Eigen::Isometry3d pose; // Transient
1039  do
1040  {
1041  std::string marker;
1042  in >> marker;
1043  if (!in.good() || in.eof())
1044  {
1045  ROS_ERROR_NAMED(LOGNAME, "Bad input stream when loading marker in scene geometry");
1046  return false;
1047  }
1048  if (marker == "*") // Start of new object
1049  {
1050  std::string object_id;
1051  std::getline(in, object_id);
1052  if (!in.good() || in.eof())
1053  {
1054  ROS_ERROR_NAMED(LOGNAME, "Bad input stream when loading object_id in scene geometry");
1055  return false;
1056  }
1057  boost::algorithm::trim(object_id);
1058 
1059  // Read in object pose (added in the new scene format)
1060  pose.setIdentity();
1061  if (uses_new_scene_format && !readPoseFromText(in, pose))
1062  {
1063  ROS_ERROR_NAMED(LOGNAME, "Failed to read object pose from scene file");
1064  return false;
1065  }
1066  Eigen::Isometry3d object_pose = offset * pose; // Transform pose by input pose offset
1067 
1068  // Read in shapes
1069  unsigned int shape_count;
1070  in >> shape_count;
1071  if (shape_count) // If there are any shapes to be loaded, clear any existing object first
1072  world_->removeObject(object_id);
1073  for (std::size_t i = 0; i < shape_count && in.good() && !in.eof(); ++i)
1074  {
1075  const auto shape = shapes::ShapeConstPtr(shapes::constructShapeFromText(in));
1076  if (!shape)
1077  {
1078  ROS_ERROR_NAMED(LOGNAME, "Failed to load shape from scene file");
1079  return false;
1080  }
1081  if (!readPoseFromText(in, pose))
1082  {
1083  ROS_ERROR_NAMED(LOGNAME, "Failed to read pose from scene file");
1084  return false;
1085  }
1086  float r, g, b, a;
1087  if (!(in >> r >> g >> b >> a))
1088  {
1089  ROS_ERROR_NAMED(LOGNAME, "Improperly formatted color in scene geometry file");
1090  return false;
1091  }
1092  if (shape)
1093  {
1094  world_->addToObject(object_id, shape, pose);
1095  if (r > 0.0f || g > 0.0f || b > 0.0f || a > 0.0f)
1096  {
1097  std_msgs::ColorRGBA color;
1098  color.r = r;
1099  color.g = g;
1100  color.b = b;
1101  color.a = a;
1102  setObjectColor(object_id, color);
1103  }
1104  }
1105  }
1106 
1107  // Finally set object's pose once
1108  world_->setObjectPose(object_id, object_pose);
1109 
1110  // Read in subframes (added in the new scene format)
1111  if (uses_new_scene_format)
1112  {
1114  unsigned int subframe_count;
1115  in >> subframe_count;
1116  for (std::size_t i = 0; i < subframe_count && in.good() && !in.eof(); ++i)
1117  {
1118  std::string subframe_name;
1119  in >> subframe_name;
1120  if (!readPoseFromText(in, pose))
1121  {
1122  ROS_ERROR_NAMED(LOGNAME, "Failed to read subframe pose from scene file");
1123  return false;
1124  }
1125  subframes[subframe_name] = pose;
1126  }
1127  world_->setSubframesOfObject(object_id, subframes);
1128  }
1129  }
1130  else if (marker == ".")
1131  {
1132  // Marks the end of the scene geometry;
1133  return true;
1134  }
1135  else
1136  {
1137  ROS_ERROR_STREAM_NAMED(LOGNAME, "Unknown marker in scene geometry file: " << marker);
1138  return false;
1139  }
1140  } while (true);
1141 }
1142 
1143 bool PlanningScene::readPoseFromText(std::istream& in, Eigen::Isometry3d& pose) const
1144 {
1145  double x, y, z, rx, ry, rz, rw;
1146  if (!(in >> x >> y >> z))
1147  {
1148  ROS_ERROR_NAMED(LOGNAME, "Improperly formatted translation in scene geometry file");
1149  return false;
1150  }
1151  if (!(in >> rx >> ry >> rz >> rw))
1152  {
1153  ROS_ERROR_NAMED(LOGNAME, "Improperly formatted rotation in scene geometry file");
1154  return false;
1155  }
1156  pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz);
1157  return true;
1158 }
1159 
1160 void PlanningScene::writePoseToText(std::ostream& out, const Eigen::Isometry3d& pose) const
1161 {
1162  out << pose.translation().x() << " " << pose.translation().y() << " " << pose.translation().z() << std::endl;
1163  Eigen::Quaterniond r(pose.linear());
1164  out << r.x() << " " << r.y() << " " << r.z() << " " << r.w() << std::endl;
1165 }
1166 
1167 void PlanningScene::setCurrentState(const moveit_msgs::RobotState& state)
1168 {
1169  // The attached bodies will be processed separately by processAttachedCollisionObjectMsgs
1170  // after robot_state_ has been updated
1171  moveit_msgs::RobotState state_no_attached(state);
1172  state_no_attached.attached_collision_objects.clear();
1173 
1174  if (parent_)
1175  {
1176  if (!robot_state_)
1177  {
1178  robot_state_ = std::make_shared<moveit::core::RobotState>(parent_->getCurrentState());
1179  robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1180  }
1182  }
1183  else
1185 
1186  for (std::size_t i = 0; i < state.attached_collision_objects.size(); ++i)
1187  {
1188  if (!state.is_diff && state.attached_collision_objects[i].object.operation != moveit_msgs::CollisionObject::ADD)
1189  {
1191  "The specified RobotState is not marked as is_diff. "
1192  "The request to modify the object '%s' is not supported. Object is ignored.",
1193  state.attached_collision_objects[i].object.id.c_str());
1194  continue;
1195  }
1196  processAttachedCollisionObjectMsg(state.attached_collision_objects[i]);
1197  }
1198 }
1201 {
1202  getCurrentStateNonConst() = state;
1203 }
1204 
1206 {
1207  if (!parent_)
1208  return;
1209 
1210  // This child planning scene did not have its own copy of frame transforms
1211  if (!scene_transforms_)
1212  {
1213  scene_transforms_ = std::make_shared<SceneTransforms>(this);
1214  scene_transforms_->setAllTransforms(parent_->getTransforms().getAllTransforms());
1215  }
1216 
1217  if (!robot_state_)
1218  {
1219  robot_state_ = std::make_shared<moveit::core::RobotState>(parent_->getCurrentState());
1220  robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1221  }
1222 
1223  if (!acm_)
1224  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(parent_->getAllowedCollisionMatrix());
1225 
1226  for (std::pair<const std::string, CollisionDetectorPtr>& it : collision_)
1227  {
1228  it.second->parent_.reset();
1229  }
1230  world_diff_.reset();
1231 
1233  {
1234  ObjectColorMap kc;
1235  parent_->getKnownObjectColors(kc);
1236  object_colors_ = std::make_unique<ObjectColorMap>(kc);
1237  }
1238  else
1239  {
1240  ObjectColorMap kc;
1241  parent_->getKnownObjectColors(kc);
1242  for (ObjectColorMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
1243  if (object_colors_->find(it->first) == object_colors_->end())
1244  (*object_colors_)[it->first] = it->second;
1245  }
1246 
1247  if (!object_types_)
1248  {
1249  ObjectTypeMap kc;
1250  parent_->getKnownObjectTypes(kc);
1251  object_types_ = std::make_unique<ObjectTypeMap>(kc);
1252  }
1253  else
1254  {
1255  ObjectTypeMap kc;
1256  parent_->getKnownObjectTypes(kc);
1257  for (ObjectTypeMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
1258  if (object_types_->find(it->first) == object_types_->end())
1259  (*object_types_)[it->first] = it->second;
1260  }
1261 
1262  parent_.reset();
1263 }
1264 
1265 bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& scene_msg)
1266 {
1267  bool result = true;
1268 
1269  ROS_DEBUG_NAMED(LOGNAME, "Adding planning scene diff");
1270  if (!scene_msg.name.empty())
1271  name_ = scene_msg.name;
1272 
1273  if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name != getRobotModel()->getName())
1274  ROS_WARN_NAMED(LOGNAME, "Setting the scene for model '%s' but model '%s' is loaded.",
1275  scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str());
1276 
1277  // there is at least one transform in the list of fixed transform: from model frame to itself;
1278  // if the list is empty, then nothing has been set
1279  if (!scene_msg.fixed_frame_transforms.empty())
1280  {
1281  if (!scene_transforms_)
1282  scene_transforms_ = std::make_shared<SceneTransforms>(this);
1283  scene_transforms_->setTransforms(scene_msg.fixed_frame_transforms);
1284  }
1285 
1286  // if at least some joints have been specified, we set them
1287  if (!scene_msg.robot_state.multi_dof_joint_state.joint_names.empty() ||
1288  !scene_msg.robot_state.joint_state.name.empty() || !scene_msg.robot_state.attached_collision_objects.empty())
1289  setCurrentState(scene_msg.robot_state);
1290 
1291  // if at least some links are mentioned in the allowed collision matrix, then we have an update
1292  if (!scene_msg.allowed_collision_matrix.entry_names.empty())
1293  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(scene_msg.allowed_collision_matrix);
1294 
1295  if (!scene_msg.link_padding.empty() || !scene_msg.link_scale.empty())
1296  {
1297  for (std::pair<const std::string, CollisionDetectorPtr>& it : collision_)
1298  {
1299  it.second->cenv_->setPadding(scene_msg.link_padding);
1300  it.second->cenv_->setScale(scene_msg.link_scale);
1301  }
1302  }
1303 
1304  // if any colors have been specified, replace the ones we have with the specified ones
1305  for (const moveit_msgs::ObjectColor& object_color : scene_msg.object_colors)
1306  setObjectColor(object_color.id, object_color.color);
1307 
1308  // process collision object updates
1309  for (const moveit_msgs::CollisionObject& collision_object : scene_msg.world.collision_objects)
1310  result &= processCollisionObjectMsg(collision_object);
1311 
1312  // if an octomap was specified, replace the one we have with that one
1313  if (!scene_msg.world.octomap.octomap.id.empty())
1314  processOctomapMsg(scene_msg.world.octomap);
1315 
1316  return result;
1317 }
1318 
1319 bool PlanningScene::setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene_msg)
1320 {
1321  ROS_DEBUG_NAMED(LOGNAME, "Setting new planning scene: '%s'", scene_msg.name.c_str());
1322  name_ = scene_msg.name;
1323 
1324  if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name != getRobotModel()->getName())
1325  ROS_WARN_NAMED(LOGNAME, "Setting the scene for model '%s' but model '%s' is loaded.",
1326  scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str());
1327 
1328  if (parent_)
1329  decoupleParent();
1330 
1331  object_types_.reset();
1332  scene_transforms_->setTransforms(scene_msg.fixed_frame_transforms);
1333  setCurrentState(scene_msg.robot_state);
1334  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(scene_msg.allowed_collision_matrix);
1335  for (std::pair<const std::string, CollisionDetectorPtr>& it : collision_)
1336  {
1337  it.second->cenv_->setPadding(scene_msg.link_padding);
1338  it.second->cenv_->setScale(scene_msg.link_scale);
1339  }
1340  object_colors_ = std::make_unique<ObjectColorMap>();
1341  for (const moveit_msgs::ObjectColor& object_color : scene_msg.object_colors)
1342  setObjectColor(object_color.id, object_color.color);
1343  world_->clearObjects();
1344  return processPlanningSceneWorldMsg(scene_msg.world);
1345 }
1346 
1347 bool PlanningScene::processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld& world)
1348 {
1349  bool result = true;
1350  for (const moveit_msgs::CollisionObject& collision_object : world.collision_objects)
1351  result &= processCollisionObjectMsg(collision_object);
1352  processOctomapMsg(world.octomap);
1353  return result;
1354 }
1355 
1356 bool PlanningScene::usePlanningSceneMsg(const moveit_msgs::PlanningScene& scene_msg)
1357 {
1358  if (scene_msg.is_diff)
1359  return setPlanningSceneDiffMsg(scene_msg);
1360  else
1361  return setPlanningSceneMsg(scene_msg);
1362 }
1363 
1364 collision_detection::OccMapTreePtr createOctomap(const octomap_msgs::Octomap& map)
1365 {
1366  std::shared_ptr<collision_detection::OccMapTree> om =
1367  std::make_shared<collision_detection::OccMapTree>(map.resolution);
1368  if (map.binary)
1369  {
1370  octomap_msgs::readTree(om.get(), map);
1371  }
1372  else
1373  {
1374  std::stringstream datastream;
1375  if (!map.data.empty())
1376  {
1377  datastream.write((const char*)&map.data[0], map.data.size());
1378  om->readData(datastream);
1379  }
1380  }
1381  return om;
1382 }
1383 
1384 void PlanningScene::processOctomapMsg(const octomap_msgs::Octomap& map)
1385 {
1386  // each octomap replaces any previous one
1387  world_->removeObject(OCTOMAP_NS);
1389  if (map.data.empty())
1390  return;
1391 
1392  if (map.id != "OcTree")
1393  {
1394  ROS_ERROR_NAMED(LOGNAME, "Received octomap is of type '%s' but type 'OcTree' is expected.", map.id.c_str());
1395  return;
1396  }
1397 
1398  std::shared_ptr<collision_detection::OccMapTree> om = createOctomap(map);
1399  if (!map.header.frame_id.empty())
1400  {
1401  const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id);
1402  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t);
1403  }
1404  else
1405  {
1406  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), Eigen::Isometry3d::Identity());
1407  }
1408 }
1409 
1411 {
1412  const std::vector<std::string>& object_ids = world_->getObjectIds();
1413  for (const std::string& object_id : object_ids)
1414  if (object_id != OCTOMAP_NS)
1415  {
1416  world_->removeObject(object_id);
1417  removeObjectColor(object_id);
1418  removeObjectType(object_id);
1419  }
1420 }
1421 
1422 void PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose& map)
1423 {
1424  // each octomap replaces any previous one
1425  world_->removeObject(OCTOMAP_NS);
1426 
1427  if (map.octomap.data.empty())
1428  return;
1429 
1430  if (map.octomap.id != "OcTree")
1431  {
1432  ROS_ERROR_NAMED(LOGNAME, "Received octomap is of type '%s' but type 'OcTree' is expected.", map.octomap.id.c_str());
1433  return;
1434  }
1435 
1436  std::shared_ptr<collision_detection::OccMapTree> om = createOctomap(map.octomap);
1437 
1438  const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id);
1439  Eigen::Isometry3d p;
1440  PlanningScene::poseMsgToEigen(map.origin, p);
1441  p = t * p;
1443 }
1444 
1445 void PlanningScene::processOctomapPtr(const std::shared_ptr<const octomap::OcTree>& octree, const Eigen::Isometry3d& t)
1446 {
1448  if (map)
1449  {
1450  if (map->shapes_.size() == 1)
1451  {
1452  // check to see if we have the same octree pointer & pose.
1453  const shapes::OcTree* o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
1454  if (o->octree == octree)
1455  {
1456  // if the pose changed, we update it
1457  if (map->shape_poses_[0].isApprox(t, std::numeric_limits<double>::epsilon() * 100.0))
1458  {
1459  if (world_diff_)
1462  }
1463  else
1464  {
1465  shapes::ShapeConstPtr shape = map->shapes_[0];
1466  map.reset(); // reset this pointer first so that caching optimizations can be used in CollisionWorld
1467  world_->moveShapeInObject(OCTOMAP_NS, shape, t);
1468  }
1469  return;
1470  }
1471  }
1472  }
1473  // if the octree pointer changed, update the structure
1474  world_->removeObject(OCTOMAP_NS);
1475  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(octree)), t);
1476 }
1478 bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject& object)
1479 {
1480  if (object.object.operation == moveit_msgs::CollisionObject::ADD && !getRobotModel()->hasLinkModel(object.link_name))
1481  {
1482  ROS_ERROR_NAMED(LOGNAME, "Unable to attach a body to link '%s' (link not found)", object.link_name.c_str());
1483  return false;
1484  }
1485 
1486  if (object.object.id == OCTOMAP_NS)
1487  {
1488  ROS_ERROR_NAMED(LOGNAME, "The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str());
1489  return false;
1490  }
1491 
1492  if (!robot_state_) // there must be a parent in this case
1493  {
1494  robot_state_ = std::make_shared<moveit::core::RobotState>(parent_->getCurrentState());
1495  robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1496  }
1497  robot_state_->update();
1498 
1499  // The ADD/REMOVE operations follow this order:
1500  // STEP 1: Get info about the object from either the message or the world/RobotState
1501  // STEP 2: Remove the object from the world/RobotState if necessary
1502  // STEP 3: Put the object in the RobotState/world
1503 
1504  if (object.object.operation == moveit_msgs::CollisionObject::ADD ||
1505  object.object.operation == moveit_msgs::CollisionObject::APPEND)
1506  {
1507  const moveit::core::LinkModel* link_model = getRobotModel()->getLinkModel(object.link_name);
1508  if (link_model)
1509  {
1510  // items to build the attached object from (filled from existing world object or message)
1511  Eigen::Isometry3d object_pose_in_link;
1512  std::vector<shapes::ShapeConstPtr> shapes;
1513  EigenSTL::vector_Isometry3d shape_poses;
1514  moveit::core::FixedTransformsMap subframe_poses;
1515 
1516  // STEP 1: Obtain info about object to be attached.
1517  // If it is in the world, message contents are ignored.
1518 
1519  collision_detection::CollisionEnv::ObjectConstPtr obj_in_world = world_->getObject(object.object.id);
1520  if (object.object.operation == moveit_msgs::CollisionObject::ADD && object.object.primitives.empty() &&
1521  object.object.meshes.empty() && object.object.planes.empty())
1522  {
1523  if (obj_in_world)
1524  {
1525  ROS_DEBUG_NAMED(LOGNAME, "Attaching world object '%s' to link '%s'", object.object.id.c_str(),
1526  object.link_name.c_str());
1527  object_pose_in_link = robot_state_->getGlobalLinkTransform(link_model).inverse() * obj_in_world->pose_;
1528  shapes = obj_in_world->shapes_;
1529  shape_poses = obj_in_world->shape_poses_;
1530  subframe_poses = obj_in_world->subframe_poses_;
1531  }
1532  else
1533  {
1535  "Attempting to attach object '%s' to link '%s' but no geometry specified "
1536  "and such an object does not exist in the collision world",
1537  object.object.id.c_str(), object.link_name.c_str());
1538  return false;
1539  }
1540  }
1541  else // If object is not in the world, use the message contents
1542  {
1543  Eigen::Isometry3d header_frame_to_object_pose;
1544  if (!shapesAndPosesFromCollisionObjectMessage(object.object, header_frame_to_object_pose, shapes, shape_poses))
1545  return false;
1546  const Eigen::Isometry3d world_to_header_frame = getFrameTransform(object.object.header.frame_id);
1547  const Eigen::Isometry3d link_to_header_frame =
1548  robot_state_->getGlobalLinkTransform(link_model).inverse() * world_to_header_frame;
1549  object_pose_in_link = link_to_header_frame * header_frame_to_object_pose;
1550 
1551  Eigen::Isometry3d subframe_pose;
1552  for (std::size_t i = 0; i < object.object.subframe_poses.size(); ++i)
1553  {
1554  PlanningScene::poseMsgToEigen(object.object.subframe_poses[i], subframe_pose);
1555  std::string name = object.object.subframe_names[i];
1556  subframe_poses[name] = subframe_pose;
1557  }
1558  }
1559 
1560  if (shapes.empty())
1561  {
1562  ROS_ERROR_NAMED(LOGNAME, "There is no geometry to attach to link '%s' as part of attached body '%s'",
1563  object.link_name.c_str(), object.object.id.c_str());
1564  return false;
1565  }
1566 
1567  if (!object.object.type.db.empty() || !object.object.type.key.empty())
1568  setObjectType(object.object.id, object.object.type);
1569 
1570  // STEP 2: Remove the object from the world (if it existed)
1571  if (obj_in_world && world_->removeObject(object.object.id))
1572  {
1573  if (object.object.operation == moveit_msgs::CollisionObject::ADD)
1574  ROS_DEBUG_NAMED(LOGNAME, "Removing world object with the same name as newly attached object: '%s'",
1575  object.object.id.c_str());
1576  else
1578  "You tried to append geometry to an attached object "
1579  "that is actually a world object ('%s'). World geometry is ignored.",
1580  object.object.id.c_str());
1581  }
1582 
1583  // STEP 3: Attach the object to the robot
1584  if (object.object.operation == moveit_msgs::CollisionObject::ADD ||
1585  !robot_state_->hasAttachedBody(object.object.id))
1586  {
1587  if (robot_state_->clearAttachedBody(object.object.id))
1589  "The robot state already had an object named '%s' attached to link '%s'. "
1590  "The object was replaced.",
1591  object.object.id.c_str(), object.link_name.c_str());
1592 
1593  robot_state_->attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, object.touch_links,
1594  object.link_name, object.detach_posture, subframe_poses);
1595  ROS_DEBUG_NAMED(LOGNAME, "Attached object '%s' to link '%s'", object.object.id.c_str(),
1596  object.link_name.c_str());
1597  }
1598  else // APPEND: augment to existing attached object
1599  {
1600  const moveit::core::AttachedBody* ab = robot_state_->getAttachedBody(object.object.id);
1601 
1602  // Allow overriding the body's pose if provided, otherwise keep the old one
1603  if (moveit::core::isEmpty(object.object.pose))
1604  object_pose_in_link = ab->getPose(); // Keep old pose
1605 
1606  shapes.insert(shapes.end(), ab->getShapes().begin(), ab->getShapes().end());
1607  shape_poses.insert(shape_poses.end(), ab->getShapePoses().begin(), ab->getShapePoses().end());
1608  subframe_poses.insert(ab->getSubframes().begin(), ab->getSubframes().end());
1609  trajectory_msgs::JointTrajectory detach_posture =
1610  object.detach_posture.joint_names.empty() ? ab->getDetachPosture() : object.detach_posture;
1611 
1612  std::set<std::string> touch_links = ab->getTouchLinks();
1613  touch_links.insert(std::make_move_iterator(object.touch_links.begin()),
1614  std::make_move_iterator(object.touch_links.end()));
1615 
1616  robot_state_->clearAttachedBody(object.object.id);
1617  robot_state_->attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, touch_links,
1618  object.link_name, detach_posture, subframe_poses);
1619  ROS_DEBUG_NAMED(LOGNAME, "Appended things to object '%s' attached to link '%s'", object.object.id.c_str(),
1620  object.link_name.c_str());
1621  }
1622  return true;
1623  }
1624  else
1625  ROS_ERROR_NAMED(LOGNAME, "Robot state is not compatible with robot model. This could be fatal.");
1626  }
1627  else if (object.object.operation == moveit_msgs::CollisionObject::REMOVE) // == DETACH
1628  {
1629  // STEP 1: Get info about the object from the RobotState
1630  std::vector<const moveit::core::AttachedBody*> attached_bodies;
1631  if (object.object.id.empty())
1632  {
1633  const moveit::core::LinkModel* link_model =
1634  object.link_name.empty() ? nullptr : getRobotModel()->getLinkModel(object.link_name);
1635  if (link_model) // if we have a link model specified, only fetch bodies attached to this link
1636  robot_state_->getAttachedBodies(attached_bodies, link_model);
1637  else
1638  robot_state_->getAttachedBodies(attached_bodies);
1639  }
1640  else // A specific object id will be removed.
1641  {
1642  const moveit::core::AttachedBody* body = robot_state_->getAttachedBody(object.object.id);
1643  if (body)
1644  {
1645  if (!object.link_name.empty() && (body->getAttachedLinkName() != object.link_name))
1646  {
1647  ROS_ERROR_STREAM_NAMED(LOGNAME, "The AttachedCollisionObject message states the object is attached to "
1648  << object.link_name << ", but it is actually attached to "
1649  << body->getAttachedLinkName()
1650  << ". Leave the link_name empty or specify the correct link.");
1651  return false;
1652  }
1653  attached_bodies.push_back(body);
1654  }
1655  }
1656 
1657  // STEP 2+3: Remove the attached object(s) from the RobotState and put them in the world
1658  for (const moveit::core::AttachedBody* attached_body : attached_bodies)
1659  {
1660  const std::string& name = attached_body->getName();
1661  if (world_->hasObject(name))
1663  "The collision world already has an object with the same name as the body about to be detached. "
1664  "NOT adding the detached body '%s' to the collision world.",
1665  object.object.id.c_str());
1666  else
1667  {
1668  const Eigen::Isometry3d& pose = attached_body->getGlobalPose();
1669  world_->addToObject(name, pose, attached_body->getShapes(), attached_body->getShapePoses());
1670  world_->setSubframesOfObject(name, attached_body->getSubframes());
1671  ROS_DEBUG_NAMED(LOGNAME, "Detached object '%s' from link '%s' and added it back in the collision world",
1672  name.c_str(), object.link_name.c_str());
1673  }
1674 
1675  robot_state_->clearAttachedBody(name);
1676  }
1677  if (!attached_bodies.empty() || object.object.id.empty())
1678  return true;
1679  }
1680  else if (object.object.operation == moveit_msgs::CollisionObject::MOVE)
1681  {
1682  ROS_ERROR_NAMED(LOGNAME, "Move for attached objects not yet implemented");
1683  }
1684  else
1685  {
1686  ROS_ERROR_NAMED(LOGNAME, "Unknown collision object operation: %d", object.object.operation);
1687  }
1688 
1689  return false;
1690 }
1691 
1692 bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject& object)
1693 {
1694  if (object.id == OCTOMAP_NS)
1695  {
1696  ROS_ERROR_NAMED(LOGNAME, "The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str());
1697  return false;
1698  }
1699 
1700  if (object.operation == moveit_msgs::CollisionObject::ADD || object.operation == moveit_msgs::CollisionObject::APPEND)
1701  {
1702  return processCollisionObjectAdd(object);
1703  }
1704  else if (object.operation == moveit_msgs::CollisionObject::REMOVE)
1705  {
1706  return processCollisionObjectRemove(object);
1707  }
1708  else if (object.operation == moveit_msgs::CollisionObject::MOVE)
1709  {
1710  return processCollisionObjectMove(object);
1711  }
1712 
1713  ROS_ERROR_NAMED(LOGNAME, "Unknown collision object operation: %d", object.operation);
1714  return false;
1715 }
1716 
1717 void PlanningScene::poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out)
1718 {
1719  Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z);
1720  Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z);
1721  if ((quaternion.x() == 0) && (quaternion.y() == 0) && (quaternion.z() == 0) && (quaternion.w() == 0))
1722  {
1723  ROS_WARN_NAMED(LOGNAME, "Empty quaternion found in pose message. Setting to neutral orientation.");
1724  quaternion.setIdentity();
1725  }
1726  else
1727  {
1728  quaternion.normalize();
1729  }
1730  out = translation * quaternion;
1731 }
1732 
1733 bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::CollisionObject& object,
1734  Eigen::Isometry3d& object_pose,
1735  std::vector<shapes::ShapeConstPtr>& shapes,
1736  EigenSTL::vector_Isometry3d& shape_poses)
1737 {
1738  if (object.primitives.size() < object.primitive_poses.size())
1739  {
1740  ROS_ERROR_NAMED(LOGNAME, "More primitive shape poses than shapes in collision object message.");
1741  return false;
1742  }
1743  if (object.meshes.size() < object.mesh_poses.size())
1744  {
1745  ROS_ERROR_NAMED(LOGNAME, "More mesh poses than meshes in collision object message.");
1746  return false;
1747  }
1748  if (object.planes.size() < object.plane_poses.size())
1749  {
1750  ROS_ERROR_NAMED(LOGNAME, "More plane poses than planes in collision object message.");
1751  return false;
1752  }
1753 
1754  const int num_shapes = object.primitives.size() + object.meshes.size() + object.planes.size();
1755  shapes.reserve(num_shapes);
1756  shape_poses.reserve(num_shapes);
1757 
1758  bool switch_object_pose_and_shape_pose = false;
1759  if (num_shapes == 1 && moveit::core::isEmpty(object.pose))
1760  {
1761  // If the object pose is not set but the shape pose is, use the shape's pose as the object pose.
1762  switch_object_pose_and_shape_pose = true;
1763  object_pose.setIdentity();
1764  }
1765  else
1766  PlanningScene::poseMsgToEigen(object.pose, object_pose);
1767 
1768  auto append = [&object_pose, &shapes, &shape_poses,
1769  &switch_object_pose_and_shape_pose](shapes::Shape* s, const geometry_msgs::Pose& pose_msg) {
1770  if (!s)
1771  return;
1772  Eigen::Isometry3d pose;
1773  PlanningScene::poseMsgToEigen(pose_msg, pose);
1774  if (!switch_object_pose_and_shape_pose)
1775  shape_poses.emplace_back(std::move(pose));
1776  else
1777  {
1778  shape_poses.emplace_back(std::move(object_pose));
1779  object_pose = pose;
1780  }
1781  shapes.emplace_back(shapes::ShapeConstPtr(s));
1782  };
1783 
1784  auto treat_shape_vectors = [&append](const auto& shape_vector, // the shape_msgs of each type
1785  const auto& shape_poses_vector, // std::vector<const geometry_msgs::Pose>
1786  const std::string& shape_type) {
1787  if (shape_vector.size() > shape_poses_vector.size())
1788  {
1789  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Number of " << shape_type
1790  << " does not match number of poses "
1791  "in collision object message. Assuming identity.");
1792  for (std::size_t i = 0; i < shape_vector.size(); ++i)
1793  {
1794  if (i >= shape_poses_vector.size())
1795  {
1796  // Empty shape pose => Identity
1797  geometry_msgs::Pose identity;
1798  identity.orientation.w = 1.0;
1799  append(shapes::constructShapeFromMsg(shape_vector[i]), identity);
1800  }
1801  else
1802  append(shapes::constructShapeFromMsg(shape_vector[i]), shape_poses_vector[i]);
1803  }
1804  }
1805  else
1806  for (std::size_t i = 0; i < shape_vector.size(); ++i)
1807  append(shapes::constructShapeFromMsg(shape_vector[i]), shape_poses_vector[i]);
1808  };
1809 
1810  treat_shape_vectors(object.primitives, object.primitive_poses, std::string("primitive_poses"));
1811  treat_shape_vectors(object.meshes, object.mesh_poses, std::string("meshes"));
1812  treat_shape_vectors(object.planes, object.plane_poses, std::string("planes"));
1813  return true;
1814 }
1815 
1816 bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject& object)
1817 {
1818  if (!knowsFrameTransform(object.header.frame_id))
1819  {
1820  ROS_ERROR_STREAM_NAMED(LOGNAME, "Unknown frame: " << object.header.frame_id);
1821  return false;
1822  }
1823 
1824  if (object.primitives.empty() && object.meshes.empty() && object.planes.empty())
1825  {
1826  ROS_ERROR_NAMED(LOGNAME, "There are no shapes specified in the collision object message");
1827  return false;
1828  }
1829 
1830  // replace the object if ADD is specified instead of APPEND
1831  if (object.operation == moveit_msgs::CollisionObject::ADD && world_->hasObject(object.id))
1832  world_->removeObject(object.id);
1833 
1834  const Eigen::Isometry3d& world_to_object_header_transform = getFrameTransform(object.header.frame_id);
1835  Eigen::Isometry3d header_to_pose_transform;
1836  std::vector<shapes::ShapeConstPtr> shapes;
1837  EigenSTL::vector_Isometry3d shape_poses;
1838  if (!shapesAndPosesFromCollisionObjectMessage(object, header_to_pose_transform, shapes, shape_poses))
1839  return false;
1840  const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform;
1841 
1842  world_->addToObject(object.id, object_frame_transform, shapes, shape_poses);
1843 
1844  if (!object.type.key.empty() || !object.type.db.empty())
1845  setObjectType(object.id, object.type);
1846 
1847  // Add subframes
1849  Eigen::Isometry3d subframe_pose;
1850  for (std::size_t i = 0; i < object.subframe_poses.size(); ++i)
1851  {
1852  PlanningScene::poseMsgToEigen(object.subframe_poses[i], subframe_pose);
1853  std::string name = object.subframe_names[i];
1854  subframes[name] = subframe_pose;
1855  }
1856  world_->setSubframesOfObject(object.id, subframes);
1857  return true;
1858 }
1859 
1860 bool PlanningScene::processCollisionObjectRemove(const moveit_msgs::CollisionObject& object)
1861 {
1862  if (object.id.empty())
1863  {
1865  }
1866  else
1867  {
1868  if (!world_->removeObject(object.id))
1869  {
1871  "Tried to remove world object '" << object.id << "', but it does not exist in this scene.");
1872  return false;
1873  }
1874 
1875  removeObjectColor(object.id);
1876  removeObjectType(object.id);
1877  }
1878  return true;
1879 }
1880 
1881 bool PlanningScene::processCollisionObjectMove(const moveit_msgs::CollisionObject& object)
1882 {
1883  if (world_->hasObject(object.id))
1884  {
1885  if (!object.primitives.empty() || !object.meshes.empty() || !object.planes.empty())
1886  ROS_WARN_NAMED(LOGNAME, "Move operation for object '%s' ignores the geometry specified in the message.",
1887  object.id.c_str());
1888 
1889  const Eigen::Isometry3d& world_to_object_header_transform = getFrameTransform(object.header.frame_id);
1890  Eigen::Isometry3d header_to_pose_transform;
1891 
1892  PlanningScene::poseMsgToEigen(object.pose, header_to_pose_transform);
1893 
1894  const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform;
1895  world_->setObjectPose(object.id, object_frame_transform);
1896  return true;
1897  }
1898 
1899  ROS_ERROR_NAMED(LOGNAME, "World object '%s' does not exist. Cannot move.", object.id.c_str());
1900  return false;
1901 }
1902 
1903 const Eigen::Isometry3d& PlanningScene::getFrameTransform(const std::string& frame_id) const
1904 {
1905  return getFrameTransform(getCurrentState(), frame_id);
1906 }
1907 
1908 const Eigen::Isometry3d& PlanningScene::getFrameTransform(const std::string& frame_id)
1909 {
1910  if (getCurrentState().dirtyLinkTransforms())
1911  return getFrameTransform(getCurrentStateNonConst(), frame_id);
1912  else
1913  return getFrameTransform(getCurrentState(), frame_id);
1914 }
1915 
1916 const Eigen::Isometry3d& PlanningScene::getFrameTransform(const moveit::core::RobotState& state,
1917  const std::string& frame_id) const
1918 {
1919  if (!frame_id.empty() && frame_id[0] == '/')
1920  // Recursively call itself without the slash in front of frame name
1921  return getFrameTransform(frame_id.substr(1));
1922 
1923  bool frame_found;
1924  const Eigen::Isometry3d& t1 = state.getFrameTransform(frame_id, &frame_found);
1925  if (frame_found)
1926  return t1;
1927 
1928  const Eigen::Isometry3d& t2 = getWorld()->getTransform(frame_id, frame_found);
1929  if (frame_found)
1930  return t2;
1931  return getTransforms().Transforms::getTransform(frame_id);
1932 }
1933 
1934 bool PlanningScene::knowsFrameTransform(const std::string& frame_id) const
1936  return knowsFrameTransform(getCurrentState(), frame_id);
1937 }
1938 
1939 bool PlanningScene::knowsFrameTransform(const moveit::core::RobotState& state, const std::string& frame_id) const
1941  if (!frame_id.empty() && frame_id[0] == '/')
1942  return knowsFrameTransform(frame_id.substr(1));
1943 
1944  if (state.knowsFrameTransform(frame_id))
1945  return true;
1946  if (getWorld()->knowsTransform(frame_id))
1947  return true;
1948  return getTransforms().Transforms::canTransform(frame_id);
1949 }
1950 
1951 bool PlanningScene::hasObjectType(const std::string& object_id) const
1952 {
1953  if (object_types_)
1954  if (object_types_->find(object_id) != object_types_->end())
1955  return true;
1956  if (parent_)
1957  return parent_->hasObjectType(object_id);
1958  return false;
1959 }
1960 
1961 const object_recognition_msgs::ObjectType& PlanningScene::getObjectType(const std::string& object_id) const
1962 {
1963  if (object_types_)
1964  {
1965  ObjectTypeMap::const_iterator it = object_types_->find(object_id);
1966  if (it != object_types_->end())
1967  return it->second;
1968  }
1969  if (parent_)
1970  return parent_->getObjectType(object_id);
1971  static const object_recognition_msgs::ObjectType EMPTY;
1972  return EMPTY;
1973 }
1974 
1975 void PlanningScene::setObjectType(const std::string& object_id, const object_recognition_msgs::ObjectType& type)
1976 {
1977  if (!object_types_)
1978  object_types_ = std::make_unique<ObjectTypeMap>();
1979  (*object_types_)[object_id] = type;
1980 }
1981 
1982 void PlanningScene::removeObjectType(const std::string& object_id)
1984  if (object_types_)
1985  object_types_->erase(object_id);
1986 }
1987 
1989 {
1990  kc.clear();
1991  if (parent_)
1992  parent_->getKnownObjectTypes(kc);
1994  for (ObjectTypeMap::const_iterator it = object_types_->begin(); it != object_types_->end(); ++it)
1995  kc[it->first] = it->second;
1996 }
1997 
1998 bool PlanningScene::hasObjectColor(const std::string& object_id) const
1999 {
2000  if (object_colors_)
2001  if (object_colors_->find(object_id) != object_colors_->end())
2002  return true;
2003  if (parent_)
2004  return parent_->hasObjectColor(object_id);
2005  return false;
2006 }
2008 const std_msgs::ColorRGBA& PlanningScene::getObjectColor(const std::string& object_id) const
2009 {
2010  if (object_colors_)
2011  {
2012  ObjectColorMap::const_iterator it = object_colors_->find(object_id);
2013  if (it != object_colors_->end())
2014  return it->second;
2015  }
2016  if (parent_)
2017  return parent_->getObjectColor(object_id);
2018  static const std_msgs::ColorRGBA EMPTY;
2019  return EMPTY;
2021 
2023 {
2024  kc.clear();
2025  if (parent_)
2026  parent_->getKnownObjectColors(kc);
2027  if (object_colors_)
2028  for (ObjectColorMap::const_iterator it = object_colors_->begin(); it != object_colors_->end(); ++it)
2029  kc[it->first] = it->second;
2031 
2032 void PlanningScene::setObjectColor(const std::string& object_id, const std_msgs::ColorRGBA& color)
2033 {
2034  if (object_id.empty())
2035  {
2036  ROS_ERROR_NAMED(LOGNAME, "Cannot set color of object with empty object_id.");
2037  return;
2038  }
2039  if (!object_colors_)
2040  object_colors_ = std::make_unique<ObjectColorMap>();
2041  (*object_colors_)[object_id] = color;
2042 }
2043 
2044 void PlanningScene::removeObjectColor(const std::string& object_id)
2045 {
2046  if (object_colors_)
2047  object_colors_->erase(object_id);
2048 }
2049 
2050 bool PlanningScene::isStateColliding(const moveit_msgs::RobotState& state, const std::string& group, bool verbose) const
2051 {
2054  return isStateColliding(s, group, verbose);
2055 }
2056 
2057 bool PlanningScene::isStateColliding(const std::string& group, bool verbose)
2058 {
2059  if (getCurrentState().dirtyCollisionBodyTransforms())
2060  return isStateColliding(getCurrentStateNonConst(), group, verbose);
2061  else
2062  return isStateColliding(getCurrentState(), group, verbose);
2063 }
2065 bool PlanningScene::isStateColliding(const moveit::core::RobotState& state, const std::string& group, bool verbose) const
2066 {
2068  req.verbose = verbose;
2069  req.group_name = group;
2071  checkCollision(req, res, state);
2072  return res.collision;
2073 }
2074 
2075 bool PlanningScene::isStateFeasible(const moveit_msgs::RobotState& state, bool verbose) const
2077  if (state_feasibility_)
2078  {
2081  return state_feasibility_(s, verbose);
2082  }
2083  return true;
2084 }
2085 
2086 bool PlanningScene::isStateFeasible(const moveit::core::RobotState& state, bool verbose) const
2087 {
2088  if (state_feasibility_)
2089  return state_feasibility_(state, verbose);
2090  return true;
2091 }
2092 
2093 bool PlanningScene::isStateConstrained(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
2094  bool verbose) const
2095 {
2098  return isStateConstrained(s, constr, verbose);
2099 }
2100 
2101 bool PlanningScene::isStateConstrained(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr,
2102  bool verbose) const
2103 {
2104  kinematic_constraints::KinematicConstraintSetPtr ks(
2106  ks->add(constr, getTransforms());
2107  if (ks->empty())
2108  return true;
2109  else
2110  return isStateConstrained(state, *ks, verbose);
2111 }
2112 
2113 bool PlanningScene::isStateConstrained(const moveit_msgs::RobotState& state,
2114  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose) const
2115 {
2118  return isStateConstrained(s, constr, verbose);
2119 }
2120 
2122  const kinematic_constraints::KinematicConstraintSet& constr, bool verbose) const
2123 {
2124  return constr.decide(state, verbose).satisfied;
2126 
2127 bool PlanningScene::isStateValid(const moveit::core::RobotState& state, const std::string& group, bool verbose) const
2128 {
2129  static const moveit_msgs::Constraints EMP_CONSTRAINTS;
2130  return isStateValid(state, EMP_CONSTRAINTS, group, verbose);
2131 }
2132 
2133 bool PlanningScene::isStateValid(const moveit_msgs::RobotState& state, const std::string& group, bool verbose) const
2134 {
2135  static const moveit_msgs::Constraints EMP_CONSTRAINTS;
2136  return isStateValid(state, EMP_CONSTRAINTS, group, verbose);
2137 }
2138 
2139 bool PlanningScene::isStateValid(const moveit_msgs::RobotState& state, const moveit_msgs::Constraints& constr,
2140  const std::string& group, bool verbose) const
2141 {
2144  return isStateValid(s, constr, group, verbose);
2146 
2147 bool PlanningScene::isStateValid(const moveit::core::RobotState& state, const moveit_msgs::Constraints& constr,
2148  const std::string& group, bool verbose) const
2149 {
2150  if (isStateColliding(state, group, verbose))
2151  return false;
2152  if (!isStateFeasible(state, verbose))
2153  return false;
2154  return isStateConstrained(state, constr, verbose);
2155 }
2156 
2158  const kinematic_constraints::KinematicConstraintSet& constr, const std::string& group,
2159  bool verbose) const
2160 {
2161  if (isStateColliding(state, group, verbose))
2162  return false;
2163  if (!isStateFeasible(state, verbose))
2164  return false;
2165  return isStateConstrained(state, constr, verbose);
2166 }
2167 
2168 bool PlanningScene::isPathValid(const moveit_msgs::RobotState& start_state,
2169  const moveit_msgs::RobotTrajectory& trajectory, const std::string& group, bool verbose,
2170  std::vector<std::size_t>* invalid_index) const
2172  static const moveit_msgs::Constraints EMP_CONSTRAINTS;
2173  static const std::vector<moveit_msgs::Constraints> EMP_CONSTRAINTS_VECTOR;
2174  return isPathValid(start_state, trajectory, EMP_CONSTRAINTS, EMP_CONSTRAINTS_VECTOR, group, verbose, invalid_index);
2175 }
2176 
2177 bool PlanningScene::isPathValid(const moveit_msgs::RobotState& start_state,
2178  const moveit_msgs::RobotTrajectory& trajectory,
2179  const moveit_msgs::Constraints& path_constraints, const std::string& group,
2180  bool verbose, std::vector<std::size_t>* invalid_index) const
2181 {
2182  static const std::vector<moveit_msgs::Constraints> EMP_CONSTRAINTS_VECTOR;
2183  return isPathValid(start_state, trajectory, path_constraints, EMP_CONSTRAINTS_VECTOR, group, verbose, invalid_index);
2184 }
2185 
2186 bool PlanningScene::isPathValid(const moveit_msgs::RobotState& start_state,
2187  const moveit_msgs::RobotTrajectory& trajectory,
2188  const moveit_msgs::Constraints& path_constraints,
2189  const moveit_msgs::Constraints& goal_constraints, const std::string& group,
2190  bool verbose, std::vector<std::size_t>* invalid_index) const
2191 {
2192  std::vector<moveit_msgs::Constraints> goal_constraints_vector(1, goal_constraints);
2193  return isPathValid(start_state, trajectory, path_constraints, goal_constraints_vector, group, verbose, invalid_index);
2194 }
2195 
2196 bool PlanningScene::isPathValid(const moveit_msgs::RobotState& start_state,
2197  const moveit_msgs::RobotTrajectory& trajectory,
2198  const moveit_msgs::Constraints& path_constraints,
2199  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group,
2200  bool verbose, std::vector<std::size_t>* invalid_index) const
2201 {
2205  t.setRobotTrajectoryMsg(start, trajectory);
2206  return isPathValid(t, path_constraints, goal_constraints, group, verbose, invalid_index);
2207 }
2208 
2210  const moveit_msgs::Constraints& path_constraints,
2211  const std::vector<moveit_msgs::Constraints>& goal_constraints, const std::string& group,
2212  bool verbose, std::vector<std::size_t>* invalid_index) const
2213 {
2214  bool result = true;
2215  if (invalid_index)
2216  invalid_index->clear();
2218  ks_p.add(path_constraints, getTransforms());
2219  std::size_t n_wp = trajectory.getWayPointCount();
2220  for (std::size_t i = 0; i < n_wp; ++i)
2221  {
2222  const moveit::core::RobotState& st = trajectory.getWayPoint(i);
2223 
2224  bool this_state_valid = true;
2225  if (isStateColliding(st, group, verbose))
2226  this_state_valid = false;
2227  if (!isStateFeasible(st, verbose))
2228  this_state_valid = false;
2229  if (!ks_p.empty() && !ks_p.decide(st, verbose).satisfied)
2230  this_state_valid = false;
2231 
2232  if (!this_state_valid)
2233  {
2234  if (invalid_index)
2235  invalid_index->push_back(i);
2236  else
2237  return false;
2238  result = false;
2239  }
2240 
2241  // check goal for last state
2242  if (i + 1 == n_wp && !goal_constraints.empty())
2243  {
2244  bool found = false;
2245  for (const moveit_msgs::Constraints& goal_constraint : goal_constraints)
2246  {
2247  if (isStateConstrained(st, goal_constraint))
2248  {
2249  found = true;
2250  break;
2251  }
2252  }
2253  if (!found)
2254  {
2255  if (verbose)
2256  ROS_INFO_NAMED(LOGNAME, "Goal not satisfied");
2257  if (invalid_index)
2258  invalid_index->push_back(i);
2259  result = false;
2260  }
2261  }
2262  }
2263  return result;
2264 }
2265 
2267  const moveit_msgs::Constraints& path_constraints,
2268  const moveit_msgs::Constraints& goal_constraints, const std::string& group,
2269  bool verbose, std::vector<std::size_t>* invalid_index) const
2270 {
2271  std::vector<moveit_msgs::Constraints> goal_constraints_vector(1, goal_constraints);
2272  return isPathValid(trajectory, path_constraints, goal_constraints_vector, group, verbose, invalid_index);
2273 }
2274 
2276  const moveit_msgs::Constraints& path_constraints, const std::string& group,
2277  bool verbose, std::vector<std::size_t>* invalid_index) const
2278 {
2279  static const std::vector<moveit_msgs::Constraints> EMP_CONSTRAINTS_VECTOR;
2280  return isPathValid(trajectory, path_constraints, EMP_CONSTRAINTS_VECTOR, group, verbose, invalid_index);
2281 }
2282 
2283 bool PlanningScene::isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group,
2284  bool verbose, std::vector<std::size_t>* invalid_index) const
2285 {
2286  static const moveit_msgs::Constraints EMP_CONSTRAINTS;
2287  static const std::vector<moveit_msgs::Constraints> EMP_CONSTRAINTS_VECTOR;
2288  return isPathValid(trajectory, EMP_CONSTRAINTS, EMP_CONSTRAINTS_VECTOR, group, verbose, invalid_index);
2289 }
2290 
2291 void PlanningScene::getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
2292  std::set<collision_detection::CostSource>& costs, double overlap_fraction) const
2293 {
2294  getCostSources(trajectory, max_costs, std::string(), costs, overlap_fraction);
2295 }
2296 
2297 void PlanningScene::getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs,
2298  const std::string& group_name, std::set<collision_detection::CostSource>& costs,
2299  double overlap_fraction) const
2300 {
2302  creq.max_cost_sources = max_costs;
2303  creq.group_name = group_name;
2304  creq.cost = true;
2305  std::set<collision_detection::CostSource> cs;
2306  std::set<collision_detection::CostSource> cs_start;
2307  std::size_t n_wp = trajectory.getWayPointCount();
2308  for (std::size_t i = 0; i < n_wp; ++i)
2309  {
2311  checkCollision(creq, cres, trajectory.getWayPoint(i));
2312  cs.insert(cres.cost_sources.begin(), cres.cost_sources.end());
2313  if (i == 0)
2314  cs_start.swap(cres.cost_sources);
2315  }
2316 
2317  if (cs.size() <= max_costs)
2318  costs.swap(cs);
2319  else
2320  {
2321  costs.clear();
2322  std::size_t i = 0;
2323  for (std::set<collision_detection::CostSource>::iterator it = cs.begin(); i < max_costs; ++it, ++i)
2324  costs.insert(*it);
2325  }
2326 
2327  collision_detection::removeCostSources(costs, cs_start, overlap_fraction);
2328  collision_detection::removeOverlapping(costs, overlap_fraction);
2330 
2331 void PlanningScene::getCostSources(const moveit::core::RobotState& state, std::size_t max_costs,
2332  std::set<collision_detection::CostSource>& costs) const
2333 {
2334  getCostSources(state, max_costs, std::string(), costs);
2335 }
2336 
2337 void PlanningScene::getCostSources(const moveit::core::RobotState& state, std::size_t max_costs,
2338  const std::string& group_name,
2339  std::set<collision_detection::CostSource>& costs) const
2340 {
2342  creq.max_cost_sources = max_costs;
2343  creq.group_name = group_name;
2344  creq.cost = true;
2346  checkCollision(creq, cres, state);
2347  cres.cost_sources.swap(costs);
2348 }
2349 
2350 void PlanningScene::printKnownObjects(std::ostream& out) const
2351 {
2352  const std::vector<std::string>& objects = getWorld()->getObjectIds();
2353  std::vector<const moveit::core::AttachedBody*> attached_bodies;
2354  getCurrentState().getAttachedBodies(attached_bodies);
2355 
2356  // Output
2357  out << "-----------------------------------------\n";
2358  out << "PlanningScene Known Objects:\n";
2359  out << " - Collision World Objects:\n ";
2360  for (const std::string& object : objects)
2361  {
2362  out << "\t- " << object << "\n";
2363  }
2364 
2365  out << " - Attached Bodies:\n";
2366  for (const moveit::core::AttachedBody* attached_body : attached_bodies)
2367  {
2368  out << "\t- " << attached_body->getName() << "\n";
2369  }
2370  out << "-----------------------------------------\n";
2371 }
2372 
2373 } // end of namespace planning_scene
planning_scene::PlanningScene::getAllowedCollisionMatrixNonConst
collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrixNonConst()
Get the allowed collision matrix.
Definition: planning_scene.cpp:643
planning_scene::PlanningScene::getCurrentState
const moveit::core::RobotState & getCurrentState() const
Get the state at which the robot is assumed to be.
Definition: planning_scene.h:261
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
collision_detection::World::ADD_SHAPE
@ ADD_SHAPE
Definition: world.h:258
occupancy_map.h
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
collision_detection::CollisionRequest::cost
bool cost
If true, a collision cost is computed.
Definition: include/moveit/collision_detection/collision_common.h:206
planning_scene::PlanningScene::isEmpty
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
Check if a message includes any information about a planning scene, or it is just a default,...
Definition: planning_scene.cpp:131
collision_detection::CollisionResult::ContactMap
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
Definition: include/moveit/collision_detection/collision_common.h:387
collision_detection::Contact::body_type_1
BodyType body_type_1
The type of the first body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:122
planning_scene::PlanningScene::printKnownObjects
void printKnownObjects(std::ostream &out=std::cout) const
Outputs debug information about the planning scene contents.
Definition: planning_scene.cpp:2382
planning_scene::PlanningScene::world_diff_
collision_detection::WorldDiffPtr world_diff_
Definition: planning_scene.h:1163
collision_detection::World::DESTROY
@ DESTROY
Definition: world.h:256
moveit::core::AttachedBody::getDetachPosture
const trajectory_msgs::JointTrajectory & getDetachPosture() const
Return the posture that is necessary for the object to be released, (if any). This is useful for exam...
Definition: attached_body.h:188
robot_trajectory::RobotTrajectory::getWayPointCount
std::size_t getWayPointCount() const
Definition: robot_trajectory.h:131
shapes
initialize
bool initialize(MeshCollisionTraversalNode< BV > &node, BVHModel< BV > &model1, Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, Transform3< typename BV::S > &tf2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, bool use_refit, bool refit_bottomup)
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
exceptions.h
shapes::OcTree::octree
std::shared_ptr< const octomap::OcTree > octree
octomap_msgs::fullMapToMsg
static bool fullMapToMsg(const OctomapT &octomap, Octomap &msg)
planning_scene::PlanningScene::PlanningScene
PlanningScene(const moveit::core::RobotModelConstPtr &robot_model, const collision_detection::WorldPtr &world=std::make_shared< collision_detection::World >())
construct using an existing RobotModel
Definition: planning_scene.cpp:146
planning_scene::PlanningScene::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Get the kinematic model for which the planning scene is maintained.
Definition: planning_scene.h:254
planning_scene::PlanningScene::getObjectColorMsgs
void getObjectColorMsgs(std::vector< moveit_msgs::ObjectColor > &object_colors) const
Construct a vector of messages (object_colors) with the colors of the objects from the planning_scene...
Definition: planning_scene.cpp:897
planning_scene::PlanningScene::getWorld
const collision_detection::WorldConstPtr & getWorld() const
Get the representation of the world.
Definition: planning_scene.h:397
planning_scene::PlanningScene::hasObjectColor
bool hasObjectColor(const std::string &id) const
Definition: planning_scene.cpp:2030
planning_scene::PlanningScene::current_world_object_update_callback_
collision_detection::World::ObserverCallbackFn current_world_object_update_callback_
Definition: planning_scene.h:1164
planning_scene::PlanningScene::getCollisionObjectMsgs
void getCollisionObjectMsgs(std::vector< moveit_msgs::CollisionObject > &collision_objs) const
Construct a vector of messages (collision_objects) with the collision object data for all objects in ...
Definition: planning_scene.cpp:840
planning_scene::PlanningScene::initialize
void initialize()
Definition: planning_scene.cpp:176
planning_scene::PlanningScene::processAttachedCollisionObjectMsg
bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &object)
Definition: planning_scene.cpp:1510
tf2_eigen.h
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
s
XmlRpcServer s
moveit::core::RobotState::knowsFrameTransform
bool knowsFrameTransform(const std::string &frame_id) const
Check if a transformation matrix from the model frame (root of model) to frame frame_id is known.
Definition: robot_state.cpp:1259
planning_scene::PlanningScene::pushDiffs
void pushDiffs(const PlanningScenePtr &scene)
If there is a parent specified for this scene, then the diffs with respect to that parent are applied...
Definition: planning_scene.cpp:437
planning_scene::PlanningScene::getTransforms
const moveit::core::Transforms & getTransforms() const
Get the set of fixed transforms from known frames to the planning frame.
Definition: planning_scene.h:285
obj_
moveit_msgs::CollisionObject * obj_
Definition: planning_scene.cpp:798
planning_scene::PlanningScene::isStateConstrained
bool isStateConstrained(const moveit_msgs::RobotState &state, const moveit_msgs::Constraints &constr, bool verbose=false) const
Check if a given state satisfies a set of constraints.
Definition: planning_scene.cpp:2125
planning_scene::PlanningScene::getAllowedCollisionMatrix
const collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrix() const
Get the allowed collision matrix.
Definition: planning_scene.h:442
collision_detection::removeCostSources
void removeCostSources(std::set< CostSource > &cost_sources, const std::set< CostSource > &cost_sources_to_remove, double overlap_fraction)
Definition: collision_tools.cpp:240
moveit::core::RobotModel
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:143
planning_scene::LOGNAME
const std::string LOGNAME
Definition: planning_scene.cpp:90
kinematic_constraints::KinematicConstraintSet::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result.
Definition: kinematic_constraint.cpp:1284
planning_scene::PlanningScene::getObjectType
const object_recognition_msgs::ObjectType & getObjectType(const std::string &id) const
Definition: planning_scene.cpp:1993
planning_scene::PlanningScene::getCostSources
void getCostSources(const robot_trajectory::RobotTrajectory &trajectory, std::size_t max_costs, std::set< collision_detection::CostSource > &costs, double overlap_fraction=0.9) const
Get the top max_costs cost sources for a specified trajectory. The resulting costs are stored in cost...
Definition: planning_scene.cpp:2323
moveit::core::AttachedBody::getTouchLinks
const std::set< std::string > & getTouchLinks() const
Get the links that the attached body is allowed to touch.
Definition: attached_body.h:181
attached_body.h
planning_scene.h
planning_scene::PlanningScene::CollisionDetector::copyPadding
void copyPadding(const CollisionDetector &src)
Definition: planning_scene.cpp:258
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
collision_detector_allocator_fcl.h
shape_operations.h
planning_scene::PlanningScene::writePoseToText
void writePoseToText(std::ostream &out, const Eigen::Isometry3d &pose) const
Definition: planning_scene.cpp:1192
planning_scene::PlanningScene::parent_
PlanningSceneConstPtr parent_
Definition: planning_scene.h:1148
planning_scene::PlanningScene::decoupleParent
void decoupleParent()
Make sure that all the data maintained in this scene is local. All unmodified data is copied from the...
Definition: planning_scene.cpp:1237
boost
planning_scene::PlanningScene::setCurrentState
void setCurrentState(const moveit_msgs::RobotState &state)
Set the current robot state to be state. If not all joint values are specified, the previously mainta...
Definition: planning_scene.cpp:1199
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
planning_scene::PlanningScene::getCollisionDetectorNames
void getCollisionDetectorNames(std::vector< std::string > &names) const
get the types of collision detector that have already been added. These are the types which can be pa...
Definition: planning_scene.cpp:359
planning_scene::PlanningScene::getObjectColor
const std_msgs::ColorRGBA & getObjectColor(const std::string &id) const
Definition: planning_scene.cpp:2040
planning_scene::SceneTransforms::canTransform
bool canTransform(const std::string &from_frame) const override
Check whether data can be transformed from a particular frame.
Definition: planning_scene.cpp:99
kinematic_constraints::ConstraintEvaluationResult::satisfied
bool satisfied
Whether or not the constraint or constraints were satisfied.
Definition: kinematic_constraint.h:134
shapes::Shape
planning_scene::PlanningScene::getCurrentStateUpdated
moveit::core::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState &update) const
Get a copy of the current state with components overwritten by the state message update.
Definition: planning_scene.cpp:620
collision_detection::Contact
Definition of a contact point.
Definition: include/moveit/collision_detection/collision_common.h:105
planning_scene::PlanningScene::active_collision_
CollisionDetectorPtr active_collision_
Definition: planning_scene.h:1168
collision_detection::AllowedCollisionMatrix::getMessage
void getMessage(moveit_msgs::AllowedCollisionMatrix &msg) const
Get the allowed collision matrix as a message.
Definition: collision_matrix.cpp:385
robot_trajectory::RobotTrajectory
Maintain a sequence of waypoints and the time durations between these waypoints.
Definition: robot_trajectory.h:84
planning_scene::PlanningScene::propogateRobotPadding
void propogateRobotPadding()
Copy scale and padding from active CollisionRobot to other CollisionRobots. This should be called aft...
Definition: planning_scene.cpp:264
planning_scene::ObjectTypeMap
std::map< std::string, object_recognition_msgs::ObjectType > ObjectTypeMap
A map from object names (e.g., attached bodies, collision objects) to their types.
Definition: planning_scene.h:197
planning_scene::PlanningScene::processOctomapPtr
void processOctomapPtr(const std::shared_ptr< const octomap::OcTree > &octree, const Eigen::Isometry3d &t)
Definition: planning_scene.cpp:1477
conversions.h
obj
CollisionObject< S > * obj
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:179
collision_detection::removeOverlapping
void removeOverlapping(std::set< CostSource > &cost_sources, double overlap_fraction)
Definition: collision_tools.cpp:210
planning_scene::PlanningScene::getCurrentStateNonConst
moveit::core::RobotState & getCurrentStateNonConst()
Get the state at which the robot is assumed to be.
Definition: planning_scene.cpp:609
collision_tools.h
planning_scene::PlanningScene::getCollisionObjectMsg
bool getCollisionObjectMsg(moveit_msgs::CollisionObject &collision_obj, const std::string &ns) const
Construct a message (collision_object) with the collision object data from the planning_scene for the...
Definition: planning_scene.cpp:803
planning_scene::PlanningScene::clearDiffs
void clearDiffs()
Clear the diffs accumulated for this planning scene, with respect to: the parent PlanningScene (if it...
Definition: planning_scene.cpp:397
planning_scene::PlanningScene::collision_
std::map< std::string, CollisionDetectorPtr > collision_
Definition: planning_scene.h:1167
planning_scene::PlanningScene::CollisionDetectorIterator
std::map< std::string, CollisionDetectorPtr >::iterator CollisionDetectorIterator
Definition: planning_scene.h:1140
planning_scene::PlanningScene::getPlanningSceneMsg
void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene) const
Construct a message (scene) with all the necessary data so that the scene can be later reconstructed ...
Definition: planning_scene.cpp:912
collision_detection::Contact::body_name_2
std::string body_name_2
The id of the second body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:125
collision_detection::Contact::body_type_2
BodyType body_type_2
The type of the second body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:128
planning_scene::PlanningScene::CollisionDetectorConstIterator
std::map< std::string, CollisionDetectorPtr >::const_iterator CollisionDetectorConstIterator
Definition: planning_scene.h:1141
planning_scene::PlanningScene::isStateValid
bool isStateValid(const moveit_msgs::RobotState &state, const std::string &group="", bool verbose=false) const
Check if a given state is valid. This means checking for collisions and feasibility.
Definition: planning_scene.cpp:2165
moveit::core::AttachedBody
Object defining bodies that can be attached to robot links.
Definition: attached_body.h:121
planning_scene::PlanningScene::getCollidingPairs
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts)
Get the names of the links that are involved in collisions for the current state. Update the link tra...
Definition: planning_scene.cpp:556
planning_scene::PlanningScene::getAttachedCollisionObjectMsgs
void getAttachedCollisionObjectMsgs(std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objs) const
Construct a vector of messages (attached_collision_objects) with the attached collision object data f...
Definition: planning_scene.cpp:868
collision_detection::Contact::body_name_1
std::string body_name_1
The id of the first body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:119
planning_scene::SceneTransforms::scene_
const PlanningScene * scene_
Definition: planning_scene.cpp:128
planning_scene::PlanningScene::current_world_object_update_observer_handle_
collision_detection::World::ObserverHandle current_world_object_update_observer_handle_
Definition: planning_scene.h:1165
planning_scene::PlanningScene::DEFAULT_SCENE_NAME
static const MOVEIT_PLANNING_SCENE_EXPORT std::string DEFAULT_SCENE_NAME
Definition: planning_scene.h:215
planning_scene::PlanningScene::knowsFrameTransform
bool knowsFrameTransform(const std::string &id) const
Check if a transform to the frame id is known. This will be known if id is a link name,...
Definition: planning_scene.cpp:1966
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix.
Definition: collision_matrix.h:112
planning_scene::PlanningScene::hasObjectType
bool hasObjectType(const std::string &id) const
Definition: planning_scene.cpp:1983
planning_scene::PlanningScene::getKnownObjectColors
void getKnownObjectColors(ObjectColorMap &kc) const
Definition: planning_scene.cpp:2054
planning_scene::PlanningScene::robot_state_
moveit::core::RobotStatePtr robot_state_
Definition: planning_scene.h:1152
planning_scene::PlanningScene
This class maintains the representation of the environment as seen by a planning instance....
Definition: planning_scene.h:202
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:729
planning_scene::PlanningScene::isStateColliding
bool isStateColliding(const std::string &group="", bool verbose=false)
Check if the current state is in collision (with the environment or self collision)....
Definition: planning_scene.cpp:2089
name
std::string name
planning_scene::PlanningScene::addCollisionDetector
void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr &allocator)
Add a new collision detector type.
Definition: planning_scene.cpp:283
planning_scene::createOctomap
collision_detection::OccMapTreePtr createOctomap(const octomap_msgs::Octomap &map)
Definition: planning_scene.cpp:1396
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
update
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
collision_detection::World::ObserverCallbackFn
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
Definition: world.h:302
planning_scene::PlanningScene::checkCollision
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in collision, and if needed, updates the collision transforms of t...
Definition: planning_scene.cpp:494
planning_scene::PlanningScene::checkCollisionUnpadded
void checkCollisionUnpadded(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in collision, but use a collision_detection::CollisionRobot instan...
Definition: planning_scene.cpp:532
kinematic_constraints::KinematicConstraintSet::add
bool add(const moveit_msgs::Constraints &c, const moveit::core::Transforms &tf)
Add all known constraints.
Definition: kinematic_constraint.cpp:1275
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:382
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
y
double y
octomap_msgs::readTree
void readTree(TreeType *octree, const Octomap &msg)
planning_scene::PlanningScene::world_
collision_detection::WorldPtr world_
Definition: planning_scene.h:1161
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
collision_detection::CollisionRequest::max_contacts
std::size_t max_contacts
Overall maximum number of contacts to compute.
Definition: include/moveit/collision_detection/collision_common.h:212
planning_scene::PlanningScene::removeObjectType
void removeObjectType(const std::string &id)
Definition: planning_scene.cpp:2014
collision_detection::CollisionRequest::verbose
bool verbose
Flag indicating whether information about detected collisions should be reported.
Definition: include/moveit/collision_detection/collision_common.h:225
planning_scene::PlanningScene::setObjectType
void setObjectType(const std::string &id, const object_recognition_msgs::ObjectType &type)
Definition: planning_scene.cpp:2007
collision_detection::CollisionRequest::group_name
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)
Definition: include/moveit/collision_detection/collision_common.h:197
planning_scene::PlanningScene::removeAllCollisionObjects
void removeAllCollisionObjects()
Clear all collision objects in planning scene.
Definition: planning_scene.cpp:1442
planning_scene::PlanningScene::isPathValid
bool isPathValid(const moveit_msgs::RobotState &start_state, const moveit_msgs::RobotTrajectory &trajectory, const std::string &group="", bool verbose=false, std::vector< std::size_t > *invalid_index=nullptr) const
Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibili...
Definition: planning_scene.cpp:2200
srdf::ModelConstSharedPtr
std::shared_ptr< const Model > ModelConstSharedPtr
moveit::core::AttachedBody::getSubframes
const moveit::core::FixedTransformsMap & getSubframes() const
Get subframes of this object (relative to the object pose). The returned transforms are guaranteed to...
Definition: attached_body.h:210
planning_scene::PlanningScene::~PlanningScene
~PlanningScene()
Definition: planning_scene.cpp:170
planning_scene::PlanningScene::state_feasibility_
StateFeasibilityFn state_feasibility_
Definition: planning_scene.h:1172
collision_detection::CollisionEnv::ObjectConstPtr
World::ObjectConstPtr ObjectConstPtr
Definition: collision_env.h:279
planning_scene::PlanningScene::shapesAndPosesFromCollisionObjectMessage
bool shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::CollisionObject &object, Eigen::Isometry3d &object_pose_in_header_frame, std::vector< shapes::ShapeConstPtr > &shapes, EigenSTL::vector_Isometry3d &shape_poses)
Takes the object message and returns the object pose, shapes and shape poses. If the object pose is e...
Definition: planning_scene.cpp:1765
planning_scene::PlanningScene::world_const_
collision_detection::WorldConstPtr world_const_
Definition: planning_scene.h:1162
planning_scene::PlanningScene::getKnownObjectTypes
void getKnownObjectTypes(ObjectTypeMap &kc) const
Definition: planning_scene.cpp:2020
moveit::core::RobotState::getAttachedBodies
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
Definition: robot_state.cpp:1113
moveit::core::AttachedBodyCallback
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Definition: attached_body.h:115
planning_scene::PlanningScene::getPlanningSceneDiffMsg
void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene &scene) const
Fill the message scene with the differences between this instance of PlanningScene with respect to th...
Definition: planning_scene.cpp:671
kinematic_constraints::KinematicConstraintSet
A class that contains many different constraints, and can check RobotState *versus the full set....
Definition: kinematic_constraint.h:904
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
planning_scene::ObjectColorMap
std::map< std::string, std_msgs::ColorRGBA > ObjectColorMap
A map from object names (e.g., attached bodies, collision objects) to their colors.
Definition: planning_scene.h:194
planning_scene::PlanningScene::readPoseFromText
bool readPoseFromText(std::istream &in, Eigen::Isometry3d &pose) const
Definition: planning_scene.cpp:1175
planning_scene::PlanningScene::object_types_
std::unique_ptr< ObjectTypeMap > object_types_
Definition: planning_scene.h:1178
planning_scene::PlanningScene::object_colors_
std::unique_ptr< ObjectColorMap > object_colors_
Definition: planning_scene.h:1175
planning_scene::PlanningScene::getCollisionEnvUnpadded
const collision_detection::CollisionEnvConstPtr & getCollisionEnvUnpadded() const
Get the active collision detector for the robot.
Definition: planning_scene.h:417
planning_scene::PlanningScene::processOctomapMsg
void processOctomapMsg(const octomap_msgs::OctomapWithPose &map)
Definition: planning_scene.cpp:1454
collision_detection::BodyTypes::ROBOT_LINK
@ ROBOT_LINK
A link on the robot.
Definition: include/moveit/collision_detection/collision_common.h:91
shapes::OcTree
planning_scene::PlanningScene::processCollisionObjectMove
bool processCollisionObjectMove(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1913
collision_detection::World::Object
A representation of an object.
Definition: world.h:79
collision_detection::CollisionDetectorAllocatorTemplate< CollisionEnvFCL, CollisionDetectorAllocatorFCL >::create
static CollisionDetectorAllocatorPtr create()
Definition: collision_detector_allocator.h:123
r
S r
planning_scene::PlanningScene::setAttachedBodyUpdateCallback
void setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback &callback)
Set the callback to be triggered when changes are made to the current scene state.
Definition: planning_scene.cpp:627
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
constructMsgFromShape
bool constructMsgFromShape(const Shape *shape, ShapeMsg &shape_msg)
collision_detection::CollisionResult::contacts
ContactMap contacts
A map returning the pairs of body ids in contact, plus their contact details.
Definition: include/moveit/collision_detection/collision_common.h:418
planning_scene::PlanningScene::acm_
collision_detection::AllowedCollisionMatrixPtr acm_
Definition: planning_scene.h:1170
planning_scene::PlanningScene::getName
const std::string & getName() const
Get the name of the planning scene. This is empty by default.
Definition: planning_scene.h:220
planning_scene::PlanningScene::removeObjectColor
void removeObjectColor(const std::string &id)
Definition: planning_scene.cpp:2076
kinematic_constraints::KinematicConstraintSet::empty
bool empty() const
Returns whether or not there are any constraints in the set.
Definition: kinematic_constraint.h:1094
shapes::saveAsText
void saveAsText(const Shape *shape, std::ostream &out)
planning_scene::PlanningScene::loadGeometryFromStream
bool loadGeometryFromStream(std::istream &in)
Load the geometry of the planning scene from a stream.
Definition: planning_scene.cpp:1041
planning_scene::PlanningScene::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: planning_scene.h:1150
start
ROSCPP_DECL void start()
planning_scene::PlanningScene::getCollidingLinks
void getCollidingLinks(std::vector< std::string > &links)
Get the names of the links that are involved in collisions for the current state.
Definition: planning_scene.cpp:579
planning_scene::PlanningScene::clone
static PlanningScenePtr clone(const PlanningSceneConstPtr &scene)
Clone a planning scene. Even if the scene scene depends on a parent, the cloned scene will not.
Definition: planning_scene.cpp:238
pose_
const geometry_msgs::Pose * pose_
Definition: planning_scene.cpp:799
moveit::core::RobotState::getFrameTransform
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
Definition: robot_state.cpp:1191
planning_scene::PlanningScene::setPlanningSceneDiffMsg
bool setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene &scene)
Apply changes to this planning scene as diffs, even if the message itself is not marked as being a di...
Definition: planning_scene.cpp:1297
planning_scene::PlanningScene::getPlanningFrame
const std::string & getPlanningFrame() const
Get the frame in which planning is performed.
Definition: planning_scene.h:278
planning_scene::PlanningScene::usePlanningSceneMsg
bool usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene)
Call setPlanningSceneMsg() or setPlanningSceneDiffMsg() depending on how the is_diff member of the me...
Definition: planning_scene.cpp:1388
planning_scene::PlanningScene::setPlanningSceneMsg
bool setPlanningSceneMsg(const moveit_msgs::PlanningScene &scene)
Set this instance of a planning scene to be the same as the one serialized in the scene message,...
Definition: planning_scene.cpp:1351
append
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
planning_scene::PlanningScene::isStateFeasible
bool isStateFeasible(const moveit_msgs::RobotState &state, bool verbose=false) const
Check if a given state is feasible, in accordance to the feasibility predicate specified by setStateF...
Definition: planning_scene.cpp:2107
shapes::constructShapeFromMsg
Shape * constructShapeFromMsg(const shape_msgs::Mesh &shape_msg)
moveit::core::Transforms
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:122
planning_scene::PlanningScene::getAttachedCollisionObjectMsg
bool getAttachedCollisionObjectMsg(moveit_msgs::AttachedCollisionObject &attached_collision_obj, const std::string &ns) const
Construct a message (attached_collision_object) with the attached collision object data from the plan...
Definition: planning_scene.cpp:852
moveit::core::Transforms::Transforms
Transforms(const std::string &target_frame)
Construct a transform list.
Definition: transforms.cpp:111
moveit::core::AttachedBody::getAttachedLinkName
const std::string & getAttachedLinkName() const
Get the name of the link this body is attached to.
Definition: attached_body.h:156
collision_detection::CollisionRequest::contacts
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
Definition: include/moveit/collision_detection/collision_common.h:209
planning_scene::PlanningScene::CollisionDetector::findParent
void findParent(const PlanningScene &scene)
Definition: planning_scene.cpp:273
planning_scene::PlanningScene::CollisionDetector::cenv_
collision_detection::CollisionEnvPtr cenv_
Definition: planning_scene.h:1119
planning_scene::PlanningScene::getCollisionEnvNonConst
const collision_detection::CollisionEnvPtr & getCollisionEnvNonConst()
Get the representation of the collision robot This can be used to set padding and link scale on the a...
Definition: planning_scene.cpp:604
moveit::core::AttachedBody::getShapes
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
Definition: attached_body.h:168
planning_scene::PlanningScene::OCTOMAP_NS
static const MOVEIT_PLANNING_SCENE_EXPORT std::string OCTOMAP_NS
Definition: planning_scene.h:214
planning_scene::PlanningScene::checkSelfCollision
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in self collision.
Definition: planning_scene.cpp:510
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
planning_scene::SceneTransforms::getTransform
const Eigen::Isometry3d & getTransform(const std::string &from_frame) const override
Get transform for from_frame (w.r.t target frame)
Definition: planning_scene.cpp:116
planning_scene::PlanningScene::scene_transforms_
moveit::core::TransformsPtr scene_transforms_
Definition: planning_scene.h:1159
planning_scene::PlanningScene::processPlanningSceneWorldMsg
bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world)
Definition: planning_scene.cpp:1379
planning_scene::PlanningScene::current_state_attached_body_callback_
moveit::core::AttachedBodyCallback current_state_attached_body_callback_
Definition: planning_scene.h:1155
planning_scene::PlanningScene::getFrameTransform
const Eigen::Isometry3d & getFrameTransform(const std::string &id) const
Get the transform corresponding to the frame id. This will be known if id is a link name,...
Definition: planning_scene.cpp:1935
planning_scene::SceneTransforms::knowsObjectFrame
bool knowsObjectFrame(const std::string &frame_id) const
Definition: planning_scene.cpp:123
tf2::toMsg
B toMsg(const A &a)
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
collision_detection::CollisionRequest::max_cost_sources
std::size_t max_cost_sources
When costs are computed, this value defines how many of the top cost sources should be returned.
Definition: include/moveit/collision_detection/collision_common.h:219
collision_detection::CollisionResult::collision
bool collision
True if collision was found, false otherwise.
Definition: include/moveit/collision_detection/collision_common.h:406
conversions.h
x
double x
octomap
planning_scene::PlanningScene::getOctomapMsg
bool getOctomapMsg(octomap_msgs::OctomapWithPose &octomap) const
Construct a message (octomap) with the octomap data from the planning_scene.
Definition: planning_scene.cpp:876
planning_scene::PlanningScene::setCollisionObjectUpdateCallback
void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn &callback)
Set the callback to be triggered when changes are made to the current scene world.
Definition: planning_scene.cpp:634
collision_detection::CollisionResult::cost_sources
std::set< CostSource > cost_sources
These are the individual cost sources when costs are computed.
Definition: include/moveit/collision_detection/collision_common.h:421
moveit::core::Transforms::copyTransforms
void copyTransforms(std::vector< geometry_msgs::TransformStamped > &transforms) const
Get a vector of all the transforms as ROS messages.
Definition: transforms.cpp:220
planning_scene::PlanningScene::getTransformsNonConst
moveit::core::Transforms & getTransformsNonConst()
Get the set of fixed transforms from known frames to the planning frame.
Definition: planning_scene.cpp:657
moveit::ConstructException
This may be thrown during construction of objects if errors occur.
Definition: exceptions.h:77
collision_detection::CollisionRequest::max_contacts_per_pair
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
Definition: include/moveit/collision_detection/collision_common.h:216
planning_scene::PlanningScene::name_
std::string name_
Definition: planning_scene.h:1146
collision_detection::World::CREATE
@ CREATE
Definition: world.h:255
header
const std::string header
planning_scene
This namespace includes the central class for representing planning scenes.
Definition: planning_interface.h:45
trajectory_tools.h
planning_scene::PlanningScene::saveGeometryToStream
void saveGeometryToStream(std::ostream &out) const
Save the geometry of the planning scene to a stream, as plain text.
Definition: planning_scene.cpp:999
collision_detection::OccMapTreePtr
std::shared_ptr< OccMapTree > OccMapTreePtr
Definition: occupancy_map.h:148
moveit::core::AttachedBody::getPose
const Eigen::Isometry3d & getPose() const
Get the pose of the attached body relative to the parent link.
Definition: attached_body.h:144
planning_scene::PlanningScene::getCollisionEnv
const collision_detection::CollisionEnvConstPtr & getCollisionEnv() const
Get the active collision environment.
Definition: planning_scene.h:411
z
double z
planning_scene::PlanningScene::processCollisionObjectRemove
bool processCollisionObjectRemove(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1892
planning_scene::PlanningScene::processCollisionObjectMsg
bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1724
moveit::core::AttachedBody::getShapePoses
const EigenSTL::vector_Isometry3d & getShapePoses() const
Get the shape poses (the transforms to the shapes of this body, relative to the pose)....
Definition: attached_body.h:175
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
message_checks.h
planning_scene::PlanningScene::CollisionDetector
friend struct CollisionDetector
Definition: planning_scene.h:1138
planning_scene::PlanningScene::createRobotModel
static moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr &urdf_model, const srdf::ModelConstSharedPtr &srdf_model)
Definition: planning_scene.cpp:192
planning_scene::PlanningScene::diff
PlanningScenePtr diff() const
Return a new child PlanningScene that uses this one as parent.
Definition: planning_scene.cpp:246
planning_scene::SceneTransforms::SceneTransforms
SceneTransforms(const PlanningScene *scene)
Definition: planning_scene.cpp:95
planning_scene::PlanningScene::processCollisionObjectAdd
bool processCollisionObjectAdd(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1848
planning_scene::SceneTransforms::isFixedFrame
bool isFixedFrame(const std::string &frame) const override
Check whether a frame stays constant as the state of the robot model changes. This is true for any tr...
Definition: planning_scene.cpp:104
planning_scene::PlanningScene::poseMsgToEigen
static void poseMsgToEigen(const geometry_msgs::Pose &msg, Eigen::Isometry3d &out)
Definition: planning_scene.cpp:1749
verbose
bool verbose
moveit::core::isEmpty
bool isEmpty(const moveit_msgs::PlanningScene &msg)
Check if a message includes any information about a planning scene, or whether it is empty.
Definition: message_checks.cpp:107
planning_scene::PlanningScene::setObjectColor
void setObjectColor(const std::string &id, const std_msgs::ColorRGBA &color)
Definition: planning_scene.cpp:2064
shapes::constructShapeFromText
Shape * constructShapeFromText(std::istream &in)
planning_scene::PlanningScene::setActiveCollisionDetector
void setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr &allocator, bool exclusive=false)
Set the type of collision detector to use. Calls addCollisionDetector() to add it if it has not alrea...
Definition: planning_scene.cpp:316
robot_trajectory::RobotTrajectory::getWayPoint
const moveit::core::RobotState & getWayPoint(std::size_t index) const
Definition: robot_trajectory.h:136


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Mar 3 2024 03:23:35