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>
48 #include <memory>
49 #include <set>
50 
51 namespace planning_scene
52 {
53 const std::string PlanningScene::OCTOMAP_NS = "<octomap>";
54 const std::string PlanningScene::DEFAULT_SCENE_NAME = "(noname)";
55 
57 {
58 public:
59  SceneTransforms(const PlanningScene* scene) : Transforms(scene->getRobotModel()->getModelFrame()), scene_(scene)
60  {
61  }
62 
63  bool canTransform(const std::string& from_frame) const override
64  {
65  return scene_->knowsFrameTransform(from_frame);
66  }
67 
68  bool isFixedFrame(const std::string& frame) const override
69  {
70  if (frame.empty())
71  return false;
72  if (Transforms::isFixedFrame(frame))
73  return true;
74  if (frame[0] == '/')
75  return knowsObject(frame.substr(1));
76  else
77  return knowsObject(frame);
78  }
79 
80  const Eigen::Affine3d& getTransform(const std::string& from_frame) const override
81  { // the call below also calls Transforms::getTransform() too
82  return scene_->getFrameTransform(from_frame);
83  }
84 
85 private:
86  bool knowsObject(const std::string& id) const
87  {
88  if (scene_->getWorld()->hasObject(id))
89  {
90  collision_detection::World::ObjectConstPtr obj = scene_->getWorld()->getObject(id);
91  return obj->shape_poses_.size() == 1;
92  }
93  return false;
94  }
95 
97 };
98 
99 bool PlanningScene::isEmpty(const moveit_msgs::PlanningScene& msg)
100 {
101  return msg.name.empty() && msg.fixed_frame_transforms.empty() && msg.allowed_collision_matrix.entry_names.empty() &&
102  msg.link_padding.empty() && msg.link_scale.empty() && isEmpty(msg.robot_state) && isEmpty(msg.world);
103 }
104 
105 bool PlanningScene::isEmpty(const moveit_msgs::RobotState& msg)
106 {
107  /* a state is empty if it includes no information and it is a diff; if the state is not a diff, then the implicit
108  information is
109  that the set of attached bodies is empty, so they must be cleared from the state to be updated */
110  return static_cast<bool>(msg.is_diff) && msg.multi_dof_joint_state.joint_names.empty() &&
111  msg.joint_state.name.empty() && msg.attached_collision_objects.empty() && msg.joint_state.position.empty() &&
112  msg.joint_state.velocity.empty() && msg.joint_state.effort.empty() &&
113  msg.multi_dof_joint_state.transforms.empty() && msg.multi_dof_joint_state.twist.empty() &&
114  msg.multi_dof_joint_state.wrench.empty();
115 }
116 
117 bool PlanningScene::isEmpty(const moveit_msgs::PlanningSceneWorld& msg)
118 {
119  return msg.collision_objects.empty() && msg.octomap.octomap.data.empty();
120 }
121 
122 PlanningScene::PlanningScene(const robot_model::RobotModelConstPtr& robot_model, collision_detection::WorldPtr world)
123  : kmodel_(robot_model), world_(world), world_const_(world)
124 {
125  initialize();
126 }
127 
128 PlanningScene::PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model,
129  const srdf::ModelConstSharedPtr& srdf_model, collision_detection::WorldPtr world)
130  : world_(world), world_const_(world)
131 {
132  if (!urdf_model)
133  throw moveit::ConstructException("The URDF model cannot be NULL");
134 
135  if (!srdf_model)
136  throw moveit::ConstructException("The SRDF model cannot be NULL");
137 
138  kmodel_ = createRobotModel(urdf_model, srdf_model);
139  if (!kmodel_)
140  throw moveit::ConstructException("Could not create RobotModel");
141 
142  initialize();
143 }
144 
146 {
149 }
150 
152 {
154 
155  ftf_.reset(new SceneTransforms(this));
156 
158  kstate_->setToDefaultValues();
159 
161  // Use default collision operations in the SRDF to setup the acm
162  const std::vector<std::string>& collision_links = kmodel_->getLinkModelNamesWithCollisionGeometry();
163  acm_->setEntry(collision_links, collision_links, false);
164 
165  // allow collisions for pairs that have been disabled
166  const std::vector<srdf::Model::DisabledCollision>& dc = getRobotModel()->getSRDF()->getDisabledCollisionPairs();
167  for (std::vector<srdf::Model::DisabledCollision>::const_iterator it = dc.begin(); it != dc.end(); ++it)
168  acm_->setEntry(it->link1_, it->link2_, true);
169 
171 }
172 
173 /* return NULL on failure */
174 robot_model::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
175  const srdf::ModelConstSharedPtr& srdf_model)
176 {
177  robot_model::RobotModelPtr robot_model(new robot_model::RobotModel(urdf_model, srdf_model));
178  if (!robot_model || !robot_model->getRootJoint())
179  return robot_model::RobotModelPtr();
180 
181  return robot_model;
182 }
183 
184 PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(parent)
185 {
186  if (!parent_)
187  throw moveit::ConstructException("NULL parent pointer for planning scene");
188 
189  if (!parent_->getName().empty())
190  name_ = parent_->getName() + "+";
191 
192  kmodel_ = parent_->kmodel_;
193 
194  // maintain a separate world. Copy on write ensures that most of the object
195  // info is shared until it is modified.
196  world_.reset(new collision_detection::World(*parent_->world_));
198 
199  // record changes to the world
201 
202  // Set up the same collision detectors as the parent
203  for (CollisionDetectorConstIterator it = parent_->collision_.begin(); it != parent_->collision_.end(); ++it)
204  {
205  const CollisionDetectorPtr& parent_detector = it->second;
206  CollisionDetectorPtr& detector = collision_[it->first];
207  detector.reset(new CollisionDetector());
208  detector->alloc_ = parent_detector->alloc_;
209  detector->parent_ = parent_detector;
210 
211  detector->cworld_ = detector->alloc_->allocateWorld(parent_detector->cworld_, world_);
212  detector->cworld_const_ = detector->cworld_;
213 
214  // leave these empty and use parent collision_robot_ unless/until a non-const one
215  // is requested (e.g. to modify link padding or scale)
216  detector->crobot_.reset();
217  detector->crobot_const_.reset();
218  detector->crobot_unpadded_.reset();
219  detector->crobot_unpadded_const_.reset();
220  }
221  setActiveCollisionDetector(parent_->getActiveCollisionDetectorName());
222 }
223 
224 PlanningScenePtr PlanningScene::clone(const PlanningSceneConstPtr& scene)
225 {
226  PlanningScenePtr result = scene->diff();
227  result->decoupleParent();
228  result->setName(scene->getName());
229  return result;
230 }
231 
232 PlanningScenePtr PlanningScene::diff() const
233 {
234  return PlanningScenePtr(new PlanningScene(shared_from_this()));
235 }
236 
237 PlanningScenePtr PlanningScene::diff(const moveit_msgs::PlanningScene& msg) const
238 {
239  PlanningScenePtr result = diff();
240  result->setPlanningSceneDiffMsg(msg);
241  return result;
242 }
243 
245 {
246  if (!crobot_)
247  {
248  alloc_->allocateRobot(parent_->getCollisionRobot());
249  crobot_const_ = crobot_;
250  }
251 
252  crobot_->setLinkPadding(src.getCollisionRobot()->getLinkPadding());
253  crobot_->setLinkScale(src.getCollisionRobot()->getLinkScale());
254 }
255 
257 {
258  if (!active_collision_->crobot_)
259  return;
260 
261  for (CollisionDetectorIterator it = collision_.begin(); it != collision_.end(); ++it)
262  {
263  if (it->second != active_collision_)
264  it->second->copyPadding(*active_collision_);
265  }
266 }
267 
269 {
270  if (parent_ || !scene.parent_)
271  return;
272 
273  CollisionDetectorConstIterator it = scene.parent_->collision_.find(alloc_->getName());
274  if (it != scene.parent_->collision_.end())
275  parent_ = it->second->parent_;
276 }
277 
278 void PlanningScene::addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator)
279 {
280  const std::string& name = allocator->getName();
281  CollisionDetectorPtr& detector = collision_[name];
282 
283  if (detector) // already added this one
284  return;
285 
286  detector.reset(new CollisionDetector());
287 
288  detector->alloc_ = allocator;
289 
290  if (!active_collision_)
291  active_collision_ = detector;
292 
293  detector->findParent(*this);
294 
295  detector->cworld_ = detector->alloc_->allocateWorld(world_);
296  detector->cworld_const_ = detector->cworld_;
297 
298  // Allocate CollisionRobot unless we can use the parent's crobot_.
299  // If active_collision_->crobot_ is non-NULL there is local padding and we cannot use the parent's crobot_.
300  if (!detector->parent_ || active_collision_->crobot_)
301  {
302  detector->crobot_ = detector->alloc_->allocateRobot(getRobotModel());
303  detector->crobot_const_ = detector->crobot_;
304 
305  if (detector != active_collision_)
306  detector->copyPadding(*active_collision_);
307  }
308 
309  // Allocate CollisionRobot unless we can use the parent's crobot_unpadded_.
310  if (!detector->parent_)
311  {
312  detector->crobot_unpadded_ = detector->alloc_->allocateRobot(getRobotModel());
313  detector->crobot_unpadded_const_ = detector->crobot_unpadded_;
314  }
315 }
316 
317 void PlanningScene::setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
318  bool exclusive)
319 {
320  if (exclusive)
321  {
322  CollisionDetectorPtr p;
323  CollisionDetectorIterator it = collision_.find(allocator->getName());
324 
325  if (it != collision_.end())
326  p = it->second;
327 
328  collision_.clear();
329  active_collision_.reset();
330 
331  if (p)
332  {
333  collision_[allocator->getName()] = p;
334  active_collision_ = p;
335  return;
336  }
337  }
338 
339  addCollisionDetector(allocator);
340  setActiveCollisionDetector(allocator->getName());
341 }
342 
343 bool PlanningScene::setActiveCollisionDetector(const std::string& collision_detector_name)
344 {
345  CollisionDetectorIterator it = collision_.find(collision_detector_name);
346  if (it != collision_.end())
347  {
348  active_collision_ = it->second;
349  return true;
350  }
351  else
352  {
353  ROS_ERROR_NAMED("planning_scene",
354  "Cannot setActiveCollisionDetector to '%s' -- it has been added to PlanningScene. "
355  "Keeping existing active collision detector '%s'",
356  collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str());
357  return false;
358  }
359 }
360 
361 const void
362 PlanningScene::addCollisionDetectorConst(const collision_detection::CollisionDetectorAllocatorPtr& allocator) const
363 {
364  const std::string& name = allocator->getName();
365  CollisionDetectorPtr& collision_detector = collision_[name];
366 
367  if (collision_detector) // already added this one
368  return;
369 
370  collision_detector.reset(new CollisionDetector());
371 
372  collision_detector->alloc_ = allocator;
373 
374  if (!active_collision_)
375  active_collision_ = collision_detector;
376 
377  collision_detector->findParent(*this);
378 
379  collision_detector->cworld_ = collision_detector->alloc_->allocateWorld(world_);
380  collision_detector->cworld_const_ = collision_detector->cworld_;
381 
382  // Allocate CollisionRobot unless we can use the parent's crobot_.
383  // If active_collision_->crobot_ is non-NULL there is local padding and we cannot use the parent's crobot_.
384  if (!collision_detector->parent_ || active_collision_->crobot_)
385  {
386  collision_detector->crobot_ = collision_detector->alloc_->allocateRobot(getRobotModel());
387  collision_detector->crobot_const_ = collision_detector->crobot_;
388 
389  if (collision_detector != active_collision_)
390  collision_detector->copyPadding(*active_collision_);
391  }
392 
393  // Allocate CollisionRobot unless we can use the parent's crobot_unpadded_.
394  if (!collision_detector->parent_)
395  {
396  collision_detector->crobot_unpadded_ = collision_detector->alloc_->allocateRobot(getRobotModel());
397  collision_detector->crobot_unpadded_const_ = collision_detector->crobot_unpadded_;
398  }
399 }
400 
402  const collision_detection::CollisionDetectorAllocatorPtr& allocator, bool exclusive) const
403 {
404  if (exclusive)
405  {
406  CollisionDetectorPtr collision_detector_ptr;
407  CollisionDetectorIterator it = collision_.find(allocator->getName());
408 
409  if (it != collision_.end())
410  collision_detector_ptr = it->second;
411 
412  collision_.clear();
413  active_collision_.reset();
414 
415  if (collision_detector_ptr)
416  {
417  collision_[allocator->getName()] = collision_detector_ptr;
418  active_collision_ = collision_detector_ptr;
419  return;
420  }
421  }
422 
423  addCollisionDetectorConst(allocator);
424  setActiveCollisionDetectorConst(allocator->getName());
425 }
426 
427 const bool PlanningScene::setActiveCollisionDetectorConst(const std::string& collision_detector_name) const
428 {
429  CollisionDetectorIterator it = collision_.find(collision_detector_name);
430  if (it != collision_.end())
431  {
432  active_collision_ = it->second;
433  return true;
434  }
435  else
436  {
437  ROS_ERROR_NAMED("planning_scene",
438  "Cannot setActiveCollisionDetector to '%s' -- it has been added to PlanningScene. "
439  "Keeping existing active collision detector '%s'",
440  collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str());
441  return false;
442  }
443 }
444 
445 void PlanningScene::getCollisionDetectorNames(std::vector<std::string>& names) const
446 {
447  names.clear();
448  names.reserve(collision_.size());
449  for (CollisionDetectorConstIterator it = collision_.begin(); it != collision_.end(); ++it)
450  names.push_back(it->first);
451 }
452 
453 const collision_detection::CollisionWorldConstPtr&
454 PlanningScene::getCollisionWorld(const std::string& collision_detector_name) const
455 {
456  CollisionDetectorConstIterator it = collision_.find(collision_detector_name);
457  if (it == collision_.end())
458  {
459  ROS_ERROR_NAMED("planning_scene",
460  "Could not get CollisionWorld named '%s'. Returning active CollisionWorld '%s' instead",
461  collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str());
462  return active_collision_->cworld_const_;
463  }
464 
465  return it->second->cworld_const_;
466 }
467 
468 const collision_detection::CollisionRobotConstPtr&
469 PlanningScene::getCollisionRobot(const std::string& collision_detector_name) const
470 {
471  CollisionDetectorConstIterator it = collision_.find(collision_detector_name);
472  if (it == collision_.end())
473  {
474  ROS_ERROR_NAMED("planning_scene",
475  "Could not get CollisionRobot named '%s'. Returning active CollisionRobot '%s' instead",
476  collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str());
477  return active_collision_->getCollisionRobot();
478  }
479 
480  return it->second->getCollisionRobot();
481 }
482 
483 const collision_detection::CollisionRobotConstPtr&
484 PlanningScene::getCollisionRobotUnpadded(const std::string& collision_detector_name) const
485 {
486  CollisionDetectorConstIterator it = collision_.find(collision_detector_name);
487  if (it == collision_.end())
488  {
489  ROS_ERROR_NAMED("planning_scene", "Could not get CollisionRobotUnpadded named '%s'. "
490  "Returning active CollisionRobotUnpadded '%s' instead",
491  collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str());
492  return active_collision_->getCollisionRobotUnpadded();
493  }
494 
495  return it->second->getCollisionRobotUnpadded();
496 }
497 
499 {
500  if (!parent_)
501  return;
502 
503  // clear everything, reset the world, record diffs
504  world_.reset(new collision_detection::World(*parent_->world_));
509 
510  // use parent crobot_ if it exists. Otherwise copy padding from parent.
511  for (CollisionDetectorIterator it = collision_.begin(); it != collision_.end(); ++it)
512  {
513  if (!it->second->parent_)
514  it->second->findParent(*this);
515 
516  if (it->second->parent_)
517  {
518  it->second->crobot_.reset();
519  it->second->crobot_const_.reset();
520  it->second->crobot_unpadded_.reset();
521  it->second->crobot_unpadded_const_.reset();
522 
523  it->second->cworld_ = it->second->alloc_->allocateWorld(it->second->parent_->cworld_, world_);
524  it->second->cworld_const_ = it->second->cworld_;
525  }
526  else
527  {
528  it->second->copyPadding(*parent_->active_collision_);
529 
530  it->second->cworld_ = it->second->alloc_->allocateWorld(world_);
531  it->second->cworld_const_ = it->second->cworld_;
532  }
533  }
534 
535  ftf_.reset();
536  kstate_.reset();
537  acm_.reset();
538  object_colors_.reset();
539  object_types_.reset();
540 }
541 
542 void PlanningScene::pushDiffs(const PlanningScenePtr& scene)
543 {
544  if (!parent_)
545  return;
546 
547  if (ftf_)
548  scene->getTransformsNonConst().setAllTransforms(ftf_->getAllTransforms());
549 
550  if (kstate_)
551  {
552  scene->getCurrentStateNonConst() = *kstate_;
553  // push colors and types for attached objects
554  std::vector<const moveit::core::AttachedBody*> attached_objs;
555  kstate_->getAttachedBodies(attached_objs);
556  for (std::vector<const moveit::core::AttachedBody*>::const_iterator it = attached_objs.begin();
557  it != attached_objs.end(); ++it)
558  {
559  if (hasObjectType((*it)->getName()))
560  scene->setObjectType((*it)->getName(), getObjectType((*it)->getName()));
561  if (hasObjectColor((*it)->getName()))
562  scene->setObjectColor((*it)->getName(), getObjectColor((*it)->getName()));
563  }
564  }
565 
566  if (acm_)
567  scene->getAllowedCollisionMatrixNonConst() = *acm_;
568 
569  if (active_collision_->crobot_)
570  {
571  collision_detection::CollisionRobotPtr active_crobot = scene->getCollisionRobotNonConst();
572  active_crobot->setLinkPadding(active_collision_->crobot_->getLinkPadding());
573  active_crobot->setLinkScale(active_collision_->crobot_->getLinkScale());
574  scene->propogateRobotPadding();
575  }
576 
577  if (world_diff_)
578  {
579  for (collision_detection::WorldDiff::const_iterator it = world_diff_->begin(); it != world_diff_->end(); ++it)
580  {
581  if (it->second == collision_detection::World::DESTROY)
582  {
583  scene->world_->removeObject(it->first);
584  scene->removeObjectColor(it->first);
585  scene->removeObjectType(it->first);
586  }
587  else
588  {
589  const collision_detection::World::Object& obj = *world_->getObject(it->first);
590  scene->world_->removeObject(obj.id_);
591  scene->world_->addToObject(obj.id_, obj.shapes_, obj.shape_poses_);
592  if (hasObjectColor(it->first))
593  scene->setObjectColor(it->first, getObjectColor(it->first));
594  if (hasObjectType(it->first))
595  scene->setObjectType(it->first, getObjectType(it->first));
596  }
597  }
598  }
599 }
600 
603 {
604  if (getCurrentState().dirtyCollisionBodyTransforms())
606  else
607  checkCollision(req, res, getCurrentState());
608 }
609 
612  const robot_state::RobotState& kstate) const
613 {
614  // check collision with the world using the padded version
615  getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobot(), kstate, getAllowedCollisionMatrix());
616 
617  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
618  {
619  // do self-collision checking with the unpadded version of the robot
620  getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, getAllowedCollisionMatrix());
621  }
622 }
623 
626 {
627  if (getCurrentState().dirtyCollisionBodyTransforms())
629  else
630  checkSelfCollision(req, res, getCurrentState());
631 }
632 
636 {
637  // check collision with the world using the padded version
638  getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobot(), kstate, acm);
639 
640  // do self-collision checking with the unpadded version of the robot
641  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
642  getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, acm);
643 }
644 
647 {
648  if (getCurrentState().dirtyCollisionBodyTransforms())
650  else
652 }
653 
656  const robot_state::RobotState& kstate,
658 {
659  // check collision with the world using the unpadded version
660  getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobotUnpadded(), kstate, acm);
661 
662  // do self-collision checking with the unpadded version of the robot
663  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
664  {
665  getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, acm);
666  }
667 }
668 
670 {
671  if (getCurrentState().dirtyCollisionBodyTransforms())
673  else
675 }
676 
678  const robot_state::RobotState& kstate,
680 {
682  req.contacts = true;
683  req.max_contacts = getRobotModel()->getLinkModelsWithCollisionGeometry().size() + 1;
684  req.max_contacts_per_pair = 1;
686  checkCollision(req, res, kstate, acm);
687  res.contacts.swap(contacts);
688 }
689 
690 void PlanningScene::getCollidingLinks(std::vector<std::string>& links)
691 {
692  if (getCurrentState().dirtyCollisionBodyTransforms())
694  else
696 }
697 
698 void PlanningScene::getCollidingLinks(std::vector<std::string>& links, const robot_state::RobotState& kstate,
700 {
702  getCollidingPairs(contacts, kstate, acm);
703  links.clear();
704  for (collision_detection::CollisionResult::ContactMap::const_iterator it = contacts.begin(); it != contacts.end();
705  ++it)
706  for (std::size_t j = 0; j < it->second.size(); ++j)
707  {
708  if (it->second[j].body_type_1 == collision_detection::BodyTypes::ROBOT_LINK)
709  links.push_back(it->second[j].body_name_1);
710  if (it->second[j].body_type_2 == collision_detection::BodyTypes::ROBOT_LINK)
711  links.push_back(it->second[j].body_name_2);
712  }
713 }
714 
715 const collision_detection::CollisionRobotPtr& PlanningScene::getCollisionRobotNonConst()
716 {
717  if (!active_collision_->crobot_)
718  {
719  active_collision_->crobot_ =
720  active_collision_->alloc_->allocateRobot(active_collision_->parent_->getCollisionRobot());
721  active_collision_->crobot_const_ = active_collision_->crobot_;
722  }
723  return active_collision_->crobot_;
724 }
725 
727 {
728  if (!kstate_)
729  {
730  kstate_.reset(new robot_state::RobotState(parent_->getCurrentState()));
731  kstate_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
732  }
733  kstate_->update();
734  return *kstate_;
735 }
736 
737 robot_state::RobotStatePtr PlanningScene::getCurrentStateUpdated(const moveit_msgs::RobotState& update) const
738 {
739  robot_state::RobotStatePtr state(new robot_state::RobotState(getCurrentState()));
741  return state;
742 }
743 
745 {
747  if (kstate_)
748  kstate_->setAttachedBodyUpdateCallback(callback);
749 }
750 
752 {
755  if (callback)
756  current_world_object_update_observer_handle_ = world_->addObserver(callback);
758 }
759 
761 {
762  if (!acm_)
763  acm_.reset(new collision_detection::AllowedCollisionMatrix(parent_->getAllowedCollisionMatrix()));
764  return *acm_;
765 }
766 
768 {
770  return static_cast<const PlanningScene*>(this)->getTransforms();
771 }
772 
774 {
776  if (!ftf_)
777  {
778  ftf_.reset(new SceneTransforms(this));
779  ftf_->setAllTransforms(parent_->getTransforms().getAllTransforms());
780  }
781  return *ftf_;
782 }
783 
784 void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene_msg) const
785 {
786  scene_msg.name = name_;
787  scene_msg.robot_model_name = getRobotModel()->getName();
788  scene_msg.is_diff = true;
789 
790  if (ftf_)
791  ftf_->copyTransforms(scene_msg.fixed_frame_transforms);
792  else
793  scene_msg.fixed_frame_transforms.clear();
794 
795  if (kstate_)
796  robot_state::robotStateToRobotStateMsg(*kstate_, scene_msg.robot_state);
797  else
798  {
799  scene_msg.robot_state = moveit_msgs::RobotState();
800  scene_msg.robot_state.is_diff = true;
801  }
802 
803  if (acm_)
804  acm_->getMessage(scene_msg.allowed_collision_matrix);
805  else
806  scene_msg.allowed_collision_matrix = moveit_msgs::AllowedCollisionMatrix();
807 
808  if (active_collision_->crobot_)
809  {
810  active_collision_->crobot_->getPadding(scene_msg.link_padding);
811  active_collision_->crobot_->getScale(scene_msg.link_scale);
812  }
813  else
814  {
815  scene_msg.link_padding.clear();
816  scene_msg.link_scale.clear();
817  }
818 
819  scene_msg.object_colors.clear();
820  if (object_colors_)
821  {
822  unsigned int i = 0;
823  scene_msg.object_colors.resize(object_colors_->size());
824  for (ObjectColorMap::const_iterator it = object_colors_->begin(); it != object_colors_->end(); ++it, ++i)
825  {
826  scene_msg.object_colors[i].id = it->first;
827  scene_msg.object_colors[i].color = it->second;
828  }
829  }
830 
831  scene_msg.world.collision_objects.clear();
832  scene_msg.world.octomap = octomap_msgs::OctomapWithPose();
833 
834  if (world_diff_)
835  {
836  bool do_omap = false;
837  for (collision_detection::WorldDiff::const_iterator it = world_diff_->begin(); it != world_diff_->end(); ++it)
838  {
839  if (it->first == OCTOMAP_NS)
840  do_omap = true;
841  else if (it->second == collision_detection::World::DESTROY)
842  {
843  moveit_msgs::CollisionObject co;
844  co.header.frame_id = getPlanningFrame();
845  co.id = it->first;
846  co.operation = moveit_msgs::CollisionObject::REMOVE;
847  scene_msg.world.collision_objects.push_back(co);
848  }
849  else
850  {
851  scene_msg.world.collision_objects.emplace_back();
852  getCollisionObjectMsg(scene_msg.world.collision_objects.back(), it->first);
853  }
854  }
855  if (do_omap)
856  getOctomapMsg(scene_msg.world.octomap);
857  }
858 }
859 
860 namespace
861 {
862 class ShapeVisitorAddToCollisionObject : public boost::static_visitor<void>
863 {
864 public:
865  ShapeVisitorAddToCollisionObject(moveit_msgs::CollisionObject* obj) : boost::static_visitor<void>(), obj_(obj)
866  {
867  }
868 
869  void setPoseMessage(const geometry_msgs::Pose* pose)
870  {
871  pose_ = pose;
872  }
873 
874  void operator()(const shape_msgs::Plane& shape_msg) const
875  {
876  obj_->planes.push_back(shape_msg);
877  obj_->plane_poses.push_back(*pose_);
878  }
879 
880  void operator()(const shape_msgs::Mesh& shape_msg) const
881  {
882  obj_->meshes.push_back(shape_msg);
883  obj_->mesh_poses.push_back(*pose_);
884  }
885 
886  void operator()(const shape_msgs::SolidPrimitive& shape_msg) const
887  {
888  obj_->primitives.push_back(shape_msg);
889  obj_->primitive_poses.push_back(*pose_);
890  }
891 
892 private:
893  moveit_msgs::CollisionObject* obj_;
894  const geometry_msgs::Pose* pose_;
895 };
896 }
897 
898 bool PlanningScene::getCollisionObjectMsg(moveit_msgs::CollisionObject& collision_obj, const std::string& ns) const
899 {
900  collision_obj.header.frame_id = getPlanningFrame();
901  collision_obj.id = ns;
902  collision_obj.operation = moveit_msgs::CollisionObject::ADD;
904  if (!obj)
905  return false;
906  ShapeVisitorAddToCollisionObject sv(&collision_obj);
907  for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
908  {
909  shapes::ShapeMsg sm;
910  if (constructMsgFromShape(obj->shapes_[j].get(), sm))
911  {
912  geometry_msgs::Pose p;
913  tf::poseEigenToMsg(obj->shape_poses_[j], p);
914 
915  sv.setPoseMessage(&p);
916  boost::apply_visitor(sv, sm);
917  }
918  }
919 
920  if (!collision_obj.primitives.empty() || !collision_obj.meshes.empty() || !collision_obj.planes.empty())
921  {
922  if (hasObjectType(collision_obj.id))
923  collision_obj.type = getObjectType(collision_obj.id);
924  }
925  return true;
926 }
927 
928 void PlanningScene::getCollisionObjectMsgs(std::vector<moveit_msgs::CollisionObject>& collision_objs) const
929 {
930  collision_objs.clear();
931  const std::vector<std::string>& ns = world_->getObjectIds();
932  for (std::size_t i = 0; i < ns.size(); ++i)
933  if (ns[i] != OCTOMAP_NS)
934  {
935  collision_objs.emplace_back();
936  getCollisionObjectMsg(collision_objs.back(), ns[i]);
937  }
938 }
939 
940 bool PlanningScene::getAttachedCollisionObjectMsg(moveit_msgs::AttachedCollisionObject& attached_collision_obj,
941  const std::string& ns) const
942 {
943  std::vector<moveit_msgs::AttachedCollisionObject> attached_collision_objs;
944  getAttachedCollisionObjectMsgs(attached_collision_objs);
945  for (std::size_t i = 0; i < attached_collision_objs.size(); ++i)
946  {
947  if (attached_collision_objs[i].object.id == ns)
948  {
949  attached_collision_obj = attached_collision_objs[i];
950  return true;
951  }
952  }
953  return false;
954 }
955 
957  std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs) const
958 {
959  std::vector<const moveit::core::AttachedBody*> attached_bodies;
960  getCurrentState().getAttachedBodies(attached_bodies);
961  attachedBodiesToAttachedCollisionObjectMsgs(attached_bodies, attached_collision_objs);
962 }
963 
964 bool PlanningScene::getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const
965 {
966  octomap.header.frame_id = getPlanningFrame();
967  octomap.octomap = octomap_msgs::Octomap();
968 
970  if (map)
971  {
972  if (map->shapes_.size() == 1)
973  {
974  const shapes::OcTree* o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
975  octomap_msgs::fullMapToMsg(*o->octree, octomap.octomap);
976  tf::poseEigenToMsg(map->shape_poses_[0], octomap.origin);
977  return true;
978  }
979  ROS_ERROR_NAMED("planning_scene",
980  "Unexpected number of shapes in octomap collision object. Not including '%s' object",
981  OCTOMAP_NS.c_str());
982  }
983  return false;
984 }
985 
986 void PlanningScene::getObjectColorMsgs(std::vector<moveit_msgs::ObjectColor>& object_colors) const
987 {
988  object_colors.clear();
989 
990  unsigned int i = 0;
991  ObjectColorMap cmap;
992  getKnownObjectColors(cmap);
993  object_colors.resize(cmap.size());
994  for (ObjectColorMap::const_iterator it = cmap.begin(); it != cmap.end(); ++it, ++i)
995  {
996  object_colors[i].id = it->first;
997  object_colors[i].color = it->second;
998  }
999 }
1000 
1001 void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg) const
1002 {
1003  scene_msg.name = name_;
1004  scene_msg.is_diff = false;
1005  scene_msg.robot_model_name = getRobotModel()->getName();
1006  getTransforms().copyTransforms(scene_msg.fixed_frame_transforms);
1007 
1008  robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state);
1009  getAllowedCollisionMatrix().getMessage(scene_msg.allowed_collision_matrix);
1010  getCollisionRobot()->getPadding(scene_msg.link_padding);
1011  getCollisionRobot()->getScale(scene_msg.link_scale);
1012 
1013  getObjectColorMsgs(scene_msg.object_colors);
1014 
1015  // add collision objects
1016  getCollisionObjectMsgs(scene_msg.world.collision_objects);
1017 
1018  // get the octomap
1019  getOctomapMsg(scene_msg.world.octomap);
1020 }
1021 
1022 void PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg,
1023  const moveit_msgs::PlanningSceneComponents& comp) const
1024 {
1025  scene_msg.is_diff = false;
1026  if (comp.components & moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS)
1027  {
1028  scene_msg.name = name_;
1029  scene_msg.robot_model_name = getRobotModel()->getName();
1030  }
1031 
1032  if (comp.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
1033  getTransforms().copyTransforms(scene_msg.fixed_frame_transforms);
1034 
1035  if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS)
1036  {
1037  robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, true);
1038  for (std::vector<moveit_msgs::AttachedCollisionObject>::iterator it =
1039  scene_msg.robot_state.attached_collision_objects.begin();
1040  it != scene_msg.robot_state.attached_collision_objects.end(); ++it)
1041  {
1042  if (hasObjectType(it->object.id))
1043  {
1044  it->object.type = getObjectType(it->object.id);
1045  }
1046  }
1047  }
1048  else if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE)
1049  {
1050  robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, false);
1051  }
1052 
1053  if (comp.components & moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX)
1054  getAllowedCollisionMatrix().getMessage(scene_msg.allowed_collision_matrix);
1055 
1056  if (comp.components & moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING)
1057  {
1058  getCollisionRobot()->getPadding(scene_msg.link_padding);
1059  getCollisionRobot()->getScale(scene_msg.link_scale);
1060  }
1061 
1062  if (comp.components & moveit_msgs::PlanningSceneComponents::OBJECT_COLORS)
1063  getObjectColorMsgs(scene_msg.object_colors);
1064 
1065  // add collision objects
1066  if (comp.components & moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY)
1067  getCollisionObjectMsgs(scene_msg.world.collision_objects);
1068  else if (comp.components & moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES)
1069  {
1070  const std::vector<std::string>& ns = world_->getObjectIds();
1071  scene_msg.world.collision_objects.clear();
1072  scene_msg.world.collision_objects.reserve(ns.size());
1073  for (std::size_t i = 0; i < ns.size(); ++i)
1074  if (ns[i] != OCTOMAP_NS)
1075  {
1076  moveit_msgs::CollisionObject co;
1077  co.id = ns[i];
1078  if (hasObjectType(co.id))
1079  co.type = getObjectType(co.id);
1080  scene_msg.world.collision_objects.push_back(co);
1081  }
1082  }
1083 
1084  // get the octomap
1085  if (comp.components & moveit_msgs::PlanningSceneComponents::OCTOMAP)
1086  getOctomapMsg(scene_msg.world.octomap);
1087 }
1088 
1089 void PlanningScene::saveGeometryToStream(std::ostream& out) const
1090 {
1091  out << name_ << std::endl;
1092  const std::vector<std::string>& ns = world_->getObjectIds();
1093  for (std::size_t i = 0; i < ns.size(); ++i)
1094  if (ns[i] != OCTOMAP_NS)
1095  {
1097  if (obj)
1098  {
1099  out << "* " << ns[i] << std::endl;
1100  out << obj->shapes_.size() << std::endl;
1101  for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
1102  {
1103  shapes::saveAsText(obj->shapes_[j].get(), out);
1104  out << obj->shape_poses_[j].translation().x() << " " << obj->shape_poses_[j].translation().y() << " "
1105  << obj->shape_poses_[j].translation().z() << std::endl;
1106  Eigen::Quaterniond r(obj->shape_poses_[j].rotation());
1107  out << r.x() << " " << r.y() << " " << r.z() << " " << r.w() << std::endl;
1108  if (hasObjectColor(ns[i]))
1109  {
1110  const std_msgs::ColorRGBA& c = getObjectColor(ns[i]);
1111  out << c.r << " " << c.g << " " << c.b << " " << c.a << std::endl;
1112  }
1113  else
1114  out << "0 0 0 0" << std::endl;
1115  }
1116  }
1117  }
1118  out << "." << std::endl;
1119 }
1120 
1122 {
1123  loadGeometryFromStream(in, Eigen::Affine3d::Identity()); // Use no offset
1124 }
1125 
1126 void PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Affine3d& offset)
1127 {
1128  if (!in.good() || in.eof())
1129  return;
1130  std::getline(in, name_);
1131  do
1132  {
1133  std::string marker;
1134  in >> marker;
1135  if (!in.good() || in.eof())
1136  return;
1137  if (marker == "*")
1138  {
1139  std::string ns;
1140  std::getline(in, ns);
1141  if (!in.good() || in.eof())
1142  return;
1143  boost::algorithm::trim(ns);
1144  unsigned int shape_count;
1145  in >> shape_count;
1146  for (std::size_t i = 0; i < shape_count && in.good() && !in.eof(); ++i)
1147  {
1149  double x, y, z, rx, ry, rz, rw;
1150  in >> x >> y >> z;
1151  in >> rx >> ry >> rz >> rw;
1152  float r, g, b, a;
1153  in >> r >> g >> b >> a;
1154  if (s)
1155  {
1156  Eigen::Affine3d pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz);
1157  // Transform pose by input pose offset
1158  pose = offset * pose;
1159  world_->addToObject(ns, shapes::ShapePtr(s), pose);
1160  if (r > 0.0f || g > 0.0f || b > 0.0f || a > 0.0f)
1161  {
1162  std_msgs::ColorRGBA color;
1163  color.r = r;
1164  color.g = g;
1165  color.b = b;
1166  color.a = a;
1167  setObjectColor(ns, color);
1168  }
1169  }
1170  }
1171  }
1172  else
1173  break;
1174  } while (true);
1175 }
1176 
1177 void PlanningScene::setCurrentState(const moveit_msgs::RobotState& state)
1178 {
1179  // The attached bodies will be processed separately by processAttachedCollisionObjectMsgs
1180  // after kstate_ has been updated
1181  moveit_msgs::RobotState state_no_attached(state);
1182  state_no_attached.attached_collision_objects.clear();
1183 
1184  if (parent_)
1185  {
1186  if (!kstate_)
1187  {
1188  kstate_.reset(new robot_state::RobotState(parent_->getCurrentState()));
1189  kstate_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1190  }
1192  }
1193  else
1194  robot_state::robotStateMsgToRobotState(*ftf_, state_no_attached, *kstate_);
1195 
1196  for (std::size_t i = 0; i < state.attached_collision_objects.size(); ++i)
1197  {
1198  if (!state.is_diff && state.attached_collision_objects[i].object.operation != moveit_msgs::CollisionObject::ADD)
1199  {
1200  ROS_ERROR_NAMED("planning_scene", "The specified RobotState is not marked as is_diff. "
1201  "The request to modify the object '%s' is not supported. Object is ignored.",
1202  state.attached_collision_objects[i].object.id.c_str());
1203  continue;
1204  }
1205  processAttachedCollisionObjectMsg(state.attached_collision_objects[i]);
1206  }
1207 }
1208 
1210 {
1211  getCurrentStateNonConst() = state;
1212 }
1213 
1215 {
1216  if (!parent_)
1217  return;
1218 
1219  if (!ftf_)
1220  {
1221  ftf_.reset(new SceneTransforms(this));
1222  ftf_->setAllTransforms(parent_->getTransforms().getAllTransforms());
1223  }
1224 
1225  if (!kstate_)
1226  {
1227  kstate_.reset(new robot_state::RobotState(parent_->getCurrentState()));
1228  kstate_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1229  }
1230 
1231  if (!acm_)
1232  acm_.reset(new collision_detection::AllowedCollisionMatrix(parent_->getAllowedCollisionMatrix()));
1233 
1234  for (CollisionDetectorIterator it = collision_.begin(); it != collision_.end(); ++it)
1235  {
1236  if (!it->second->crobot_)
1237  {
1238  it->second->crobot_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobot());
1239  it->second->crobot_const_ = it->second->crobot_;
1240  }
1241  if (!it->second->crobot_unpadded_)
1242  {
1243  it->second->crobot_unpadded_ =
1244  it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobotUnpadded());
1245  it->second->crobot_unpadded_const_ = it->second->crobot_unpadded_;
1246  }
1247  it->second->parent_.reset();
1248  }
1249  world_diff_.reset();
1250 
1251  if (!object_colors_)
1252  {
1253  ObjectColorMap kc;
1254  parent_->getKnownObjectColors(kc);
1255  object_colors_.reset(new ObjectColorMap(kc));
1256  }
1257  else
1258  {
1259  ObjectColorMap kc;
1260  parent_->getKnownObjectColors(kc);
1261  for (ObjectColorMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
1262  if (object_colors_->find(it->first) == object_colors_->end())
1263  (*object_colors_)[it->first] = it->second;
1264  }
1265 
1266  if (!object_types_)
1267  {
1268  ObjectTypeMap kc;
1269  parent_->getKnownObjectTypes(kc);
1270  object_types_.reset(new ObjectTypeMap(kc));
1271  }
1272  else
1273  {
1274  ObjectTypeMap kc;
1275  parent_->getKnownObjectTypes(kc);
1276  for (ObjectTypeMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
1277  if (object_types_->find(it->first) == object_types_->end())
1278  (*object_types_)[it->first] = it->second;
1279  }
1280 
1281  parent_.reset();
1282 }
1283 
1284 bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& scene_msg)
1285 {
1286  bool result = true;
1287 
1288  ROS_DEBUG_NAMED("planning_scene", "Adding planning scene diff");
1289  if (!scene_msg.name.empty())
1290  name_ = scene_msg.name;
1291 
1292  if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name != getRobotModel()->getName())
1293  ROS_WARN_NAMED("planning_scene", "Setting the scene for model '%s' but model '%s' is loaded.",
1294  scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str());
1295 
1296  // there is at least one transform in the list of fixed transform: from model frame to itself;
1297  // if the list is empty, then nothing has been set
1298  if (!scene_msg.fixed_frame_transforms.empty())
1299  {
1300  if (!ftf_)
1301  ftf_.reset(new SceneTransforms(this));
1302  ftf_->setTransforms(scene_msg.fixed_frame_transforms);
1303  }
1304 
1305  // if at least some joints have been specified, we set them
1306  if (!scene_msg.robot_state.multi_dof_joint_state.joint_names.empty() ||
1307  !scene_msg.robot_state.joint_state.name.empty() || !scene_msg.robot_state.attached_collision_objects.empty())
1308  setCurrentState(scene_msg.robot_state);
1309 
1310  // if at least some links are mentioned in the allowed collision matrix, then we have an update
1311  if (!scene_msg.allowed_collision_matrix.entry_names.empty())
1312  acm_.reset(new collision_detection::AllowedCollisionMatrix(scene_msg.allowed_collision_matrix));
1313 
1314  if (!scene_msg.link_padding.empty() || !scene_msg.link_scale.empty())
1315  {
1316  for (CollisionDetectorIterator it = collision_.begin(); it != collision_.end(); ++it)
1317  {
1318  if (!it->second->crobot_)
1319  {
1320  it->second->crobot_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobot());
1321  it->second->crobot_const_ = it->second->crobot_;
1322  }
1323  it->second->crobot_->setPadding(scene_msg.link_padding);
1324  it->second->crobot_->setScale(scene_msg.link_scale);
1325  }
1326  }
1327 
1328  // if any colors have been specified, replace the ones we have with the specified ones
1329  for (std::size_t i = 0; i < scene_msg.object_colors.size(); ++i)
1330  setObjectColor(scene_msg.object_colors[i].id, scene_msg.object_colors[i].color);
1331 
1332  // process collision object updates
1333  for (std::size_t i = 0; i < scene_msg.world.collision_objects.size(); ++i)
1334  result &= processCollisionObjectMsg(scene_msg.world.collision_objects[i]);
1335 
1336  // if an octomap was specified, replace the one we have with that one
1337  if (!scene_msg.world.octomap.octomap.data.empty())
1338  processOctomapMsg(scene_msg.world.octomap);
1339 
1340  return result;
1341 }
1342 
1343 bool PlanningScene::setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene_msg)
1344 {
1345  ROS_DEBUG_NAMED("planning_scene", "Setting new planning scene: '%s'", scene_msg.name.c_str());
1346  name_ = scene_msg.name;
1347 
1348  if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name != getRobotModel()->getName())
1349  ROS_WARN_NAMED("planning_scene", "Setting the scene for model '%s' but model '%s' is loaded.",
1350  scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str());
1351 
1352  if (parent_)
1353  decoupleParent();
1354 
1355  object_types_.reset();
1356  ftf_->setTransforms(scene_msg.fixed_frame_transforms);
1357  setCurrentState(scene_msg.robot_state);
1358  acm_.reset(new collision_detection::AllowedCollisionMatrix(scene_msg.allowed_collision_matrix));
1359  for (CollisionDetectorIterator it = collision_.begin(); it != collision_.end(); ++it)
1360  {
1361  if (!it->second->crobot_)
1362  {
1363  it->second->crobot_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobot());
1364  it->second->crobot_const_ = it->second->crobot_;
1365  }
1366  it->second->crobot_->setPadding(scene_msg.link_padding);
1367  it->second->crobot_->setScale(scene_msg.link_scale);
1368  }
1369  object_colors_.reset(new ObjectColorMap());
1370  for (std::size_t i = 0; i < scene_msg.object_colors.size(); ++i)
1371  setObjectColor(scene_msg.object_colors[i].id, scene_msg.object_colors[i].color);
1372  world_->clearObjects();
1373  return processPlanningSceneWorldMsg(scene_msg.world);
1374 }
1375 
1376 bool PlanningScene::processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld& world)
1377 {
1378  bool result = true;
1379  for (std::size_t i = 0; i < world.collision_objects.size(); ++i)
1380  result &= processCollisionObjectMsg(world.collision_objects[i]);
1381  processOctomapMsg(world.octomap);
1382  return result;
1383 }
1384 
1385 bool PlanningScene::usePlanningSceneMsg(const moveit_msgs::PlanningScene& scene_msg)
1386 {
1387  if (scene_msg.is_diff)
1388  return setPlanningSceneDiffMsg(scene_msg);
1389  else
1390  return setPlanningSceneMsg(scene_msg);
1391 }
1392 
1393 void PlanningScene::processOctomapMsg(const octomap_msgs::Octomap& map)
1394 {
1395  // each octomap replaces any previous one
1396  world_->removeObject(OCTOMAP_NS);
1397 
1398  if (map.data.empty())
1399  return;
1400 
1401  if (map.id != "OcTree")
1402  {
1403  ROS_ERROR_NAMED("planning_scene", "Received octomap is of type '%s' but type 'OcTree' is expected.",
1404  map.id.c_str());
1405  return;
1406  }
1407 
1408  std::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map)));
1409  if (!map.header.frame_id.empty())
1410  {
1411  const Eigen::Affine3d& t = getTransforms().getTransform(map.header.frame_id);
1412  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t);
1413  }
1414  else
1415  {
1416  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), Eigen::Affine3d::Identity());
1417  }
1418 }
1419 
1421 {
1422  const std::vector<std::string>& object_ids = world_->getObjectIds();
1423  for (std::size_t i = 0; i < object_ids.size(); ++i)
1424  if (object_ids[i] != OCTOMAP_NS)
1425  {
1426  world_->removeObject(object_ids[i]);
1427  removeObjectColor(object_ids[i]);
1428  removeObjectType(object_ids[i]);
1429  }
1430 }
1431 
1432 void PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose& map)
1433 {
1434  // each octomap replaces any previous one
1435  world_->removeObject(OCTOMAP_NS);
1436 
1437  if (map.octomap.data.empty())
1438  return;
1439 
1440  if (map.octomap.id != "OcTree")
1441  {
1442  ROS_ERROR_NAMED("planning_scene", "Received octomap is of type '%s' but type 'OcTree' is expected.",
1443  map.octomap.id.c_str());
1444  return;
1445  }
1446 
1447  std::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map.octomap)));
1448  const Eigen::Affine3d& t = getTransforms().getTransform(map.header.frame_id);
1449  Eigen::Affine3d p;
1450  tf::poseMsgToEigen(map.origin, p);
1451  p = t * p;
1452  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), p);
1453 }
1454 
1455 void PlanningScene::processOctomapPtr(const std::shared_ptr<const octomap::OcTree>& octree, const Eigen::Affine3d& t)
1456 {
1458  if (map)
1459  {
1460  if (map->shapes_.size() == 1)
1461  {
1462  // check to see if we have the same octree pointer & pose.
1463  const shapes::OcTree* o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
1464  if (o->octree == octree)
1465  {
1466  // if the pose changed, we update it
1467  if (map->shape_poses_[0].isApprox(t, std::numeric_limits<double>::epsilon() * 100.0))
1468  {
1469  if (world_diff_)
1472  }
1473  else
1474  {
1475  shapes::ShapeConstPtr shape = map->shapes_[0];
1476  map.reset(); // reset this pointer first so that caching optimizations can be used in CollisionWorld
1477  world_->moveShapeInObject(OCTOMAP_NS, shape, t);
1478  }
1479  return;
1480  }
1481  }
1482  }
1483  // if the octree pointer changed, update the structure
1484  world_->removeObject(OCTOMAP_NS);
1485  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(octree)), t);
1486 }
1487 
1488 bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject& object)
1489 {
1490  if (object.object.operation == moveit_msgs::CollisionObject::ADD && !getRobotModel()->hasLinkModel(object.link_name))
1491  {
1492  ROS_ERROR_NAMED("planning_scene", "Unable to attach a body to link '%s' (link not found)",
1493  object.link_name.c_str());
1494  return false;
1495  }
1496 
1497  if (object.object.id == OCTOMAP_NS)
1498  {
1499  ROS_ERROR_NAMED("planning_scene", "The ID '%s' cannot be used for collision objects (name reserved)",
1500  OCTOMAP_NS.c_str());
1501  return false;
1502  }
1503 
1504  if (!kstate_) // there must be a parent in this case
1505  {
1506  kstate_.reset(new robot_state::RobotState(parent_->getCurrentState()));
1507  kstate_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1508  }
1509  kstate_->update();
1510 
1511  if (object.object.operation == moveit_msgs::CollisionObject::ADD ||
1512  object.object.operation == moveit_msgs::CollisionObject::APPEND)
1513  {
1514  if (object.object.primitives.size() != object.object.primitive_poses.size())
1515  {
1516  ROS_ERROR_NAMED("planning_scene", "Number of primitive shapes does not match number of poses "
1517  "in attached collision object message");
1518  return false;
1519  }
1520 
1521  if (object.object.meshes.size() != object.object.mesh_poses.size())
1522  {
1523  ROS_ERROR_NAMED("planning_scene", "Number of meshes does not match number of poses "
1524  "in attached collision object message");
1525  return false;
1526  }
1527 
1528  if (object.object.planes.size() != object.object.plane_poses.size())
1529  {
1530  ROS_ERROR_NAMED("planning_scene", "Number of planes does not match number of poses "
1531  "in attached collision object message");
1532  return false;
1533  }
1534 
1535  const robot_model::LinkModel* lm = getRobotModel()->getLinkModel(object.link_name);
1536  if (lm)
1537  {
1538  std::vector<shapes::ShapeConstPtr> shapes;
1540 
1541  // we need to add some shapes; if the message is empty, maybe the object is already in the world
1542  if (object.object.operation == moveit_msgs::CollisionObject::ADD && object.object.primitives.empty() &&
1543  object.object.meshes.empty() && object.object.planes.empty())
1544  {
1545  collision_detection::CollisionWorld::ObjectConstPtr obj = world_->getObject(object.object.id);
1546  if (obj)
1547  {
1548  ROS_DEBUG_NAMED("planning_scene", "Attaching world object '%s' to link '%s'", object.object.id.c_str(),
1549  object.link_name.c_str());
1550 
1551  // extract the shapes from the world
1552  shapes = obj->shapes_;
1553  poses = obj->shape_poses_;
1554  // remove the pointer to the objects from the collision world
1555  world_->removeObject(object.object.id);
1556 
1557  // need to transform poses to the link frame
1558  const Eigen::Affine3d& i_t = kstate_->getGlobalLinkTransform(lm).inverse();
1559  for (std::size_t i = 0; i < poses.size(); ++i)
1560  poses[i] = i_t * poses[i];
1561  }
1562  else
1563  {
1564  ROS_ERROR_NAMED("planning_scene", "Attempting to attach object '%s' to link '%s' but no geometry specified "
1565  "and such an object does not exist in the collision world",
1566  object.object.id.c_str(), object.link_name.c_str());
1567  return false;
1568  }
1569  }
1570  else
1571  {
1572  // we clear the world objects with the same name, since we got an update on their geometry
1573  if (world_->removeObject(object.object.id))
1574  {
1575  if (object.object.operation == moveit_msgs::CollisionObject::ADD)
1576  ROS_DEBUG_NAMED("planning_scene", "Removing world object with the same name as newly attached object: '%s'",
1577  object.object.id.c_str());
1578  else
1579  ROS_WARN_NAMED("planning_scene",
1580  "You tried to append geometry to an attached object that is actually a world object ('%s'). "
1581  "World geometry is ignored.",
1582  object.object.id.c_str());
1583  }
1584 
1585  for (std::size_t i = 0; i < object.object.primitives.size(); ++i)
1586  {
1587  shapes::Shape* s = shapes::constructShapeFromMsg(object.object.primitives[i]);
1588  if (s)
1589  {
1590  Eigen::Affine3d p;
1591  tf::poseMsgToEigen(object.object.primitive_poses[i], p);
1592  shapes.push_back(shapes::ShapeConstPtr(s));
1593  poses.push_back(p);
1594  }
1595  }
1596  for (std::size_t i = 0; i < object.object.meshes.size(); ++i)
1597  {
1598  shapes::Shape* s = shapes::constructShapeFromMsg(object.object.meshes[i]);
1599  if (s)
1600  {
1601  Eigen::Affine3d p;
1602  tf::poseMsgToEigen(object.object.mesh_poses[i], p);
1603  shapes.push_back(shapes::ShapeConstPtr(s));
1604  poses.push_back(p);
1605  }
1606  }
1607  for (std::size_t i = 0; i < object.object.planes.size(); ++i)
1608  {
1609  shapes::Shape* s = shapes::constructShapeFromMsg(object.object.planes[i]);
1610  if (s)
1611  {
1612  Eigen::Affine3d p;
1613  tf::poseMsgToEigen(object.object.plane_poses[i], p);
1614  shapes.push_back(shapes::ShapeConstPtr(s));
1615  poses.push_back(p);
1616  }
1617  }
1618 
1619  // transform poses to link frame
1620  if (object.object.header.frame_id != object.link_name)
1621  {
1622  const Eigen::Affine3d& t = kstate_->getGlobalLinkTransform(lm).inverse() *
1623  getTransforms().getTransform(object.object.header.frame_id);
1624  for (std::size_t i = 0; i < poses.size(); ++i)
1625  poses[i] = t * poses[i];
1626  }
1627  }
1628 
1629  if (shapes.empty())
1630  {
1631  ROS_ERROR_NAMED("planning_scene", "There is no geometry to attach to link '%s' as part of attached body '%s'",
1632  object.link_name.c_str(), object.object.id.c_str());
1633  return false;
1634  }
1635 
1636  if (!object.object.type.db.empty() || !object.object.type.key.empty())
1637  setObjectType(object.object.id, object.object.type);
1638 
1639  if (object.object.operation == moveit_msgs::CollisionObject::ADD || !kstate_->hasAttachedBody(object.object.id))
1640  {
1641  // there should not exist an attached object with this name
1642  if (kstate_->clearAttachedBody(object.object.id))
1643  ROS_DEBUG_NAMED("planning_scene", "The robot state already had an object named '%s' attached to link '%s'. "
1644  "The object was replaced.",
1645  object.object.id.c_str(), object.link_name.c_str());
1646  kstate_->attachBody(object.object.id, shapes, poses, object.touch_links, object.link_name,
1647  object.detach_posture);
1648  ROS_DEBUG_NAMED("planning_scene", "Attached object '%s' to link '%s'", object.object.id.c_str(),
1649  object.link_name.c_str());
1650  }
1651  else
1652  {
1653  const robot_state::AttachedBody* ab = kstate_->getAttachedBody(object.object.id);
1654  shapes.insert(shapes.end(), ab->getShapes().begin(), ab->getShapes().end());
1655  poses.insert(poses.end(), ab->getFixedTransforms().begin(), ab->getFixedTransforms().end());
1656  trajectory_msgs::JointTrajectory detach_posture =
1657  object.detach_posture.joint_names.empty() ? ab->getDetachPosture() : object.detach_posture;
1658  std::set<std::string> ab_touch_links = ab->getTouchLinks();
1659  kstate_->clearAttachedBody(object.object.id);
1660  if (object.touch_links.empty())
1661  kstate_->attachBody(object.object.id, shapes, poses, ab_touch_links, object.link_name, detach_posture);
1662  else
1663  kstate_->attachBody(object.object.id, shapes, poses, object.touch_links, object.link_name, detach_posture);
1664  ROS_DEBUG_NAMED("planning_scene", "Added shapes to object '%s' attached to link '%s'", object.object.id.c_str(),
1665  object.link_name.c_str());
1666  }
1667 
1668  return true;
1669  }
1670  else
1671  ROS_ERROR_NAMED("planning_scene", "Robot state is not compatible with robot model. This could be fatal.");
1672  }
1673  else if (object.object.operation == moveit_msgs::CollisionObject::REMOVE)
1674  {
1675  std::vector<const robot_state::AttachedBody*> attached_bodies;
1676  if (object.link_name.empty())
1677  {
1678  if (object.object.id.empty())
1679  kstate_->getAttachedBodies(attached_bodies);
1680  else
1681  {
1682  const robot_state::AttachedBody* ab = kstate_->getAttachedBody(object.object.id);
1683  if (ab)
1684  attached_bodies.push_back(ab);
1685  }
1686  }
1687  else
1688  {
1689  const robot_model::LinkModel* lm = getRobotModel()->getLinkModel(object.link_name);
1690  if (lm)
1691  {
1692  if (object.object.id.empty()) // if no specific object id is given, then we remove all objects attached to the
1693  // link_name
1694  {
1695  kstate_->getAttachedBodies(attached_bodies, lm);
1696  }
1697  else // a specific object id will be removed
1698  {
1699  const robot_state::AttachedBody* ab = kstate_->getAttachedBody(object.object.id);
1700  if (ab)
1701  attached_bodies.push_back(ab);
1702  }
1703  }
1704  }
1705 
1706  for (std::size_t i = 0; i < attached_bodies.size(); ++i)
1707  {
1708  std::vector<shapes::ShapeConstPtr> shapes = attached_bodies[i]->getShapes();
1709  EigenSTL::vector_Affine3d poses = attached_bodies[i]->getGlobalCollisionBodyTransforms();
1710  std::string name = attached_bodies[i]->getName();
1711 
1712  kstate_->clearAttachedBody(name);
1713 
1714  if (world_->hasObject(name))
1715  ROS_WARN_NAMED("planning_scene",
1716  "The collision world already has an object with the same name as the body about to be detached. "
1717  "NOT adding the detached body '%s' to the collision world.",
1718  object.object.id.c_str());
1719  else
1720  {
1721  world_->addToObject(name, shapes, poses);
1722  ROS_DEBUG_NAMED("planning_scene",
1723  "Detached object '%s' from link '%s' and added it back in the collision world", name.c_str(),
1724  object.link_name.c_str());
1725  }
1726  }
1727  if (!attached_bodies.empty() || object.object.id.empty())
1728  return true;
1729  }
1730  else if (object.object.operation == moveit_msgs::CollisionObject::MOVE)
1731  {
1732  ROS_ERROR_NAMED("planning_scene", "Move for attached objects not yet implemented");
1733  }
1734  else
1735  {
1736  ROS_ERROR_NAMED("planning_scene", "Unknown collision object operation: %d", object.object.operation);
1737  }
1738 
1739  return false;
1740 }
1741 
1742 bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject& object)
1743 {
1744  if (object.id == OCTOMAP_NS)
1745  {
1746  ROS_ERROR_NAMED("planning_scene", "The ID '%s' cannot be used for collision objects (name reserved)",
1747  OCTOMAP_NS.c_str());
1748  return false;
1749  }
1750 
1751  if (object.operation == moveit_msgs::CollisionObject::ADD || object.operation == moveit_msgs::CollisionObject::APPEND)
1752  {
1753  if (object.primitives.empty() && object.meshes.empty() && object.planes.empty())
1754  {
1755  ROS_ERROR_NAMED("planning_scene", "There are no shapes specified in the collision object message");
1756  return false;
1757  }
1758 
1759  if (object.primitives.size() != object.primitive_poses.size())
1760  {
1761  ROS_ERROR_NAMED("planning_scene", "Number of primitive shapes does not match number of poses "
1762  "in collision object message");
1763  return false;
1764  }
1765 
1766  if (object.meshes.size() != object.mesh_poses.size())
1767  {
1768  ROS_ERROR_NAMED("planning_scene", "Number of meshes does not match number of poses in collision object message");
1769  return false;
1770  }
1771 
1772  if (object.planes.size() != object.plane_poses.size())
1773  {
1774  ROS_ERROR_NAMED("planning_scene", "Number of planes does not match number of poses in collision object message");
1775  return false;
1776  }
1777 
1778  // replace the object if ADD is specified instead of APPEND
1779  if (object.operation == moveit_msgs::CollisionObject::ADD && world_->hasObject(object.id))
1780  world_->removeObject(object.id);
1781 
1782  const Eigen::Affine3d& t = getTransforms().getTransform(object.header.frame_id);
1783 
1784  for (std::size_t i = 0; i < object.primitives.size(); ++i)
1785  {
1786  shapes::Shape* s = shapes::constructShapeFromMsg(object.primitives[i]);
1787  if (s)
1788  {
1789  Eigen::Affine3d p;
1790  tf::poseMsgToEigen(object.primitive_poses[i], p);
1791  world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
1792  }
1793  }
1794  for (std::size_t i = 0; i < object.meshes.size(); ++i)
1795  {
1796  shapes::Shape* s = shapes::constructShapeFromMsg(object.meshes[i]);
1797  if (s)
1798  {
1799  Eigen::Affine3d p;
1800  tf::poseMsgToEigen(object.mesh_poses[i], p);
1801  world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
1802  }
1803  }
1804  for (std::size_t i = 0; i < object.planes.size(); ++i)
1805  {
1806  shapes::Shape* s = shapes::constructShapeFromMsg(object.planes[i]);
1807  if (s)
1808  {
1809  Eigen::Affine3d p;
1810  tf::poseMsgToEigen(object.plane_poses[i], p);
1811  world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
1812  }
1813  }
1814  if (!object.type.key.empty() || !object.type.db.empty())
1815  setObjectType(object.id, object.type);
1816  return true;
1817  }
1818  else if (object.operation == moveit_msgs::CollisionObject::REMOVE)
1819  {
1820  if (object.id.empty())
1821  {
1823  }
1824  else
1825  {
1826  world_->removeObject(object.id);
1827  removeObjectColor(object.id);
1828  removeObjectType(object.id);
1829  }
1830  return true;
1831  }
1832  else if (object.operation == moveit_msgs::CollisionObject::MOVE)
1833  {
1834  if (world_->hasObject(object.id))
1835  {
1836  if (!object.primitives.empty() || !object.meshes.empty() || !object.planes.empty())
1837  ROS_WARN_NAMED("planning_scene",
1838  "Move operation for object '%s' ignores the geometry specified in the message.",
1839  object.id.c_str());
1840 
1841  const Eigen::Affine3d& t = getTransforms().getTransform(object.header.frame_id);
1842  EigenSTL::vector_Affine3d new_poses;
1843  for (std::size_t i = 0; i < object.primitive_poses.size(); ++i)
1844  {
1845  Eigen::Affine3d p;
1846  tf::poseMsgToEigen(object.primitive_poses[i], p);
1847  new_poses.push_back(t * p);
1848  }
1849  for (std::size_t i = 0; i < object.mesh_poses.size(); ++i)
1850  {
1851  Eigen::Affine3d p;
1852  tf::poseMsgToEigen(object.mesh_poses[i], p);
1853  new_poses.push_back(t * p);
1854  }
1855  for (std::size_t i = 0; i < object.plane_poses.size(); ++i)
1856  {
1857  Eigen::Affine3d p;
1858  tf::poseMsgToEigen(object.plane_poses[i], p);
1859  new_poses.push_back(t * p);
1860  }
1861 
1862  collision_detection::World::ObjectConstPtr obj = world_->getObject(object.id);
1863  if (obj->shapes_.size() == new_poses.size())
1864  {
1865  std::vector<shapes::ShapeConstPtr> shapes = obj->shapes_;
1866  obj.reset();
1867  world_->removeObject(object.id);
1868  world_->addToObject(object.id, shapes, new_poses);
1869  }
1870  else
1871  {
1872  ROS_ERROR_NAMED("planning_scene",
1873  "Number of supplied poses (%zu) for object '%s' does not match number of shapes (%zu). "
1874  "Not moving.",
1875  new_poses.size(), object.id.c_str(), obj->shapes_.size());
1876  return false;
1877  }
1878  return true;
1879  }
1880  else
1881  ROS_ERROR_NAMED("planning_scene", "World object '%s' does not exist. Cannot move.", object.id.c_str());
1882  }
1883  else
1884  ROS_ERROR_NAMED("planning_scene", "Unknown collision object operation: %d", object.operation);
1885  return false;
1886 }
1887 
1888 const Eigen::Affine3d& PlanningScene::getFrameTransform(const std::string& id) const
1889 {
1890  return getFrameTransform(getCurrentState(), id);
1891 }
1892 
1893 const Eigen::Affine3d& PlanningScene::getFrameTransform(const std::string& id)
1894 {
1895  if (getCurrentState().dirtyLinkTransforms())
1897  else
1898  return getFrameTransform(getCurrentState(), id);
1899 }
1900 
1901 const Eigen::Affine3d& PlanningScene::getFrameTransform(const robot_state::RobotState& state,
1902  const std::string& id) const
1903 {
1904  if (!id.empty() && id[0] == '/')
1905  return getFrameTransform(id.substr(1));
1906  if (state.knowsFrameTransform(id))
1907  return state.getFrameTransform(id);
1908  if (getWorld()->hasObject(id))
1909  {
1910  collision_detection::World::ObjectConstPtr obj = getWorld()->getObject(id);
1911  if (obj->shape_poses_.size() > 1)
1912  {
1913  ROS_WARN_NAMED("planning_scene", "More than one shapes in object '%s'. Using first one to decide transform",
1914  id.c_str());
1915  return obj->shape_poses_[0];
1916  }
1917  else if (obj->shape_poses_.size() == 1)
1918  return obj->shape_poses_[0];
1919  }
1920  return getTransforms().Transforms::getTransform(id);
1921 }
1922 
1923 bool PlanningScene::knowsFrameTransform(const std::string& id) const
1924 {
1925  return knowsFrameTransform(getCurrentState(), id);
1926 }
1927 
1928 bool PlanningScene::knowsFrameTransform(const robot_state::RobotState& state, const std::string& id) const
1929 {
1930  if (!id.empty() && id[0] == '/')
1931  return knowsFrameTransform(id.substr(1));
1932  if (state.knowsFrameTransform(id))
1933  return true;
1934  if (getWorld()->hasObject(id))
1935  {
1936  collision_detection::World::ObjectConstPtr obj = getWorld()->getObject(id);
1937  return obj->shape_poses_.size() == 1;
1938  }
1939  return getTransforms().Transforms::canTransform(id);
1940 }
1941 
1942 bool PlanningScene::hasObjectType(const std::string& id) const
1943 {
1944  if (object_types_)
1945  if (object_types_->find(id) != object_types_->end())
1946  return true;
1947  if (parent_)
1948  return parent_->hasObjectType(id);
1949  return false;
1950 }
1951 
1952 const object_recognition_msgs::ObjectType& PlanningScene::getObjectType(const std::string& id) const
1953 {
1954  if (object_types_)
1955  {
1956  ObjectTypeMap::const_iterator it = object_types_->find(id);
1957  if (it != object_types_->end())
1958  return it->second;
1959  }
1960  if (parent_)
1961  return parent_->getObjectType(id);
1962  static const object_recognition_msgs::ObjectType empty;
1963  return empty;
1964 }
1965 
1966 void PlanningScene::setObjectType(const std::string& id, const object_recognition_msgs::ObjectType& type)
1967 {
1968  if (!object_types_)
1969  object_types_.reset(new ObjectTypeMap());
1970  (*object_types_)[id] = type;
1971 }
1972 
1973 void PlanningScene::removeObjectType(const std::string& id)
1974 {
1975  if (object_types_)
1976  object_types_->erase(id);
1977 }
1978 
1980 {
1981  kc.clear();
1982  if (parent_)
1983  parent_->getKnownObjectTypes(kc);
1984  if (object_types_)
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& id) const
1990 {
1991  if (object_colors_)
1992  if (object_colors_->find(id) != object_colors_->end())
1993  return true;
1994  if (parent_)
1995  return parent_->hasObjectColor(id);
1996  return false;
1997 }
1998 
1999 const std_msgs::ColorRGBA& PlanningScene::getObjectColor(const std::string& id) const
2000 {
2001  if (object_colors_)
2002  {
2003  ObjectColorMap::const_iterator it = object_colors_->find(id);
2004  if (it != object_colors_->end())
2005  return it->second;
2006  }
2007  if (parent_)
2008  return parent_->getObjectColor(id);
2009  static const std_msgs::ColorRGBA empty;
2010  return empty;
2011 }
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;
2021 }
2022 
2023 void PlanningScene::setObjectColor(const std::string& id, const std_msgs::ColorRGBA& color)
2024 {
2025  if (id.empty())
2026  {
2027  ROS_ERROR_NAMED("planning_scene", "Cannot set color of object with empty id.");
2028  return;
2029  }
2030  if (!object_colors_)
2031  object_colors_.reset(new ObjectColorMap());
2032  (*object_colors_)[id] = color;
2033 }
2034 
2035 void PlanningScene::removeObjectColor(const std::string& id)
2036 {
2037  if (object_colors_)
2038  object_colors_->erase(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 }
2055 
2056 bool PlanningScene::isStateColliding(const robot_state::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
2067 {
2068  if (state_feasibility_)
2069  {
2072  return state_feasibility_(s, verbose);
2073  }
2074  return true;
2075 }
2076 
2077 bool PlanningScene::isStateFeasible(const robot_state::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 robot_state::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;
2116 }
2117 
2118 bool PlanningScene::isStateValid(const robot_state::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);
2136 }
2137 
2138 bool PlanningScene::isStateValid(const robot_state::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
2162 {
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 robot_state::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 (std::size_t k = 0; k < goal_constraints.size(); ++k)
2237  {
2238  if (isStateConstrained(st, goal_constraints[k]))
2239  {
2240  found = true;
2241  break;
2242  }
2243  }
2244  if (!found)
2245  {
2246  if (verbose)
2247  ROS_INFO_NAMED("planning_scene", "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 
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 
2283  std::set<collision_detection::CostSource>& costs, double overlap_fraction) const
2284 {
2285  getCostSources(trajectory, max_costs, std::string(), costs, overlap_fraction);
2286 }
2287 
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);
2320 }
2321 
2322 void PlanningScene::getCostSources(const robot_state::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 robot_state::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 
2345  out << "Collision World Objects:\n\t ";
2346  std::copy(objects.begin(), objects.end(), std::ostream_iterator<std::string>(out, "\n\t "));
2347 
2348  std::vector<const robot_state::AttachedBody*> attached_bodies;
2349  getCurrentState().getAttachedBodies(attached_bodies);
2350 
2351  out << "\nAttached Bodies:\n";
2352  for (std::size_t i = 0; i < attached_bodies.size(); ++i)
2353  {
2354  out << "\t " << attached_bodies[i]->getName() << "\n";
2355  }
2356 }
2357 
2358 } // end of namespace planning_scene
bool empty() const
Returns whether or not there are any constraints in the set.
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=NULL) const
Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibili...
const collision_detection::CollisionRobotConstPtr & getCollisionRobot() const
Get the active collision detector for the robot.
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
Maintain a diff list of changes that have happened to a World.
Definition: world_diff.h:50
std::map< std::string, World::Action >::const_iterator const_iterator
Definition: world_diff.h:82
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...
This may be thrown during construction of objects if errors occur.
Definition: exceptions.h:46
Core components of MoveIt!
bool isEmpty(const moveit_msgs::Constraints &constr)
Check if any constraints were specified.
Definition: utils.cpp:111
void attachedBodiesToAttachedCollisionObjectMsgs(const std::vector< const AttachedBody * > &attached_bodies, std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objs)
Convert AttachedBodies to AttachedCollisionObjects.
void copyTransforms(std::vector< geometry_msgs::TransformStamped > &transforms) const
Get a vector of all the transforms as ROS messages.
Definition: transforms.cpp:165
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...
robot_state::RobotState & getCurrentStateNonConst()
Get the state at which the robot is assumed to be.
robot_state::TransformsPtr ftf_
#define ROS_INFO_NAMED(name,...)
void setObjectType(const std::string &id, const object_recognition_msgs::ObjectType &type)
void decoupleParent()
Make sure that all the data maintained in this scene is local. All unmodified data is copied from the...
void processOctomapMsg(const octomap_msgs::OctomapWithPose &map)
ROSCPP_DECL void start()
bool hasObjectType(const std::string &id) const
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
const collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrix() const
Get the allowed collision matrix.
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...
#define ROS_WARN_NAMED(name,...)
void getMessage(moveit_msgs::AllowedCollisionMatrix &msg) const
Get the allowed collision matrix as a message.
f
bool knowsFrameTransform(const std::string &id) const
Check if a transformation matrix from the model frame to frame id is known.
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.
void removeObjectColor(const std::string &id)
robot_state::RobotStatePtr kstate_
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
StateFeasibilityFn state_feasibility_
PlanningScene(const robot_model::RobotModelConstPtr &robot_model, collision_detection::WorldPtr world=collision_detection::WorldPtr(new collision_detection::World()))
construct using an existing RobotModel
bool canTransform(const std::string &from_frame) const override
Check whether data can be transformed from a particular frame.
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
std::vector< shapes::ShapeConstPtr > shapes_
All the shapes making up this object.
Definition: world.h:102
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in self collision.
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...
static const std::string DEFAULT_SCENE_NAME
Maintain a representation of the environment.
Definition: world.h:60
XmlRpcServer s
static bool fullMapToMsg(const OctomapT &octomap, Octomap &msg)
const Eigen::Affine3d & getFrameTransform(const std::string &id) const
Get the transform corresponding to the frame id. This will be known if id is a link name...
ContactMap contacts
A map returning the pairs of ids of the bodies in contact, plus information about the contacts themse...
const Eigen::Affine3d & getTransform(const std::string &from_frame) const override
Get transform for from_frame (w.r.t target frame)
static octomap::AbstractOcTree * msgToMap(const Octomap &msg)
bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &object)
A class that contains many different constraints, and can check RobotState *versus the full set...
robot_model::RobotModelConstPtr kmodel_
bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world)
const Eigen::Affine3d & getFrameTransform(const std::string &id)
Get the transformation matrix from the model frame to the frame identified by id. ...
const PlanningScene * scene_
std_msgs::Header * header(M &m)
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...
void removeObjectType(const std::string &id)
const collision_detection::CollisionRobotPtr & getCollisionRobotNonConst()
Get the representation of the collision robot This can be used to set padding and link scale on the a...
std::map< std::string, CollisionDetectorPtr >::const_iterator CollisionDetectorConstIterator
void setObjectColor(const std::string &id, const std_msgs::ColorRGBA &color)
static const std::string OCTOMAP_NS
bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &object)
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:61
std::shared_ptr< Shape > ShapePtr
const std_msgs::ColorRGBA & getObjectColor(const std::string &id) const
void copyPadding(const CollisionDetector &src)
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
collision_detection::AllowedCollisionMatrixPtr acm_
static robot_model::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr &urdf_model, const srdf::ModelConstSharedPtr &srdf_model)
double y
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string id_
The id for this object.
Definition: world.h:89
void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn &callback)
Set the callback to be triggered when changes are made to the current scene world.
bool collision
True if collision was found, false otherwise.
void printKnownObjects(std::ostream &out) const
Outputs debug information about the planning scene contents.
collision_detection::WorldConstPtr world_const_
static PlanningScenePtr clone(const PlanningSceneConstPtr &scene)
Clone a planning scene. Even if the scene scene depends on a parent, the cloned scene will not...
std::unique_ptr< ObjectColorMap > object_colors_
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
std::set< CostSource > cost_sources
When costs are computed, the individual cost sources are.
bool add(const moveit_msgs::Constraints &c, const robot_state::Transforms &tf)
Add all known constraints.
PlanningScenePtr diff() const
Return a new child PlanningScene that uses this one as parent.
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
Definition: attached_body.h:88
virtual const Eigen::Affine3d & getTransform(const std::string &from_frame) const
Get transform for from_frame (w.r.t target frame)
Definition: transforms.cpp:100
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
Definition: world.h:232
std::map< std::string, object_recognition_msgs::ObjectType > ObjectTypeMap
A map from object names (e.g., attached bodies, collision objects) to their types.
World::ObjectConstPtr ObjectConstPtr
bool satisfied
Whether or not the constraint or constraints were satisfied.
#define ROS_DEBUG_NAMED(name,...)
std::shared_ptr< const octomap::OcTree > octree
std::unique_ptr< ObjectTypeMap > object_types_
robot_state::Transforms & getTransformsNonConst()
Get the set of fixed transforms from known frames to the planning frame.
std::size_t max_contacts
Overall maximum number of contacts to compute.
A representation of an object.
Definition: world.h:80
void getKnownObjectColors(ObjectColorMap &kc) const
Transforms(const std::string &target_frame)
Construct a transform list.
Definition: transforms.cpp:46
bool verbose
Flag indicating whether information about detected collisions should be reported. ...
const EigenSTL::vector_Affine3d & getFixedTransforms() const
Get the fixed transform (the transforms to the shapes associated with this body)
void update(bool force=false)
Update all transforms.
robot_state::AttachedBodyCallback current_state_attached_body_callback_
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Definition: attached_body.h:50
This class maintains the representation of the environment as seen by a planning instance. The environment geometry, the robot geometry and state are maintained.
void findParent(const PlanningScene &scene)
const robot_model::RobotModelConstPtr & getRobotModel() const
Get the kinematic model for which the planning scene is maintained.
void getCollidingLinks(std::vector< std::string > &links)
Get the names of the links that are involved in collisions for the current state. ...
SceneTransforms(const PlanningScene *scene)
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 of a structure for the allowed collision matrix. All elements in the collision world are r...
const robot_state::Transforms & getTransforms() const
Get the set of fixed transforms from known frames to the planning frame.
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
void propogateRobotPadding()
Copy scale and padding from active CollisionRobot to other CollisionRobots. This should be called aft...
std::map< std::string, std_msgs::ColorRGBA > ObjectColorMap
A map from object names (e.g., attached bodies, collision objects) to their colors.
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...
const std::set< std::string > & getTouchLinks() const
Get the links that the attached body is allowed to touch.
Definition: attached_body.h:94
const robot_state::RobotState & getCurrentState() const
Get the state at which the robot is assumed to be.
void setAttachedBodyUpdateCallback(const robot_state::AttachedBodyCallback &callback)
Set the callback to be triggered when changes are made to the current scene state.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
Maintain a sequence of waypoints and the time durations between these waypoints.
double z
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...
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...
void saveAsText(const Shape *shape, std::ostream &out)
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...
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...
void removeOverlapping(std::set< CostSource > &cost_sources, double overlap_fraction)
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...
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...
const std::string & getPlanningFrame() const
Get the frame in which planning is performed.
const collision_detection::CollisionRobotConstPtr & getCollisionRobotUnpadded() const
Get the active collision detector for the robot.
Shape * constructShapeFromMsg(const shape_msgs::SolidPrimitive &shape_msg)
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...
void removeCostSources(std::set< CostSource > &cost_sources, const std::set< CostSource > &cost_sources_to_remove, double overlap_fraction)
This namespace includes the central class for representing planning contexts.
void getKnownObjectTypes(ObjectTypeMap &kc) const
bool knowsObject(const std::string &id) const
collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrixNonConst()
Get the allowed collision matrix.
moveit_msgs::CollisionObject * obj_
const robot_state::RobotState & getWayPoint(std::size_t index) const
void removeAllCollisionObjects()
Clear all collision objects in planning scene.
CollisionDetectorPtr active_collision_
std::size_t getWayPointCount() const
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...
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.
collision_detection::World::ObserverCallbackFn current_world_object_update_callback_
bool constructMsgFromShape(const Shape *shape, ShapeMsg &shape_msg)
void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr &allocator)
Add a new collision detector type.
collision_detection::WorldDiffPtr world_diff_
std::map< std::string, CollisionDetectorPtr > collision_
void pushDiffs(const PlanningScenePtr &scene)
If there is a parent specified for this scene, then the diffs with respect to that parent are applied...
const geometry_msgs::Pose * pose_
bool hasObjectColor(const std::string &id) const
const object_recognition_msgs::ObjectType & getObjectType(const std::string &id) const
std::size_t max_cost_sources
When costs are computed, this value defines how many of the top cost sources should be returned...
robot_state::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState &update) const
Get a copy of the current state with components overwritten by the state message update.
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.
bool usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene)
Call setPlanningSceneMsg() or setPlanningSceneDiffMsg() depending on how the is_diff member of the me...
void clearDiffs()
Clear the diffs accumulated for this planning scene, with respect to the parent. This function is a n...
collision_detection::World::ObserverHandle current_world_object_update_observer_handle_
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...
void processOctomapPtr(const std::shared_ptr< const octomap::OcTree > &octree, const Eigen::Affine3d &t)
std::map< std::string, CollisionDetectorPtr >::iterator CollisionDetectorIterator
#define ROS_ERROR_NAMED(name,...)
collision_detection::WorldPtr world_
const void addCollisionDetectorConst(const collision_detection::CollisionDetectorAllocatorPtr &allocator) const
Add a new collision detector type.
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 ...
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
Shape * constructShapeFromText(std::istream &in)
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...
const void setActiveCollisionDetectorConst(const collision_detection::CollisionDetectorAllocatorPtr &allocator, bool exclusive=false) const
Set the type of collision detector to use. Calls addCollisionDetector_ConstVersion() to add it if it ...
void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene &scene) const
Fill the message scene with the differences between this instance of PlanningScene with respect to th...
const collision_detection::CollisionRobotConstPtr & getCollisionRobot() const
EigenSTL::vector_Affine3d shape_poses_
The poses of the corresponding entries in shapes_.
Definition: world.h:107
void loadGeometryFromStream(std::istream &in)
Load the geometry of the planning scene from a stream.
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
double x
r
bool getOctomapMsg(octomap_msgs::OctomapWithPose &octomap) const
Construct a message (octomap) with the octomap data from the planning_scene.
void setRobotTrajectoryMsg(const robot_state::RobotState &reference_state, const trajectory_msgs::JointTrajectory &trajectory)
Copy the content of the trajectory message into this class. The trajectory message itself is not requ...
PlanningSceneConstPtr parent_
void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene) const
Construct a message (scene) with all the necessary data so that the scene can be later reconstructed ...
boost::variant< shape_msgs::SolidPrimitive, shape_msgs::Mesh, shape_msgs::Plane > ShapeMsg
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result...
void saveGeometryToStream(std::ostream &out) const
Save the geometry of the planning scene to a stream, as plain text.
bool isStateColliding(const std::string &group="", bool verbose=false)
Check if the current state is in collision (with the environment or self collision). If a group name is specified, collision checking is done for that group only. Since the function is non-const, the current state transforms are updated before the collision check.
const collision_detection::CollisionWorldConstPtr & getCollisionWorld() const
Get the active collision detector for the world.
const collision_detection::WorldConstPtr & getWorld() const
Get the representation of the world.
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...
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...
std::shared_ptr< const Shape > ShapeConstPtr
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...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Oct 15 2018 03:44:47