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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 21 2021 02:23:41