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


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