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  CONSOLE_BRIDGE_logError("Cannot setActiveCollisionDetector to '%s' -- it has been added to PlanningScene. "
359  "Keeping existing 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  CONSOLE_BRIDGE_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  CONSOLE_BRIDGE_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  CONSOLE_BRIDGE_logError("Could not get CollisionRobotUnpadded named '%s'. "
408  "Returning active CollisionRobotUnpadded '%s' instead",
409  collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str());
410  return active_collision_->getCollisionRobotUnpadded();
411  }
412 
413  return it->second->getCollisionRobotUnpadded();
414 }
415 
417 {
418  if (!parent_)
419  return;
420 
421  // clear everything, reset the world, record diffs
422  world_.reset(new collision_detection::World(*parent_->world_));
427 
428  // use parent crobot_ if it exists. Otherwise copy padding from parent.
429  for (CollisionDetectorIterator it = collision_.begin(); it != collision_.end(); ++it)
430  {
431  if (!it->second->parent_)
432  it->second->findParent(*this);
433 
434  if (it->second->parent_)
435  {
436  it->second->crobot_.reset();
437  it->second->crobot_const_.reset();
438  it->second->crobot_unpadded_.reset();
439  it->second->crobot_unpadded_const_.reset();
440 
441  it->second->cworld_ = it->second->alloc_->allocateWorld(it->second->parent_->cworld_, world_);
442  it->second->cworld_const_ = it->second->cworld_;
443  }
444  else
445  {
446  it->second->copyPadding(*parent_->active_collision_);
447 
448  it->second->cworld_ = it->second->alloc_->allocateWorld(world_);
449  it->second->cworld_const_ = it->second->cworld_;
450  }
451  }
452 
453  ftf_.reset();
454  kstate_.reset();
455  acm_.reset();
456  object_colors_.reset();
457  object_types_.reset();
458 }
459 
460 void planning_scene::PlanningScene::pushDiffs(const PlanningScenePtr& scene)
461 {
462  if (!parent_)
463  return;
464 
465  if (ftf_)
466  scene->getTransformsNonConst().setAllTransforms(ftf_->getAllTransforms());
467 
468  if (kstate_)
469  {
470  scene->getCurrentStateNonConst() = *kstate_;
471  // push colors and types for attached objects
472  std::vector<const moveit::core::AttachedBody*> attached_objs;
473  kstate_->getAttachedBodies(attached_objs);
474  for (std::vector<const moveit::core::AttachedBody*>::const_iterator it = attached_objs.begin();
475  it != attached_objs.end(); ++it)
476  {
477  if (hasObjectType((*it)->getName()))
478  scene->setObjectType((*it)->getName(), getObjectType((*it)->getName()));
479  if (hasObjectColor((*it)->getName()))
480  scene->setObjectColor((*it)->getName(), getObjectColor((*it)->getName()));
481  }
482  }
483 
484  if (acm_)
485  scene->getAllowedCollisionMatrixNonConst() = *acm_;
486 
487  if (active_collision_->crobot_)
488  {
489  collision_detection::CollisionRobotPtr active_crobot = scene->getCollisionRobotNonConst();
490  active_crobot->setLinkPadding(active_collision_->crobot_->getLinkPadding());
491  active_crobot->setLinkScale(active_collision_->crobot_->getLinkScale());
492  scene->propogateRobotPadding();
493  }
494 
495  if (world_diff_)
496  {
497  for (collision_detection::WorldDiff::const_iterator it = world_diff_->begin(); it != world_diff_->end(); ++it)
498  {
499  if (it->second == collision_detection::World::DESTROY)
500  {
501  scene->world_->removeObject(it->first);
502  scene->removeObjectColor(it->first);
503  scene->removeObjectType(it->first);
504  }
505  else
506  {
507  const collision_detection::World::Object& obj = *world_->getObject(it->first);
508  scene->world_->removeObject(obj.id_);
509  scene->world_->addToObject(obj.id_, obj.shapes_, obj.shape_poses_);
510  if (hasObjectColor(it->first))
511  scene->setObjectColor(it->first, getObjectColor(it->first));
512  if (hasObjectType(it->first))
513  scene->setObjectType(it->first, getObjectType(it->first));
514  }
515  }
516  }
517 }
518 
521 {
522  if (getCurrentState().dirtyCollisionBodyTransforms())
524  else
525  checkCollision(req, res, getCurrentState());
526 }
527 
530  const robot_state::RobotState& kstate) const
531 {
532  // check collision with the world using the padded version
533  getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobot(), kstate, getAllowedCollisionMatrix());
534 
535  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
536  {
537  // do self-collision checking with the unpadded version of the robot
538  getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, getAllowedCollisionMatrix());
539  }
540 }
541 
544 {
545  if (getCurrentState().dirtyCollisionBodyTransforms())
547  else
548  checkSelfCollision(req, res, getCurrentState());
549 }
550 
553  const robot_state::RobotState& kstate,
555 {
556  // check collision with the world using the padded version
557  getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobot(), kstate, acm);
558 
559  // do self-collision checking with the unpadded version of the robot
560  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
561  getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, acm);
562 }
563 
566 {
567  if (getCurrentState().dirtyCollisionBodyTransforms())
569  else
571 }
572 
575  const robot_state::RobotState& kstate,
577 {
578  // check collision with the world using the unpadded version
579  getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobotUnpadded(), kstate, acm);
580 
581  // do self-collision checking with the unpadded version of the robot
582  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
583  {
584  getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, acm);
585  }
586 }
587 
589 {
590  if (getCurrentState().dirtyCollisionBodyTransforms())
592  else
594 }
595 
597  const robot_state::RobotState& kstate,
599 {
601  req.contacts = true;
602  req.max_contacts = getRobotModel()->getLinkModelsWithCollisionGeometry().size() + 1;
603  req.max_contacts_per_pair = 1;
605  checkCollision(req, res, kstate, acm);
606  res.contacts.swap(contacts);
607 }
608 
609 void planning_scene::PlanningScene::getCollidingLinks(std::vector<std::string>& links)
610 {
611  if (getCurrentState().dirtyCollisionBodyTransforms())
613  else
615 }
616 
617 void planning_scene::PlanningScene::getCollidingLinks(std::vector<std::string>& links,
618  const robot_state::RobotState& kstate,
620 {
622  getCollidingPairs(contacts, kstate, acm);
623  links.clear();
624  for (collision_detection::CollisionResult::ContactMap::const_iterator it = contacts.begin(); it != contacts.end();
625  ++it)
626  for (std::size_t j = 0; j < it->second.size(); ++j)
627  {
628  if (it->second[j].body_type_1 == collision_detection::BodyTypes::ROBOT_LINK)
629  links.push_back(it->second[j].body_name_1);
630  if (it->second[j].body_type_2 == collision_detection::BodyTypes::ROBOT_LINK)
631  links.push_back(it->second[j].body_name_2);
632  }
633 }
634 
635 const collision_detection::CollisionRobotPtr& planning_scene::PlanningScene::getCollisionRobotNonConst()
636 {
637  if (!active_collision_->crobot_)
638  {
639  active_collision_->crobot_ =
640  active_collision_->alloc_->allocateRobot(active_collision_->parent_->getCollisionRobot());
641  active_collision_->crobot_const_ = active_collision_->crobot_;
642  }
643  return active_collision_->crobot_;
644 }
645 
647 {
648  if (!kstate_)
649  {
650  kstate_.reset(new robot_state::RobotState(parent_->getCurrentState()));
651  kstate_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
652  }
653  kstate_->update();
654  return *kstate_;
655 }
656 
657 robot_state::RobotStatePtr
658 planning_scene::PlanningScene::getCurrentStateUpdated(const moveit_msgs::RobotState& update) const
659 {
660  robot_state::RobotStatePtr state(new robot_state::RobotState(getCurrentState()));
662  return state;
663 }
664 
666 {
668  if (kstate_)
669  kstate_->setAttachedBodyUpdateCallback(callback);
670 }
671 
674 {
677  if (callback)
678  current_world_object_update_observer_handle_ = world_->addObserver(callback);
680 }
681 
683 {
684  if (!acm_)
685  acm_.reset(new collision_detection::AllowedCollisionMatrix(parent_->getAllowedCollisionMatrix()));
686  return *acm_;
687 }
688 
690 {
692  return static_cast<const PlanningScene*>(this)->getTransforms();
693 }
694 
696 {
698  if (!ftf_)
699  {
700  ftf_.reset(new SceneTransforms(this));
701  ftf_->setAllTransforms(parent_->getTransforms().getAllTransforms());
702  }
703  return *ftf_;
704 }
705 
706 void planning_scene::PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene_msg) const
707 {
708  scene_msg.name = name_;
709  scene_msg.robot_model_name = getRobotModel()->getName();
710  scene_msg.is_diff = true;
711 
712  if (ftf_)
713  ftf_->copyTransforms(scene_msg.fixed_frame_transforms);
714  else
715  scene_msg.fixed_frame_transforms.clear();
716 
717  if (kstate_)
718  robot_state::robotStateToRobotStateMsg(*kstate_, scene_msg.robot_state);
719  else
720  scene_msg.robot_state = moveit_msgs::RobotState();
721 
722  if (acm_)
723  acm_->getMessage(scene_msg.allowed_collision_matrix);
724  else
725  scene_msg.allowed_collision_matrix = moveit_msgs::AllowedCollisionMatrix();
726 
727  if (active_collision_->crobot_)
728  {
729  active_collision_->crobot_->getPadding(scene_msg.link_padding);
730  active_collision_->crobot_->getScale(scene_msg.link_scale);
731  }
732  else
733  {
734  scene_msg.link_padding.clear();
735  scene_msg.link_scale.clear();
736  }
737 
738  scene_msg.object_colors.clear();
739  if (object_colors_)
740  {
741  unsigned int i = 0;
742  scene_msg.object_colors.resize(object_colors_->size());
743  for (ObjectColorMap::const_iterator it = object_colors_->begin(); it != object_colors_->end(); ++it, ++i)
744  {
745  scene_msg.object_colors[i].id = it->first;
746  scene_msg.object_colors[i].color = it->second;
747  }
748  }
749 
750  scene_msg.world.collision_objects.clear();
751  scene_msg.world.octomap = octomap_msgs::OctomapWithPose();
752 
753  if (world_diff_)
754  {
755  bool do_omap = false;
756  for (collision_detection::WorldDiff::const_iterator it = world_diff_->begin(); it != world_diff_->end(); ++it)
757  {
758  if (it->first == OCTOMAP_NS)
759  do_omap = true;
760  else if (it->second == collision_detection::World::DESTROY)
761  {
762  moveit_msgs::CollisionObject co;
763  co.header.frame_id = getPlanningFrame();
764  co.id = it->first;
765  co.operation = moveit_msgs::CollisionObject::REMOVE;
766  scene_msg.world.collision_objects.push_back(co);
767  }
768  else
769  {
770  scene_msg.world.collision_objects.emplace_back();
771  getCollisionObjectMsg(scene_msg.world.collision_objects.back(), it->first);
772  }
773  }
774  if (do_omap)
775  getOctomapMsg(scene_msg.world.octomap);
776  }
777 }
778 
779 namespace planning_scene
780 {
781 namespace
782 {
783 class ShapeVisitorAddToCollisionObject : public boost::static_visitor<void>
784 {
785 public:
786  ShapeVisitorAddToCollisionObject(moveit_msgs::CollisionObject* obj) : boost::static_visitor<void>(), obj_(obj)
787  {
788  }
789 
790  void setPoseMessage(const geometry_msgs::Pose* pose)
791  {
792  pose_ = pose;
793  }
794 
795  void operator()(const shape_msgs::Plane& shape_msg) const
796  {
797  obj_->planes.push_back(shape_msg);
798  obj_->plane_poses.push_back(*pose_);
799  }
800 
801  void operator()(const shape_msgs::Mesh& shape_msg) const
802  {
803  obj_->meshes.push_back(shape_msg);
804  obj_->mesh_poses.push_back(*pose_);
805  }
806 
807  void operator()(const shape_msgs::SolidPrimitive& shape_msg) const
808  {
809  obj_->primitives.push_back(shape_msg);
810  obj_->primitive_poses.push_back(*pose_);
811  }
812 
813 private:
814  moveit_msgs::CollisionObject* obj_;
815  const geometry_msgs::Pose* pose_;
816 };
817 }
818 }
819 
820 bool planning_scene::PlanningScene::getCollisionObjectMsg(moveit_msgs::CollisionObject& collision_obj,
821  const std::string& ns) const
822 {
823  collision_obj.header.frame_id = getPlanningFrame();
824  collision_obj.id = ns;
825  collision_obj.operation = moveit_msgs::CollisionObject::ADD;
827  if (!obj)
828  return false;
829  ShapeVisitorAddToCollisionObject sv(&collision_obj);
830  for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
831  {
832  shapes::ShapeMsg sm;
833  if (constructMsgFromShape(obj->shapes_[j].get(), sm))
834  {
835  geometry_msgs::Pose p;
836  tf::poseEigenToMsg(obj->shape_poses_[j], p);
837 
838  sv.setPoseMessage(&p);
839  boost::apply_visitor(sv, sm);
840  }
841  }
842 
843  if (!collision_obj.primitives.empty() || !collision_obj.meshes.empty() || !collision_obj.planes.empty())
844  {
845  if (hasObjectType(collision_obj.id))
846  collision_obj.type = getObjectType(collision_obj.id);
847  }
848  return true;
849 }
850 
852  std::vector<moveit_msgs::CollisionObject>& collision_objs) const
853 {
854  collision_objs.clear();
855  const std::vector<std::string>& ns = world_->getObjectIds();
856  for (std::size_t i = 0; i < ns.size(); ++i)
857  if (ns[i] != OCTOMAP_NS)
858  {
859  collision_objs.emplace_back();
860  getCollisionObjectMsg(collision_objs.back(), ns[i]);
861  }
862 }
863 
865  moveit_msgs::AttachedCollisionObject& attached_collision_obj, const std::string& ns) const
866 {
867  std::vector<moveit_msgs::AttachedCollisionObject> attached_collision_objs;
868  getAttachedCollisionObjectMsgs(attached_collision_objs);
869  for (std::size_t i = 0; i < attached_collision_objs.size(); ++i)
870  {
871  if (attached_collision_objs[i].object.id == ns)
872  {
873  attached_collision_obj = attached_collision_objs[i];
874  return true;
875  }
876  }
877  return false;
878 }
879 
881  std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs) const
882 {
883  std::vector<const moveit::core::AttachedBody*> attached_bodies;
884  getCurrentState().getAttachedBodies(attached_bodies);
885  attachedBodiesToAttachedCollisionObjectMsgs(attached_bodies, attached_collision_objs);
886 }
887 
888 bool planning_scene::PlanningScene::getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const
889 {
890  octomap.header.frame_id = getPlanningFrame();
891  octomap.octomap = octomap_msgs::Octomap();
892 
894  if (map)
895  {
896  if (map->shapes_.size() == 1)
897  {
898  const shapes::OcTree* o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
899  octomap_msgs::fullMapToMsg(*o->octree, octomap.octomap);
900  tf::poseEigenToMsg(map->shape_poses_[0], octomap.origin);
901  return true;
902  }
903  CONSOLE_BRIDGE_logError("Unexpected number of shapes in octomap collision object. Not including '%s' object",
904  OCTOMAP_NS.c_str());
905  }
906  return false;
907 }
908 
909 void planning_scene::PlanningScene::getObjectColorMsgs(std::vector<moveit_msgs::ObjectColor>& object_colors) const
910 {
911  object_colors.clear();
912 
913  unsigned int i = 0;
914  ObjectColorMap cmap;
915  getKnownObjectColors(cmap);
916  object_colors.resize(cmap.size());
917  for (ObjectColorMap::const_iterator it = cmap.begin(); it != cmap.end(); ++it, ++i)
918  {
919  object_colors[i].id = it->first;
920  object_colors[i].color = it->second;
921  }
922 }
923 
924 void planning_scene::PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg) const
925 {
926  scene_msg.name = name_;
927  scene_msg.is_diff = false;
928  scene_msg.robot_model_name = getRobotModel()->getName();
929  getTransforms().copyTransforms(scene_msg.fixed_frame_transforms);
930 
932  getAllowedCollisionMatrix().getMessage(scene_msg.allowed_collision_matrix);
933  getCollisionRobot()->getPadding(scene_msg.link_padding);
934  getCollisionRobot()->getScale(scene_msg.link_scale);
935 
936  getObjectColorMsgs(scene_msg.object_colors);
937 
938  // add collision objects
939  getCollisionObjectMsgs(scene_msg.world.collision_objects);
940 
941  // get the octomap
942  getOctomapMsg(scene_msg.world.octomap);
943 }
944 
945 void planning_scene::PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene& scene_msg,
946  const moveit_msgs::PlanningSceneComponents& comp) const
947 {
948  scene_msg.is_diff = false;
949  if (comp.components & moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS)
950  {
951  scene_msg.name = name_;
952  scene_msg.robot_model_name = getRobotModel()->getName();
953  }
954 
955  if (comp.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
956  getTransforms().copyTransforms(scene_msg.fixed_frame_transforms);
957 
958  if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS)
959  {
960  robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, true);
961  for (std::vector<moveit_msgs::AttachedCollisionObject>::iterator it =
962  scene_msg.robot_state.attached_collision_objects.begin();
963  it != scene_msg.robot_state.attached_collision_objects.end(); ++it)
964  {
965  if (hasObjectType(it->object.id))
966  {
967  it->object.type = getObjectType(it->object.id);
968  }
969  }
970  }
971  else if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE)
972  {
973  robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, false);
974  }
975 
976  if (comp.components & moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX)
977  getAllowedCollisionMatrix().getMessage(scene_msg.allowed_collision_matrix);
978 
979  if (comp.components & moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING)
980  {
981  getCollisionRobot()->getPadding(scene_msg.link_padding);
982  getCollisionRobot()->getScale(scene_msg.link_scale);
983  }
984 
985  if (comp.components & moveit_msgs::PlanningSceneComponents::OBJECT_COLORS)
986  getObjectColorMsgs(scene_msg.object_colors);
987 
988  // add collision objects
989  if (comp.components & moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY)
990  getCollisionObjectMsgs(scene_msg.world.collision_objects);
991  else if (comp.components & moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES)
992  {
993  const std::vector<std::string>& ns = world_->getObjectIds();
994  scene_msg.world.collision_objects.clear();
995  scene_msg.world.collision_objects.reserve(ns.size());
996  for (std::size_t i = 0; i < ns.size(); ++i)
997  if (ns[i] != OCTOMAP_NS)
998  {
999  moveit_msgs::CollisionObject co;
1000  co.id = ns[i];
1001  if (hasObjectType(co.id))
1002  co.type = getObjectType(co.id);
1003  scene_msg.world.collision_objects.push_back(co);
1004  }
1005  }
1006 
1007  // get the octomap
1008  if (comp.components & moveit_msgs::PlanningSceneComponents::OCTOMAP)
1009  getOctomapMsg(scene_msg.world.octomap);
1010 }
1011 
1013 {
1014  out << name_ << std::endl;
1015  const std::vector<std::string>& ns = world_->getObjectIds();
1016  for (std::size_t i = 0; i < ns.size(); ++i)
1017  if (ns[i] != OCTOMAP_NS)
1018  {
1020  if (obj)
1021  {
1022  out << "* " << ns[i] << std::endl;
1023  out << obj->shapes_.size() << std::endl;
1024  for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
1025  {
1026  shapes::saveAsText(obj->shapes_[j].get(), out);
1027  out << obj->shape_poses_[j].translation().x() << " " << obj->shape_poses_[j].translation().y() << " "
1028  << obj->shape_poses_[j].translation().z() << std::endl;
1029  Eigen::Quaterniond r(obj->shape_poses_[j].rotation());
1030  out << r.x() << " " << r.y() << " " << r.z() << " " << r.w() << std::endl;
1031  if (hasObjectColor(ns[i]))
1032  {
1033  const std_msgs::ColorRGBA& c = getObjectColor(ns[i]);
1034  out << c.r << " " << c.g << " " << c.b << " " << c.a << std::endl;
1035  }
1036  else
1037  out << "0 0 0 0" << std::endl;
1038  }
1039  }
1040  }
1041  out << "." << std::endl;
1042 }
1043 
1045 {
1046  loadGeometryFromStream(in, Eigen::Affine3d::Identity()); // Use no offset
1047 }
1048 
1049 void planning_scene::PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Affine3d& offset)
1050 {
1051  if (!in.good() || in.eof())
1052  return;
1053  std::getline(in, name_);
1054  do
1055  {
1056  std::string marker;
1057  in >> marker;
1058  if (!in.good() || in.eof())
1059  return;
1060  if (marker == "*")
1061  {
1062  std::string ns;
1063  std::getline(in, ns);
1064  if (!in.good() || in.eof())
1065  return;
1066  boost::algorithm::trim(ns);
1067  unsigned int shape_count;
1068  in >> shape_count;
1069  for (std::size_t i = 0; i < shape_count && in.good() && !in.eof(); ++i)
1070  {
1072  double x, y, z, rx, ry, rz, rw;
1073  in >> x >> y >> z;
1074  in >> rx >> ry >> rz >> rw;
1075  float r, g, b, a;
1076  in >> r >> g >> b >> a;
1077  if (s)
1078  {
1079  Eigen::Affine3d pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz);
1080  // Transform pose by input pose offset
1081  pose = offset * pose;
1082  world_->addToObject(ns, shapes::ShapePtr(s), pose);
1083  if (r > 0.0f || g > 0.0f || b > 0.0f || a > 0.0f)
1084  {
1085  std_msgs::ColorRGBA color;
1086  color.r = r;
1087  color.g = g;
1088  color.b = b;
1089  color.a = a;
1090  setObjectColor(ns, color);
1091  }
1092  }
1093  }
1094  }
1095  else
1096  break;
1097  } while (true);
1098 }
1099 
1100 void planning_scene::PlanningScene::setCurrentState(const moveit_msgs::RobotState& state)
1101 {
1102  // The attached bodies will be processed separately by processAttachedCollisionObjectMsgs
1103  // after kstate_ has been updated
1104  moveit_msgs::RobotState state_no_attached(state);
1105  state_no_attached.attached_collision_objects.clear();
1106 
1107  if (parent_)
1108  {
1109  if (!kstate_)
1110  {
1111  kstate_.reset(new robot_state::RobotState(parent_->getCurrentState()));
1112  kstate_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1113  }
1115  }
1116  else
1117  robot_state::robotStateMsgToRobotState(*ftf_, state_no_attached, *kstate_);
1118 
1119  for (std::size_t i = 0; i < state.attached_collision_objects.size(); ++i)
1120  {
1121  if (!state.is_diff && state.attached_collision_objects[i].object.operation != moveit_msgs::CollisionObject::ADD)
1122  {
1123  CONSOLE_BRIDGE_logError("The specified RobotState is not marked as is_diff. "
1124  "The request to modify the object '%s' is not supported. Object is ignored.",
1125  state.attached_collision_objects[i].object.id.c_str());
1126  continue;
1127  }
1128  processAttachedCollisionObjectMsg(state.attached_collision_objects[i]);
1129  }
1130 }
1131 
1133 {
1134  getCurrentStateNonConst() = state;
1135 }
1136 
1138 {
1139  if (!parent_)
1140  return;
1141 
1142  if (!ftf_)
1143  {
1144  ftf_.reset(new SceneTransforms(this));
1145  ftf_->setAllTransforms(parent_->getTransforms().getAllTransforms());
1146  }
1147 
1148  if (!kstate_)
1149  {
1150  kstate_.reset(new robot_state::RobotState(parent_->getCurrentState()));
1151  kstate_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1152  }
1153 
1154  if (!acm_)
1155  acm_.reset(new collision_detection::AllowedCollisionMatrix(parent_->getAllowedCollisionMatrix()));
1156 
1157  for (CollisionDetectorIterator it = collision_.begin(); it != collision_.end(); ++it)
1158  {
1159  if (!it->second->crobot_)
1160  {
1161  it->second->crobot_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobot());
1162  it->second->crobot_const_ = it->second->crobot_;
1163  }
1164  if (!it->second->crobot_unpadded_)
1165  {
1166  it->second->crobot_unpadded_ =
1167  it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobotUnpadded());
1168  it->second->crobot_unpadded_const_ = it->second->crobot_unpadded_;
1169  }
1170  it->second->parent_.reset();
1171  }
1172  world_diff_.reset();
1173 
1174  if (!object_colors_)
1175  {
1176  ObjectColorMap kc;
1177  parent_->getKnownObjectColors(kc);
1178  object_colors_.reset(new ObjectColorMap(kc));
1179  }
1180  else
1181  {
1182  ObjectColorMap kc;
1183  parent_->getKnownObjectColors(kc);
1184  for (ObjectColorMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
1185  if (object_colors_->find(it->first) == object_colors_->end())
1186  (*object_colors_)[it->first] = it->second;
1187  }
1188 
1189  if (!object_types_)
1190  {
1191  ObjectTypeMap kc;
1192  parent_->getKnownObjectTypes(kc);
1193  object_types_.reset(new ObjectTypeMap(kc));
1194  }
1195  else
1196  {
1197  ObjectTypeMap kc;
1198  parent_->getKnownObjectTypes(kc);
1199  for (ObjectTypeMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
1200  if (object_types_->find(it->first) == object_types_->end())
1201  (*object_types_)[it->first] = it->second;
1202  }
1203 
1204  parent_.reset();
1205 }
1206 
1207 bool planning_scene::PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& scene_msg)
1208 {
1209  bool result = true;
1210 
1211  CONSOLE_BRIDGE_logDebug("moveit.planning_scene: Adding planning scene diff");
1212  if (!scene_msg.name.empty())
1213  name_ = scene_msg.name;
1214 
1215  if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name != getRobotModel()->getName())
1216  CONSOLE_BRIDGE_logWarn("Setting the scene for model '%s' but model '%s' is loaded.",
1217  scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str());
1218 
1219  // there is at least one transform in the list of fixed transform: from model frame to itself;
1220  // if the list is empty, then nothing has been set
1221  if (!scene_msg.fixed_frame_transforms.empty())
1222  {
1223  if (!ftf_)
1224  ftf_.reset(new SceneTransforms(this));
1225  ftf_->setTransforms(scene_msg.fixed_frame_transforms);
1226  }
1227 
1228  // if at least some joints have been specified, we set them
1229  if (!scene_msg.robot_state.multi_dof_joint_state.joint_names.empty() ||
1230  !scene_msg.robot_state.joint_state.name.empty() || !scene_msg.robot_state.attached_collision_objects.empty())
1231  setCurrentState(scene_msg.robot_state);
1232 
1233  // if at least some links are mentioned in the allowed collision matrix, then we have an update
1234  if (!scene_msg.allowed_collision_matrix.entry_names.empty())
1235  acm_.reset(new collision_detection::AllowedCollisionMatrix(scene_msg.allowed_collision_matrix));
1236 
1237  if (!scene_msg.link_padding.empty() || !scene_msg.link_scale.empty())
1238  {
1239  for (CollisionDetectorIterator it = collision_.begin(); it != collision_.end(); ++it)
1240  {
1241  if (!it->second->crobot_)
1242  {
1243  it->second->crobot_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobot());
1244  it->second->crobot_const_ = it->second->crobot_;
1245  }
1246  it->second->crobot_->setPadding(scene_msg.link_padding);
1247  it->second->crobot_->setScale(scene_msg.link_scale);
1248  }
1249  }
1250 
1251  // if any colors have been specified, replace the ones we have with the specified ones
1252  for (std::size_t i = 0; i < scene_msg.object_colors.size(); ++i)
1253  setObjectColor(scene_msg.object_colors[i].id, scene_msg.object_colors[i].color);
1254 
1255  // process collision object updates
1256  for (std::size_t i = 0; i < scene_msg.world.collision_objects.size(); ++i)
1257  result &= processCollisionObjectMsg(scene_msg.world.collision_objects[i]);
1258 
1259  // if an octomap was specified, replace the one we have with that one
1260  if (!scene_msg.world.octomap.octomap.data.empty())
1261  processOctomapMsg(scene_msg.world.octomap);
1262 
1263  return result;
1264 }
1265 
1266 bool planning_scene::PlanningScene::setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene_msg)
1267 {
1268  CONSOLE_BRIDGE_logDebug("moveit.planning_scene: Setting new planning scene: '%s'", scene_msg.name.c_str());
1269  name_ = scene_msg.name;
1270 
1271  if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name != getRobotModel()->getName())
1272  CONSOLE_BRIDGE_logWarn("Setting the scene for model '%s' but model '%s' is loaded.",
1273  scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str());
1274 
1275  if (parent_)
1276  decoupleParent();
1277 
1278  object_types_.reset();
1279  ftf_->setTransforms(scene_msg.fixed_frame_transforms);
1280  setCurrentState(scene_msg.robot_state);
1281  acm_.reset(new collision_detection::AllowedCollisionMatrix(scene_msg.allowed_collision_matrix));
1282  for (CollisionDetectorIterator it = collision_.begin(); it != collision_.end(); ++it)
1283  {
1284  if (!it->second->crobot_)
1285  {
1286  it->second->crobot_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobot());
1287  it->second->crobot_const_ = it->second->crobot_;
1288  }
1289  it->second->crobot_->setPadding(scene_msg.link_padding);
1290  it->second->crobot_->setScale(scene_msg.link_scale);
1291  }
1292  object_colors_.reset(new ObjectColorMap());
1293  for (std::size_t i = 0; i < scene_msg.object_colors.size(); ++i)
1294  setObjectColor(scene_msg.object_colors[i].id, scene_msg.object_colors[i].color);
1295  world_->clearObjects();
1296  return processPlanningSceneWorldMsg(scene_msg.world);
1297 }
1298 
1299 bool planning_scene::PlanningScene::processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld& world)
1300 {
1301  bool result = true;
1302  for (std::size_t i = 0; i < world.collision_objects.size(); ++i)
1303  result &= processCollisionObjectMsg(world.collision_objects[i]);
1304  processOctomapMsg(world.octomap);
1305  return result;
1306 }
1307 
1308 bool planning_scene::PlanningScene::usePlanningSceneMsg(const moveit_msgs::PlanningScene& scene_msg)
1309 {
1310  if (scene_msg.is_diff)
1311  return setPlanningSceneDiffMsg(scene_msg);
1312  else
1313  return setPlanningSceneMsg(scene_msg);
1314 }
1315 
1316 void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::Octomap& map)
1317 {
1318  // each octomap replaces any previous one
1319  world_->removeObject(OCTOMAP_NS);
1320 
1321  if (map.data.empty())
1322  return;
1323 
1324  if (map.id != "OcTree")
1325  {
1326  CONSOLE_BRIDGE_logError("Received octomap is of type '%s' but type 'OcTree' is expected.", map.id.c_str());
1327  return;
1328  }
1329 
1330  std::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map)));
1331  if (!map.header.frame_id.empty())
1332  {
1333  const Eigen::Affine3d& t = getTransforms().getTransform(map.header.frame_id);
1334  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t);
1335  }
1336  else
1337  {
1338  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), Eigen::Affine3d::Identity());
1339  }
1340 }
1341 
1343 {
1344  const std::vector<std::string>& object_ids = world_->getObjectIds();
1345  for (std::size_t i = 0; i < object_ids.size(); ++i)
1346  if (object_ids[i] != OCTOMAP_NS)
1347  {
1348  world_->removeObject(object_ids[i]);
1349  removeObjectColor(object_ids[i]);
1350  removeObjectType(object_ids[i]);
1351  }
1352 }
1353 
1354 void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose& map)
1355 {
1356  // each octomap replaces any previous one
1357  world_->removeObject(OCTOMAP_NS);
1358 
1359  if (map.octomap.data.empty())
1360  return;
1361 
1362  if (map.octomap.id != "OcTree")
1363  {
1364  CONSOLE_BRIDGE_logError("Received octomap is of type '%s' but type 'OcTree' is expected.", map.octomap.id.c_str());
1365  return;
1366  }
1367 
1368  std::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map.octomap)));
1369  const Eigen::Affine3d& t = getTransforms().getTransform(map.header.frame_id);
1370  Eigen::Affine3d p;
1371  tf::poseMsgToEigen(map.origin, p);
1372  p = t * p;
1373  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), p);
1374 }
1375 
1376 void planning_scene::PlanningScene::processOctomapPtr(const std::shared_ptr<const octomap::OcTree>& octree,
1377  const Eigen::Affine3d& t)
1378 {
1380  if (map)
1381  {
1382  if (map->shapes_.size() == 1)
1383  {
1384  // check to see if we have the same octree pointer & pose.
1385  const shapes::OcTree* o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
1386  if (o->octree == octree)
1387  {
1388  // if the pose changed, we update it
1389  if (map->shape_poses_[0].isApprox(t, std::numeric_limits<double>::epsilon() * 100.0))
1390  {
1391  if (world_diff_)
1394  }
1395  else
1396  {
1397  shapes::ShapeConstPtr shape = map->shapes_[0];
1398  map.reset(); // reset this pointer first so that caching optimizations can be used in CollisionWorld
1399  world_->moveShapeInObject(OCTOMAP_NS, shape, t);
1400  }
1401  return;
1402  }
1403  }
1404  }
1405  // if the octree pointer changed, update the structure
1406  world_->removeObject(OCTOMAP_NS);
1407  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(octree)), t);
1408 }
1409 
1411  const moveit_msgs::AttachedCollisionObject& object)
1412 {
1413  if (object.object.operation == moveit_msgs::CollisionObject::ADD && !getRobotModel()->hasLinkModel(object.link_name))
1414  {
1415  CONSOLE_BRIDGE_logError("Unable to attach a body to link '%s' (link not found)", object.link_name.c_str());
1416  return false;
1417  }
1418 
1419  if (object.object.id == OCTOMAP_NS)
1420  {
1421  CONSOLE_BRIDGE_logError("The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str());
1422  return false;
1423  }
1424 
1425  if (!kstate_) // there must be a parent in this case
1426  {
1427  kstate_.reset(new robot_state::RobotState(parent_->getCurrentState()));
1428  kstate_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1429  }
1430  kstate_->update();
1431 
1432  if (object.object.operation == moveit_msgs::CollisionObject::ADD ||
1433  object.object.operation == moveit_msgs::CollisionObject::APPEND)
1434  {
1435  if (object.object.primitives.size() != object.object.primitive_poses.size())
1436  {
1437  CONSOLE_BRIDGE_logError("Number of primitive shapes does not match "
1438  "number of poses in attached collision object message");
1439  return false;
1440  }
1441 
1442  if (object.object.meshes.size() != object.object.mesh_poses.size())
1443  {
1444  CONSOLE_BRIDGE_logError("Number of meshes does not match number of poses in attached collision object message");
1445  return false;
1446  }
1447 
1448  if (object.object.planes.size() != object.object.plane_poses.size())
1449  {
1450  CONSOLE_BRIDGE_logError("Number of planes does not match number of poses in attached collision object message");
1451  return false;
1452  }
1453 
1454  const robot_model::LinkModel* lm = getRobotModel()->getLinkModel(object.link_name);
1455  if (lm)
1456  {
1457  std::vector<shapes::ShapeConstPtr> shapes;
1459 
1460  // we need to add some shapes; if the message is empty, maybe the object is already in the world
1461  if (object.object.operation == moveit_msgs::CollisionObject::ADD && object.object.primitives.empty() &&
1462  object.object.meshes.empty() && object.object.planes.empty())
1463  {
1464  collision_detection::CollisionWorld::ObjectConstPtr obj = world_->getObject(object.object.id);
1465  if (obj)
1466  {
1467  CONSOLE_BRIDGE_logInform("Attaching world object '%s' to link '%s'", object.object.id.c_str(),
1468  object.link_name.c_str());
1469 
1470  // extract the shapes from the world
1471  shapes = obj->shapes_;
1472  poses = obj->shape_poses_;
1473  // remove the pointer to the objects from the collision world
1474  world_->removeObject(object.object.id);
1475 
1476  // need to transform poses to the link frame
1477  const Eigen::Affine3d& i_t = kstate_->getGlobalLinkTransform(lm).inverse();
1478  for (std::size_t i = 0; i < poses.size(); ++i)
1479  poses[i] = i_t * poses[i];
1480  }
1481  else
1482  {
1483  CONSOLE_BRIDGE_logError("Attempting to attach object '%s' to link '%s' but no geometry specified "
1484  "and such an object does not exist in the collision world",
1485  object.object.id.c_str(), object.link_name.c_str());
1486  return false;
1487  }
1488  }
1489  else
1490  {
1491  // we clear the world objects with the same name, since we got an update on their geometry
1492  if (world_->removeObject(object.object.id))
1493  {
1494  if (object.object.operation == moveit_msgs::CollisionObject::ADD)
1495  CONSOLE_BRIDGE_logInform("Removing world object with the same name as newly attached object: '%s'",
1496  object.object.id.c_str());
1497  else
1498  CONSOLE_BRIDGE_logWarn("You tried to append geometry to an attached object "
1499  "that is actually a world object ('%s'). World geometry is ignored.",
1500  object.object.id.c_str());
1501  }
1502 
1503  for (std::size_t i = 0; i < object.object.primitives.size(); ++i)
1504  {
1505  shapes::Shape* s = shapes::constructShapeFromMsg(object.object.primitives[i]);
1506  if (s)
1507  {
1508  Eigen::Affine3d p;
1509  tf::poseMsgToEigen(object.object.primitive_poses[i], p);
1510  shapes.push_back(shapes::ShapeConstPtr(s));
1511  poses.push_back(p);
1512  }
1513  }
1514  for (std::size_t i = 0; i < object.object.meshes.size(); ++i)
1515  {
1516  shapes::Shape* s = shapes::constructShapeFromMsg(object.object.meshes[i]);
1517  if (s)
1518  {
1519  Eigen::Affine3d p;
1520  tf::poseMsgToEigen(object.object.mesh_poses[i], p);
1521  shapes.push_back(shapes::ShapeConstPtr(s));
1522  poses.push_back(p);
1523  }
1524  }
1525  for (std::size_t i = 0; i < object.object.planes.size(); ++i)
1526  {
1527  shapes::Shape* s = shapes::constructShapeFromMsg(object.object.planes[i]);
1528  if (s)
1529  {
1530  Eigen::Affine3d p;
1531  tf::poseMsgToEigen(object.object.plane_poses[i], p);
1532  shapes.push_back(shapes::ShapeConstPtr(s));
1533  poses.push_back(p);
1534  }
1535  }
1536 
1537  // transform poses to link frame
1538  if (object.object.header.frame_id != object.link_name)
1539  {
1540  const Eigen::Affine3d& t = kstate_->getGlobalLinkTransform(lm).inverse() *
1541  getTransforms().getTransform(object.object.header.frame_id);
1542  for (std::size_t i = 0; i < poses.size(); ++i)
1543  poses[i] = t * poses[i];
1544  }
1545  }
1546 
1547  if (shapes.empty())
1548  {
1549  CONSOLE_BRIDGE_logError("There is no geometry to attach to link '%s' as part of attached body '%s'",
1550  object.link_name.c_str(), object.object.id.c_str());
1551  return false;
1552  }
1553 
1554  if (!object.object.type.db.empty() || !object.object.type.key.empty())
1555  setObjectType(object.object.id, object.object.type);
1556 
1557  if (object.object.operation == moveit_msgs::CollisionObject::ADD || !kstate_->hasAttachedBody(object.object.id))
1558  {
1559  // there should not exist an attached object with this name
1560  if (kstate_->clearAttachedBody(object.object.id))
1561  CONSOLE_BRIDGE_logInform("The robot state already had an object named '%s' attached to link '%s'. "
1562  "The object was replaced.",
1563  object.object.id.c_str(), object.link_name.c_str());
1564  kstate_->attachBody(object.object.id, shapes, poses, object.touch_links, object.link_name,
1565  object.detach_posture);
1566  CONSOLE_BRIDGE_logInform("Attached object '%s' to link '%s'", object.object.id.c_str(),
1567  object.link_name.c_str());
1568  }
1569  else
1570  {
1571  const robot_state::AttachedBody* ab = kstate_->getAttachedBody(object.object.id);
1572  shapes.insert(shapes.end(), ab->getShapes().begin(), ab->getShapes().end());
1573  poses.insert(poses.end(), ab->getFixedTransforms().begin(), ab->getFixedTransforms().end());
1574  trajectory_msgs::JointTrajectory detach_posture =
1575  object.detach_posture.joint_names.empty() ? ab->getDetachPosture() : object.detach_posture;
1576  std::set<std::string> ab_touch_links = ab->getTouchLinks();
1577  kstate_->clearAttachedBody(object.object.id);
1578  if (object.touch_links.empty())
1579  kstate_->attachBody(object.object.id, shapes, poses, ab_touch_links, object.link_name, detach_posture);
1580  else
1581  kstate_->attachBody(object.object.id, shapes, poses, object.touch_links, object.link_name, detach_posture);
1582  CONSOLE_BRIDGE_logInform("Added shapes to object '%s' attached to link '%s'", object.object.id.c_str(),
1583  object.link_name.c_str());
1584  }
1585 
1586  return true;
1587  }
1588  else
1589  CONSOLE_BRIDGE_logError("Robot state is not compatible with robot model. This could be fatal.");
1590  }
1591  else if (object.object.operation == moveit_msgs::CollisionObject::REMOVE)
1592  {
1593  std::vector<const robot_state::AttachedBody*> attached_bodies;
1594  if (object.link_name.empty())
1595  {
1596  if (object.object.id.empty())
1597  kstate_->getAttachedBodies(attached_bodies);
1598  else
1599  {
1600  const robot_state::AttachedBody* ab = kstate_->getAttachedBody(object.object.id);
1601  if (ab)
1602  attached_bodies.push_back(ab);
1603  }
1604  }
1605  else
1606  {
1607  const robot_model::LinkModel* lm = getRobotModel()->getLinkModel(object.link_name);
1608  if (lm)
1609  {
1610  if (object.object.id.empty()) // if no specific object id is given, then we remove all objects attached to the
1611  // link_name
1612  {
1613  kstate_->getAttachedBodies(attached_bodies, lm);
1614  }
1615  else // a specific object id will be removed
1616  {
1617  const robot_state::AttachedBody* ab = kstate_->getAttachedBody(object.object.id);
1618  if (ab)
1619  attached_bodies.push_back(ab);
1620  }
1621  }
1622  }
1623 
1624  for (std::size_t i = 0; i < attached_bodies.size(); ++i)
1625  {
1626  std::vector<shapes::ShapeConstPtr> shapes = attached_bodies[i]->getShapes();
1627  EigenSTL::vector_Affine3d poses = attached_bodies[i]->getGlobalCollisionBodyTransforms();
1628  std::string name = attached_bodies[i]->getName();
1629 
1630  kstate_->clearAttachedBody(name);
1631 
1632  if (world_->hasObject(name))
1633  CONSOLE_BRIDGE_logWarn("The collision world already has an object with the same name as the body about to be "
1634  "detached. NOT adding the detached body '%s' to the collision world.",
1635  object.object.id.c_str());
1636  else
1637  {
1638  world_->addToObject(name, shapes, poses);
1639  CONSOLE_BRIDGE_logInform("Detached object '%s' from link '%s' and added it back in the collision world",
1640  name.c_str(), object.link_name.c_str());
1641  }
1642  }
1643  if (!attached_bodies.empty() || object.object.id.empty())
1644  return true;
1645  }
1646  else if (object.object.operation == moveit_msgs::CollisionObject::MOVE)
1647  {
1648  CONSOLE_BRIDGE_logError("Move for attached objects not yet implemented");
1649  }
1650  else
1651  {
1652  CONSOLE_BRIDGE_logError("Unknown collision object operation: %d", object.object.operation);
1653  }
1654 
1655  return false;
1656 }
1657 
1658 bool planning_scene::PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject& object)
1659 {
1660  if (object.id == OCTOMAP_NS)
1661  {
1662  CONSOLE_BRIDGE_logError("The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str());
1663  return false;
1664  }
1665 
1666  if (object.operation == moveit_msgs::CollisionObject::ADD || object.operation == moveit_msgs::CollisionObject::APPEND)
1667  {
1668  if (object.primitives.empty() && object.meshes.empty() && object.planes.empty())
1669  {
1670  CONSOLE_BRIDGE_logError("There are no shapes specified in the collision object message");
1671  return false;
1672  }
1673 
1674  if (object.primitives.size() != object.primitive_poses.size())
1675  {
1676  CONSOLE_BRIDGE_logError("Number of primitive shapes does not match number of poses in collision object message");
1677  return false;
1678  }
1679 
1680  if (object.meshes.size() != object.mesh_poses.size())
1681  {
1682  CONSOLE_BRIDGE_logError("Number of meshes does not match number of poses in collision object message");
1683  return false;
1684  }
1685 
1686  if (object.planes.size() != object.plane_poses.size())
1687  {
1688  CONSOLE_BRIDGE_logError("Number of planes does not match number of poses in collision object message");
1689  return false;
1690  }
1691 
1692  // replace the object if ADD is specified instead of APPEND
1693  if (object.operation == moveit_msgs::CollisionObject::ADD && world_->hasObject(object.id))
1694  world_->removeObject(object.id);
1695 
1696  const Eigen::Affine3d& t = getTransforms().getTransform(object.header.frame_id);
1697 
1698  for (std::size_t i = 0; i < object.primitives.size(); ++i)
1699  {
1700  shapes::Shape* s = shapes::constructShapeFromMsg(object.primitives[i]);
1701  if (s)
1702  {
1703  Eigen::Affine3d p;
1704  tf::poseMsgToEigen(object.primitive_poses[i], p);
1705  world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
1706  }
1707  }
1708  for (std::size_t i = 0; i < object.meshes.size(); ++i)
1709  {
1710  shapes::Shape* s = shapes::constructShapeFromMsg(object.meshes[i]);
1711  if (s)
1712  {
1713  Eigen::Affine3d p;
1714  tf::poseMsgToEigen(object.mesh_poses[i], p);
1715  world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
1716  }
1717  }
1718  for (std::size_t i = 0; i < object.planes.size(); ++i)
1719  {
1720  shapes::Shape* s = shapes::constructShapeFromMsg(object.planes[i]);
1721  if (s)
1722  {
1723  Eigen::Affine3d p;
1724  tf::poseMsgToEigen(object.plane_poses[i], p);
1725  world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
1726  }
1727  }
1728  if (!object.type.key.empty() || !object.type.db.empty())
1729  setObjectType(object.id, object.type);
1730  return true;
1731  }
1732  else if (object.operation == moveit_msgs::CollisionObject::REMOVE)
1733  {
1734  if (object.id.empty())
1735  {
1737  }
1738  else
1739  {
1740  world_->removeObject(object.id);
1741  removeObjectColor(object.id);
1742  removeObjectType(object.id);
1743  }
1744  return true;
1745  }
1746  else if (object.operation == moveit_msgs::CollisionObject::MOVE)
1747  {
1748  if (world_->hasObject(object.id))
1749  {
1750  if (!object.primitives.empty() || !object.meshes.empty() || !object.planes.empty())
1751  CONSOLE_BRIDGE_logWarn("Move operation for object '%s' ignores the geometry specified in the message.",
1752  object.id.c_str());
1753 
1754  const Eigen::Affine3d& t = getTransforms().getTransform(object.header.frame_id);
1755  EigenSTL::vector_Affine3d new_poses;
1756  for (std::size_t i = 0; i < object.primitive_poses.size(); ++i)
1757  {
1758  Eigen::Affine3d p;
1759  tf::poseMsgToEigen(object.primitive_poses[i], p);
1760  new_poses.push_back(t * p);
1761  }
1762  for (std::size_t i = 0; i < object.mesh_poses.size(); ++i)
1763  {
1764  Eigen::Affine3d p;
1765  tf::poseMsgToEigen(object.mesh_poses[i], p);
1766  new_poses.push_back(t * p);
1767  }
1768  for (std::size_t i = 0; i < object.plane_poses.size(); ++i)
1769  {
1770  Eigen::Affine3d p;
1771  tf::poseMsgToEigen(object.plane_poses[i], p);
1772  new_poses.push_back(t * p);
1773  }
1774 
1775  collision_detection::World::ObjectConstPtr obj = world_->getObject(object.id);
1776  if (obj->shapes_.size() == new_poses.size())
1777  {
1778  std::vector<shapes::ShapeConstPtr> shapes = obj->shapes_;
1779  obj.reset();
1780  world_->removeObject(object.id);
1781  world_->addToObject(object.id, shapes, new_poses);
1782  }
1783  else
1784  {
1785  CONSOLE_BRIDGE_logError("Number of supplied poses (%zu) for object '%s' "
1786  "does not match number of shapes (%zu). Not moving.",
1787  new_poses.size(), object.id.c_str(), obj->shapes_.size());
1788  return false;
1789  }
1790  return true;
1791  }
1792  else
1793  CONSOLE_BRIDGE_logError("World object '%s' does not exist. Cannot move.", object.id.c_str());
1794  }
1795  else
1796  CONSOLE_BRIDGE_logError("Unknown collision object operation: %d", object.operation);
1797  return false;
1798 }
1799 
1800 const Eigen::Affine3d& planning_scene::PlanningScene::getFrameTransform(const std::string& id) const
1801 {
1802  return getFrameTransform(getCurrentState(), id);
1803 }
1804 
1805 const Eigen::Affine3d& planning_scene::PlanningScene::getFrameTransform(const std::string& id)
1806 {
1807  if (getCurrentState().dirtyLinkTransforms())
1809  else
1810  return getFrameTransform(getCurrentState(), id);
1811 }
1812 
1814  const std::string& id) const
1815 {
1816  if (!id.empty() && id[0] == '/')
1817  return getFrameTransform(id.substr(1));
1818  if (state.knowsFrameTransform(id))
1819  return state.getFrameTransform(id);
1820  if (getWorld()->hasObject(id))
1821  {
1822  collision_detection::World::ObjectConstPtr obj = getWorld()->getObject(id);
1823  if (obj->shape_poses_.size() > 1)
1824  {
1825  CONSOLE_BRIDGE_logWarn("More than one shapes in object '%s'. Using first one to decide transform", id.c_str());
1826  return obj->shape_poses_[0];
1827  }
1828  else if (obj->shape_poses_.size() == 1)
1829  return obj->shape_poses_[0];
1830  }
1831  return getTransforms().Transforms::getTransform(id);
1832 }
1833 
1835 {
1836  return knowsFrameTransform(getCurrentState(), id);
1837 }
1838 
1840  const std::string& id) const
1841 {
1842  if (!id.empty() && id[0] == '/')
1843  return knowsFrameTransform(id.substr(1));
1844  if (state.knowsFrameTransform(id))
1845  return true;
1846  if (getWorld()->hasObject(id))
1847  {
1848  collision_detection::World::ObjectConstPtr obj = getWorld()->getObject(id);
1849  return obj->shape_poses_.size() == 1;
1850  }
1851  return getTransforms().Transforms::canTransform(id);
1852 }
1853 
1854 bool planning_scene::PlanningScene::hasObjectType(const std::string& id) const
1855 {
1856  if (object_types_)
1857  if (object_types_->find(id) != object_types_->end())
1858  return true;
1859  if (parent_)
1860  return parent_->hasObjectType(id);
1861  return false;
1862 }
1863 
1864 const object_recognition_msgs::ObjectType& planning_scene::PlanningScene::getObjectType(const std::string& id) const
1865 {
1866  if (object_types_)
1867  {
1868  ObjectTypeMap::const_iterator it = object_types_->find(id);
1869  if (it != object_types_->end())
1870  return it->second;
1871  }
1872  if (parent_)
1873  return parent_->getObjectType(id);
1874  static const object_recognition_msgs::ObjectType empty;
1875  return empty;
1876 }
1877 
1879  const object_recognition_msgs::ObjectType& type)
1880 {
1881  if (!object_types_)
1882  object_types_.reset(new ObjectTypeMap());
1883  (*object_types_)[id] = type;
1884 }
1885 
1887 {
1888  if (object_types_)
1889  object_types_->erase(id);
1890 }
1891 
1893 {
1894  kc.clear();
1895  if (parent_)
1896  parent_->getKnownObjectTypes(kc);
1897  if (object_types_)
1898  for (ObjectTypeMap::const_iterator it = object_types_->begin(); it != object_types_->end(); ++it)
1899  kc[it->first] = it->second;
1900 }
1901 
1902 bool planning_scene::PlanningScene::hasObjectColor(const std::string& id) const
1903 {
1904  if (object_colors_)
1905  if (object_colors_->find(id) != object_colors_->end())
1906  return true;
1907  if (parent_)
1908  return parent_->hasObjectColor(id);
1909  return false;
1910 }
1911 
1912 const std_msgs::ColorRGBA& planning_scene::PlanningScene::getObjectColor(const std::string& id) const
1913 {
1914  if (object_colors_)
1915  {
1916  ObjectColorMap::const_iterator it = object_colors_->find(id);
1917  if (it != object_colors_->end())
1918  return it->second;
1919  }
1920  if (parent_)
1921  return parent_->getObjectColor(id);
1922  static const std_msgs::ColorRGBA empty;
1923  return empty;
1924 }
1925 
1927 {
1928  kc.clear();
1929  if (parent_)
1930  parent_->getKnownObjectColors(kc);
1931  if (object_colors_)
1932  for (ObjectColorMap::const_iterator it = object_colors_->begin(); it != object_colors_->end(); ++it)
1933  kc[it->first] = it->second;
1934 }
1935 
1936 void planning_scene::PlanningScene::setObjectColor(const std::string& id, const std_msgs::ColorRGBA& color)
1937 {
1938  if (id.empty())
1939  {
1940  CONSOLE_BRIDGE_logError("Cannot set color of object with empty id.");
1941  return;
1942  }
1943  if (!object_colors_)
1944  object_colors_.reset(new ObjectColorMap());
1945  (*object_colors_)[id] = color;
1946 }
1947 
1949 {
1950  if (object_colors_)
1951  object_colors_->erase(id);
1952 }
1953 
1954 bool planning_scene::PlanningScene::isStateColliding(const moveit_msgs::RobotState& state, const std::string& group,
1955  bool verbose) const
1956 {
1959  return isStateColliding(s, group, verbose);
1960 }
1961 
1962 bool planning_scene::PlanningScene::isStateColliding(const std::string& group, bool verbose)
1963 {
1964  if (getCurrentState().dirtyCollisionBodyTransforms())
1965  return isStateColliding(getCurrentStateNonConst(), group, verbose);
1966  else
1967  return isStateColliding(getCurrentState(), group, verbose);
1968 }
1969 
1971  bool verbose) const
1972 {
1974  req.verbose = verbose;
1975  req.group_name = group;
1977  checkCollision(req, res, state);
1978  return res.collision;
1979 }
1980 
1981 bool planning_scene::PlanningScene::isStateFeasible(const moveit_msgs::RobotState& state, bool verbose) const
1982 {
1983  if (state_feasibility_)
1984  {
1987  return state_feasibility_(s, verbose);
1988  }
1989  return true;
1990 }
1991 
1993 {
1994  if (state_feasibility_)
1995  return state_feasibility_(state, verbose);
1996  return true;
1997 }
1998 
1999 bool planning_scene::PlanningScene::isStateConstrained(const moveit_msgs::RobotState& state,
2000  const moveit_msgs::Constraints& constr, bool verbose) const
2001 {
2004  return isStateConstrained(s, constr, verbose);
2005 }
2006 
2008  const moveit_msgs::Constraints& constr, bool verbose) const
2009 {
2010  kinematic_constraints::KinematicConstraintSetPtr ks(
2012  ks->add(constr, getTransforms());
2013  if (ks->empty())
2014  return true;
2015  else
2016  return isStateConstrained(state, *ks, verbose);
2017 }
2018 
2019 bool planning_scene::PlanningScene::isStateConstrained(const moveit_msgs::RobotState& state,
2021  bool verbose) const
2022 {
2025  return isStateConstrained(s, constr, verbose);
2026 }
2027 
2030  bool verbose) const
2031 {
2032  return constr.decide(state, verbose).satisfied;
2033 }
2034 
2035 bool planning_scene::PlanningScene::isStateValid(const robot_state::RobotState& state, const std::string& group,
2036  bool verbose) const
2037 {
2038  static const moveit_msgs::Constraints emp_constraints;
2039  return isStateValid(state, emp_constraints, group, verbose);
2040 }
2041 
2042 bool planning_scene::PlanningScene::isStateValid(const moveit_msgs::RobotState& state, const std::string& group,
2043  bool verbose) const
2044 {
2045  static const moveit_msgs::Constraints emp_constraints;
2046  return isStateValid(state, emp_constraints, group, verbose);
2047 }
2048 
2049 bool planning_scene::PlanningScene::isStateValid(const moveit_msgs::RobotState& state,
2050  const moveit_msgs::Constraints& constr, const std::string& group,
2051  bool verbose) const
2052 {
2055  return isStateValid(s, constr, group, verbose);
2056 }
2057 
2059  const moveit_msgs::Constraints& constr, const std::string& group,
2060  bool verbose) const
2061 {
2062  if (isStateColliding(state, group, verbose))
2063  return false;
2064  if (!isStateFeasible(state, verbose))
2065  return false;
2066  return isStateConstrained(state, constr, verbose);
2067 }
2068 
2071  const std::string& group, bool verbose) const
2072 {
2073  if (isStateColliding(state, group, verbose))
2074  return false;
2075  if (!isStateFeasible(state, verbose))
2076  return false;
2077  return isStateConstrained(state, constr, verbose);
2078 }
2079 
2080 bool planning_scene::PlanningScene::isPathValid(const moveit_msgs::RobotState& start_state,
2081  const moveit_msgs::RobotTrajectory& trajectory,
2082  const std::string& group, bool verbose,
2083  std::vector<std::size_t>* invalid_index) const
2084 {
2085  static const moveit_msgs::Constraints emp_constraints;
2086  static const std::vector<moveit_msgs::Constraints> emp_constraints_vector;
2087  return isPathValid(start_state, trajectory, emp_constraints, emp_constraints_vector, group, verbose, invalid_index);
2088 }
2089 
2090 bool planning_scene::PlanningScene::isPathValid(const moveit_msgs::RobotState& start_state,
2091  const moveit_msgs::RobotTrajectory& trajectory,
2092  const moveit_msgs::Constraints& path_constraints,
2093  const std::string& group, bool verbose,
2094  std::vector<std::size_t>* invalid_index) const
2095 {
2096  static const std::vector<moveit_msgs::Constraints> emp_constraints_vector;
2097  return isPathValid(start_state, trajectory, path_constraints, emp_constraints_vector, group, verbose, invalid_index);
2098 }
2099 
2100 bool planning_scene::PlanningScene::isPathValid(const moveit_msgs::RobotState& start_state,
2101  const moveit_msgs::RobotTrajectory& trajectory,
2102  const moveit_msgs::Constraints& path_constraints,
2103  const moveit_msgs::Constraints& goal_constraints,
2104  const std::string& group, bool verbose,
2105  std::vector<std::size_t>* invalid_index) const
2106 {
2107  std::vector<moveit_msgs::Constraints> goal_constraints_vector(1, goal_constraints);
2108  return isPathValid(start_state, trajectory, path_constraints, goal_constraints_vector, group, verbose, invalid_index);
2109 }
2110 
2111 bool planning_scene::PlanningScene::isPathValid(const moveit_msgs::RobotState& start_state,
2112  const moveit_msgs::RobotTrajectory& trajectory,
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 {
2121  t.setRobotTrajectoryMsg(start, trajectory);
2122  return isPathValid(t, path_constraints, goal_constraints, group, verbose, invalid_index);
2123 }
2124 
2126  const moveit_msgs::Constraints& path_constraints,
2127  const std::vector<moveit_msgs::Constraints>& goal_constraints,
2128  const std::string& group, bool verbose,
2129  std::vector<std::size_t>* invalid_index) const
2130 {
2131  bool result = true;
2132  if (invalid_index)
2133  invalid_index->clear();
2135  ks_p.add(path_constraints, getTransforms());
2136  std::size_t n_wp = trajectory.getWayPointCount();
2137  for (std::size_t i = 0; i < n_wp; ++i)
2138  {
2139  const robot_state::RobotState& st = trajectory.getWayPoint(i);
2140 
2141  bool this_state_valid = true;
2142  if (isStateColliding(st, group, verbose))
2143  this_state_valid = false;
2144  if (!isStateFeasible(st, verbose))
2145  this_state_valid = false;
2146  if (!ks_p.empty() && !ks_p.decide(st, verbose).satisfied)
2147  this_state_valid = false;
2148 
2149  if (!this_state_valid)
2150  {
2151  if (invalid_index)
2152  invalid_index->push_back(i);
2153  else
2154  return false;
2155  result = false;
2156  }
2157 
2158  // check goal for last state
2159  if (i + 1 == n_wp && !goal_constraints.empty())
2160  {
2161  bool found = false;
2162  for (std::size_t k = 0; k < goal_constraints.size(); ++k)
2163  {
2164  if (isStateConstrained(st, goal_constraints[k]))
2165  {
2166  found = true;
2167  break;
2168  }
2169  }
2170  if (!found)
2171  {
2172  if (verbose)
2173  CONSOLE_BRIDGE_logInform("Goal not satisfied");
2174  if (invalid_index)
2175  invalid_index->push_back(i);
2176  result = false;
2177  }
2178  }
2179  }
2180  return result;
2181 }
2182 
2184  const moveit_msgs::Constraints& path_constraints,
2185  const moveit_msgs::Constraints& goal_constraints,
2186  const std::string& group, bool verbose,
2187  std::vector<std::size_t>* invalid_index) const
2188 {
2189  std::vector<moveit_msgs::Constraints> goal_constraints_vector(1, goal_constraints);
2190  return isPathValid(trajectory, path_constraints, goal_constraints_vector, group, verbose, invalid_index);
2191 }
2192 
2194  const moveit_msgs::Constraints& path_constraints,
2195  const std::string& group, bool verbose,
2196  std::vector<std::size_t>* invalid_index) const
2197 {
2198  static const std::vector<moveit_msgs::Constraints> emp_constraints_vector;
2199  return isPathValid(trajectory, path_constraints, emp_constraints_vector, group, verbose, invalid_index);
2200 }
2201 
2203  const std::string& group, bool verbose,
2204  std::vector<std::size_t>* invalid_index) const
2205 {
2206  static const moveit_msgs::Constraints emp_constraints;
2207  static const std::vector<moveit_msgs::Constraints> emp_constraints_vector;
2208  return isPathValid(trajectory, emp_constraints, emp_constraints_vector, group, verbose, invalid_index);
2209 }
2210 
2212  std::size_t max_costs,
2213  std::set<collision_detection::CostSource>& costs,
2214  double overlap_fraction) const
2215 {
2216  getCostSources(trajectory, max_costs, std::string(), costs, overlap_fraction);
2217 }
2218 
2220  std::size_t max_costs, const std::string& group_name,
2221  std::set<collision_detection::CostSource>& costs,
2222  double overlap_fraction) const
2223 {
2225  creq.max_cost_sources = max_costs;
2226  creq.group_name = group_name;
2227  creq.cost = true;
2228  std::set<collision_detection::CostSource> cs;
2229  std::set<collision_detection::CostSource> cs_start;
2230  std::size_t n_wp = trajectory.getWayPointCount();
2231  for (std::size_t i = 0; i < n_wp; ++i)
2232  {
2234  checkCollision(creq, cres, trajectory.getWayPoint(i));
2235  cs.insert(cres.cost_sources.begin(), cres.cost_sources.end());
2236  if (i == 0)
2237  cs_start.swap(cres.cost_sources);
2238  }
2239 
2240  if (cs.size() <= max_costs)
2241  costs.swap(cs);
2242  else
2243  {
2244  costs.clear();
2245  std::size_t i = 0;
2246  for (std::set<collision_detection::CostSource>::iterator it = cs.begin(); i < max_costs; ++it, ++i)
2247  costs.insert(*it);
2248  }
2249 
2250  collision_detection::removeCostSources(costs, cs_start, overlap_fraction);
2251  collision_detection::removeOverlapping(costs, overlap_fraction);
2252 }
2253 
2255  std::set<collision_detection::CostSource>& costs) const
2256 {
2257  getCostSources(state, max_costs, std::string(), costs);
2258 }
2259 
2261  const std::string& group_name,
2262  std::set<collision_detection::CostSource>& costs) const
2263 {
2265  creq.max_cost_sources = max_costs;
2266  creq.group_name = group_name;
2267  creq.cost = true;
2269  checkCollision(creq, cres, state);
2270  cres.cost_sources.swap(costs);
2271 }
2272 
2274 {
2275  const std::vector<std::string>& objects = getWorld()->getObjectIds();
2276 
2277  out << "Collision World Objects:\n\t ";
2278  std::copy(objects.begin(), objects.end(), std::ostream_iterator<std::string>(out, "\n\t "));
2279 
2280  std::vector<const robot_state::AttachedBody*> attached_bodies;
2281  getCurrentState().getAttachedBodies(attached_bodies);
2282 
2283  out << "\nAttached Bodies:\n";
2284  for (std::size_t i = 0; i < attached_bodies.size(); ++i)
2285  {
2286  out << "\t " << attached_bodies[i]->getName() << "\n";
2287  }
2288 }
bool empty() const
Returns whether or not there are any constraints in the set.
#define CONSOLE_BRIDGE_logWarn(fmt,...)
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:162
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_
#define CONSOLE_BRIDGE_logDebug(fmt,...)
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.
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
void propogateRobotPadding()
Copy scale and padding from active CollisionRobot to other CollisionRobots. This should be called aft...
std::map< std::string, std_msgs::ColorRGBA > ObjectColorMap
A map from object names (e.g., attached bodies, collision objects) to their colors.
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in collision, and if needed, updates the collision transforms of t...
const std::set< std::string > & getTouchLinks() const
Get the links that the attached body is allowed to touch.
Definition: attached_body.h:94
const robot_state::RobotState & getCurrentState() const
Get the state at which the robot is assumed to be.
void setAttachedBodyUpdateCallback(const robot_state::AttachedBodyCallback &callback)
Set the callback to be triggered when changes are made to the current scene state.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
Maintain a sequence of waypoints and the time durations between these waypoints.
double z
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_
#define CONSOLE_BRIDGE_logError(fmt,...)
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:110
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_
#define CONSOLE_BRIDGE_logInform(fmt,...)
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 Sat Apr 21 2018 02:54:51