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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat May 21 2022 02:24:56