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