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