planning_scene_monitor.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 
40 #include <moveit_msgs/GetPlanningScene.h>
41 
42 #include <dynamic_reconfigure/server.h>
43 #include <moveit_ros_planning/PlanningSceneMonitorDynamicReconfigureConfig.h>
46 
47 #include <memory>
48 
49 namespace planning_scene_monitor
50 {
51 using namespace moveit_ros_planning;
52 
53 static const std::string LOGNAME = "planning_scene_monitor";
54 
56 {
57 public:
59  : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle(decideNamespace(owner->getName())))
60  {
61  dynamic_reconfigure_server_.setCallback(
62  boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
63  }
64 
65 private:
66  // make sure we do not advertise the same service multiple times, in case we use multiple PlanningSceneMonitor
67  // instances in a process
68  static std::string decideNamespace(const std::string& name)
69  {
70  std::string ns = "~/" + name;
71  std::replace(ns.begin(), ns.end(), ' ', '_');
72  std::transform(ns.begin(), ns.end(), ns.begin(), ::tolower);
73  if (ros::service::exists(ns + "/set_parameters", false))
74  {
75  unsigned int c = 1;
76  while (ros::service::exists(ns + boost::lexical_cast<std::string>(c) + "/set_parameters", false))
77  c++;
78  ns += boost::lexical_cast<std::string>(c);
79  }
80  return ns;
81  }
82 
83  void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig& config, uint32_t level)
84  {
86  if (config.publish_geometry_updates)
88  if (config.publish_state_updates)
90  if (config.publish_transforms_updates)
92  if (config.publish_planning_scene)
93  {
94  owner_->setPlanningScenePublishingFrequency(config.publish_planning_scene_hz);
95  owner_->startPublishingPlanningScene(event);
96  }
97  else
98  owner_->stopPublishingPlanningScene();
99  }
100 
102  dynamic_reconfigure::Server<PlanningSceneMonitorDynamicReconfigureConfig> dynamic_reconfigure_server_;
103 };
104 }
105 
108  "attached_collision_object";
111  "world";
115  "scene";
116 
119  const std::string& name)
120  : monitor_name_(name), nh_("~"), tf_(tf)
121 {
122  rm_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
123  initialize(planning_scene::PlanningScenePtr());
124 }
125 
126 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
127  const std::string& robot_description,
129  const std::string& name)
130  : monitor_name_(name), nh_("~"), tf_(tf)
131 {
132  rm_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
133  initialize(scene);
134 }
135 
137  const robot_model_loader::RobotModelLoaderPtr& rm_loader, const boost::shared_ptr<tf::Transformer>& tf,
138  const std::string& name)
139  : monitor_name_(name), nh_("~"), tf_(tf), rm_loader_(rm_loader)
140 {
141  initialize(planning_scene::PlanningScenePtr());
142 }
143 
145  const planning_scene::PlanningScenePtr& scene, const robot_model_loader::RobotModelLoaderPtr& rm_loader,
146  const boost::shared_ptr<tf::Transformer>& tf, const std::string& name)
147  : monitor_name_(name), nh_("~"), tf_(tf), rm_loader_(rm_loader)
148 {
149  initialize(scene);
150 }
151 
153 {
154  if (scene_)
155  {
156  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
157  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
158  }
163 
164  delete reconfigure_impl_;
165  current_state_monitor_.reset();
166  scene_const_.reset();
167  scene_.reset();
168  parent_scene_.reset();
169  robot_model_.reset();
170  rm_loader_.reset();
171 }
172 
173 void planning_scene_monitor::PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& scene)
174 {
176  moveit::tools::Profiler::ScopedBlock prof_block("PlanningSceneMonitor::initialize");
177 
178  if (monitor_name_.empty())
179  monitor_name_ = "planning_scene_monitor";
180  robot_description_ = rm_loader_->getRobotDescription();
181  if (rm_loader_->getModel())
182  {
183  robot_model_ = rm_loader_->getModel();
184  scene_ = scene;
187  if (!scene_)
188  {
189  try
190  {
191  scene_.reset(new planning_scene::PlanningScene(rm_loader_->getModel()));
196 
197  scene_->getCollisionRobotNonConst()->setPadding(default_robot_padd_);
198  scene_->getCollisionRobotNonConst()->setScale(default_robot_scale_);
199  for (std::map<std::string, double>::iterator it = default_robot_link_padd_.begin();
200  it != default_robot_link_padd_.end(); ++it)
201  {
202  scene_->getCollisionRobotNonConst()->setLinkPadding(it->first, it->second);
203  }
204  for (std::map<std::string, double>::iterator it = default_robot_link_scale_.begin();
205  it != default_robot_link_scale_.end(); ++it)
206  {
207  scene_->getCollisionRobotNonConst()->setLinkScale(it->first, it->second);
208  }
209  scene_->propogateRobotPadding();
210  }
211  catch (moveit::ConstructException& e)
212  {
213  ROS_ERROR_NAMED(LOGNAME, "Configuration of planning scene failed");
214  scene_.reset();
216  }
217  }
218  if (scene_)
219  {
220  scene_->setAttachedBodyUpdateCallback(
222  scene_->setCollisionObjectUpdateCallback(
223  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
224  }
225  }
226  else
227  {
228  ROS_ERROR_NAMED(LOGNAME, "Robot model not loaded");
229  }
230 
233 
237 
238  double temp_wait_time = 0.05;
239 
240  if (!robot_description_.empty())
241  nh_.param(robot_description_ + "_planning/shape_transform_cache_lookup_wait_time", temp_wait_time, temp_wait_time);
242 
244 
245  state_update_pending_ = false;
247  false, // not a oneshot timer
248  false); // do not start the timer yet
249 
251 }
252 
254 {
255  if (scene_)
256  {
257  if (flag)
258  {
259  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
260  if (scene_)
261  {
262  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
263  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
264  scene_->decoupleParent();
266  scene_ = parent_scene_->diff();
268  scene_->setAttachedBodyUpdateCallback(
270  scene_->setCollisionObjectUpdateCallback(
271  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
272  }
273  }
274  else
275  {
277  {
278  ROS_WARN_NAMED(LOGNAME, "Diff monitoring was stopped while publishing planning scene diffs. "
279  "Stopping planning scene diff publisher");
281  }
282  {
283  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
284  if (scene_)
285  {
286  scene_->decoupleParent();
287  parent_scene_.reset();
288  // remove the '+' added by .diff() at the end of the scene name
289  if (!scene_->getName().empty())
290  {
291  if (scene_->getName()[scene_->getName().length() - 1] == '+')
292  scene_->setName(scene_->getName().substr(0, scene_->getName().length() - 1));
293  }
294  }
295  }
296  }
297  }
298 }
299 
301 {
303  {
304  std::unique_ptr<boost::thread> copy;
305  copy.swap(publish_planning_scene_);
307  copy->join();
308  monitorDiffs(false);
310  ROS_INFO_NAMED(LOGNAME, "Stopped publishing maintained planning scene.");
311  }
312 }
313 
315  const std::string& planning_scene_topic)
316 {
317  publish_update_types_ = update_type;
319  {
320  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>(planning_scene_topic, 100, false);
321  ROS_INFO_NAMED(LOGNAME, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
322  monitorDiffs(true);
323  publish_planning_scene_.reset(new boost::thread(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this)));
324  }
325 }
326 
328 {
329  ROS_DEBUG_NAMED(LOGNAME, "Started scene publishing thread ...");
330 
331  // publish the full planning scene once
332  {
333  moveit_msgs::PlanningScene msg;
334  {
336  if (octomap_monitor_)
337  lock = octomap_monitor_->getOcTreePtr()->reading();
338  scene_->getPlanningSceneMsg(msg);
339  }
341  ROS_DEBUG_NAMED(LOGNAME, "Published the full planning scene: '%s'", msg.name.c_str());
342  }
343 
344  do
345  {
346  moveit_msgs::PlanningScene msg;
347  bool publish_msg = false;
348  bool is_full = false;
350  {
351  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
355  {
356  if ((publish_update_types_ & new_scene_update_) || new_scene_update_ == UPDATE_SCENE)
357  {
358  if (new_scene_update_ == UPDATE_SCENE)
359  is_full = true;
360  else
361  {
363  if (octomap_monitor_)
364  lock = octomap_monitor_->getOcTreePtr()->reading();
365  scene_->getPlanningSceneDiffMsg(msg);
366  }
367  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the
368  // transform cache to
369  // update while we are
370  // potentially changing
371  // attached bodies
372  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
373  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
374  scene_->pushDiffs(parent_scene_);
375  scene_->clearDiffs();
376  scene_->setAttachedBodyUpdateCallback(
378  scene_->setCollisionObjectUpdateCallback(
379  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
380  if (octomap_monitor_)
381  {
382  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
383  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
384  }
385  if (is_full)
386  {
388  if (octomap_monitor_)
389  lock = octomap_monitor_->getOcTreePtr()->reading();
390  scene_->getPlanningSceneMsg(msg);
391  }
392  // also publish timestamp of this robot_state
393  msg.robot_state.joint_state.header.stamp = last_robot_motion_time_;
394  publish_msg = true;
395  }
396  new_scene_update_ = UPDATE_NONE;
397  }
398  }
399  if (publish_msg)
400  {
401  rate.reset();
403  if (is_full)
404  ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str());
405  rate.sleep();
406  }
407  } while (publish_planning_scene_);
408 }
409 
410 void planning_scene_monitor::PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string>& topics) const
411 {
412  topics.clear();
414  {
415  const std::string& t = current_state_monitor_->getMonitoredTopic();
416  if (!t.empty())
417  topics.push_back(t);
418  }
420  topics.push_back(planning_scene_subscriber_.getTopic());
422  topics.push_back(collision_object_subscriber_->getTopic());
424  topics.push_back(planning_scene_world_subscriber_.getTopic());
425 }
426 
427 namespace
428 {
429 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr& scene,
430  const planning_scene::PlanningScene* possible_parent)
431 {
432  if (scene && scene.get() == possible_parent)
433  return true;
434  if (scene)
435  return sceneIsParentOf(scene->getParent(), possible_parent);
436  return false;
437 }
438 }
439 
440 bool planning_scene_monitor::PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr& scene) const
441 {
442  return sceneIsParentOf(scene_const_, scene.get());
443 }
444 
446  const planning_scene::PlanningSceneConstPtr& scene) const
447 {
448  return sceneIsParentOf(scene_const_, scene.get());
449 }
450 
452 {
453  // do not modify update functions while we are calling them
454  boost::recursive_mutex::scoped_lock lock(update_lock_);
455 
456  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
457  update_callbacks_[i](update_type);
458  new_scene_update_ = (SceneUpdateType)((int)new_scene_update_ | (int)update_type);
460 }
461 
463 {
464  // use global namespace for service
465  ros::ServiceClient client = ros::NodeHandle().serviceClient<moveit_msgs::GetPlanningScene>(service_name);
466  moveit_msgs::GetPlanningScene srv;
467  srv.request.components.components =
468  srv.request.components.SCENE_SETTINGS | srv.request.components.ROBOT_STATE |
469  srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS | srv.request.components.WORLD_OBJECT_NAMES |
470  srv.request.components.WORLD_OBJECT_GEOMETRY | srv.request.components.OCTOMAP |
471  srv.request.components.TRANSFORMS | srv.request.components.ALLOWED_COLLISION_MATRIX |
472  srv.request.components.LINK_PADDING_AND_SCALING | srv.request.components.OBJECT_COLORS;
473 
474  // Make sure client is connected to server
475  if (!client.exists())
476  {
477  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for service `" << service_name << "` to exist.");
478  client.waitForExistence(ros::Duration(5.0));
479  }
480 
481  if (client.call(srv))
482  {
483  newPlanningSceneMessage(srv.response.scene);
484  }
485  else
486  {
487  ROS_INFO_NAMED(LOGNAME, "Failed to call service %s, have you launched move_group? at %s:%d", service_name.c_str(),
488  __FILE__, __LINE__);
489  return false;
490  }
491  return true;
492 }
493 
495  const moveit_msgs::PlanningSceneConstPtr& scene)
496 {
497  newPlanningSceneMessage(*scene);
498 }
499 
501 {
502  octomap_monitor_->getOcTreePtr()->lockWrite();
503  octomap_monitor_->getOcTreePtr()->clear();
504  octomap_monitor_->getOcTreePtr()->unlockWrite();
505 }
506 
508 {
509  if (!scene_)
510  return false;
511 
512  bool result;
513 
515  std::string old_scene_name;
516  {
517  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
518  // we don't want the transform cache to update while we are potentially changing attached bodies
519  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
520 
522  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
523  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
524  "scene update " << fmod(last_update_time_.toSec(), 10.)
525  << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
526  old_scene_name = scene_->getName();
527  result = scene_->usePlanningSceneMsg(scene);
528  if (octomap_monitor_)
529  {
530  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
531  {
532  octomap_monitor_->getOcTreePtr()->lockWrite();
533  octomap_monitor_->getOcTreePtr()->clear();
534  octomap_monitor_->getOcTreePtr()->unlockWrite();
535  }
536  }
537  robot_model_ = scene_->getRobotModel();
538 
539  // if we just reset the scene completely but we were maintaining diffs, we need to fix that
540  if (!scene.is_diff && parent_scene_)
541  {
542  // the scene is now decoupled from the parent, since we just reset it
543  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
544  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
546  scene_ = parent_scene_->diff();
548  scene_->setAttachedBodyUpdateCallback(
550  scene_->setCollisionObjectUpdateCallback(
551  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
552  }
553  if (octomap_monitor_)
554  {
555  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
556  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
557  }
558  }
559 
560  // if we have a diff, try to more accuratelly determine the update type
561  if (scene.is_diff)
562  {
563  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
564  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
565  scene.link_scale.empty();
566  if (no_other_scene_upd)
567  {
568  upd = UPDATE_NONE;
569  if (!planning_scene::PlanningScene::isEmpty(scene.world))
570  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
571 
572  if (!scene.fixed_frame_transforms.empty())
573  upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
574 
575  if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
576  {
577  upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
578  if (!scene.robot_state.attached_collision_objects.empty() || scene.robot_state.is_diff == false)
579  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
580  }
581  }
582  }
584  return result;
585 }
586 
588  const moveit_msgs::PlanningSceneWorldConstPtr& world)
589 {
590  if (scene_)
591  {
593  {
594  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
596  scene_->getWorldNonConst()->clearObjects();
597  scene_->processPlanningSceneWorldMsg(*world);
598  if (octomap_monitor_)
599  {
600  if (world->octomap.octomap.data.empty())
601  {
602  octomap_monitor_->getOcTreePtr()->lockWrite();
603  octomap_monitor_->getOcTreePtr()->clear();
604  octomap_monitor_->getOcTreePtr()->unlockWrite();
605  }
606  }
607  }
609  }
610 }
611 
613  const moveit_msgs::CollisionObjectConstPtr& obj, tf::filter_failure_reasons::FilterFailureReason reason)
614 {
615  // if we just want to remove objects, the frame does not matter
616  if (reason == tf::filter_failure_reasons::EmptyFrameID && obj->operation == moveit_msgs::CollisionObject::REMOVE)
618 }
619 
621  const moveit_msgs::CollisionObjectConstPtr& obj)
622 {
623  if (scene_)
624  {
626  {
627  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
629  scene_->processCollisionObjectMsg(*obj);
630  }
632  }
633 }
634 
636  const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
637 {
638  if (scene_)
639  {
641  {
642  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
644  scene_->processAttachedCollisionObjectMsg(*obj);
645  }
647  }
648 }
649 
651 {
652  if (!octomap_monitor_)
653  return;
654 
655  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
656 
658  const std::vector<const robot_model::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
660  bool warned = false;
661  for (std::size_t i = 0; i < links.size(); ++i)
662  {
663  std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes(); // copy shared ptrs on purpuse
664  for (std::size_t j = 0; j < shapes.size(); ++j)
665  {
666  // merge mesh vertices up to 0.1 mm apart
667  if (shapes[j]->type == shapes::MESH)
668  {
669  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
670  m->mergeVertices(1e-4);
671  shapes[j].reset(m);
672  }
673 
674  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
675  if (h)
676  link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
677  }
678  if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
679  {
680  ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
681  warned = true;
682  }
683  }
684 }
685 
687 {
688  if (!octomap_monitor_)
689  return;
690 
691  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
692 
693  for (LinkShapeHandles::iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
694  for (std::size_t i = 0; i < it->second.size(); ++i)
695  octomap_monitor_->forgetShape(it->second[i].first);
696  link_shape_handles_.clear();
697 }
698 
700 {
701  if (!octomap_monitor_)
702  return;
703 
704  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
705 
706  // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
707  for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin();
708  it != attached_body_shape_handles_.end(); ++it)
709  for (std::size_t k = 0; k < it->second.size(); ++k)
710  octomap_monitor_->forgetShape(it->second[k].first);
712 }
713 
715 {
716  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
717 
719  // add attached objects again
720  std::vector<const robot_state::AttachedBody*> ab;
721  scene_->getCurrentState().getAttachedBodies(ab);
722  for (std::size_t i = 0; i < ab.size(); ++i)
724 }
725 
727 {
728  if (!octomap_monitor_)
729  return;
730 
731  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
732 
733  // clear information about any attached object
734  for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin();
735  it != collision_body_shape_handles_.end(); ++it)
736  for (std::size_t k = 0; k < it->second.size(); ++k)
737  octomap_monitor_->forgetShape(it->second[k].first);
739 }
740 
742 {
743  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
744 
746  for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end();
747  ++it)
748  excludeWorldObjectFromOctree(it->second);
749 }
750 
752  const robot_state::AttachedBody* attached_body)
753 {
754  if (!octomap_monitor_)
755  return;
756 
757  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
758  bool found = false;
759  for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
760  {
761  if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
762  continue;
763  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
764  if (h)
765  {
766  found = true;
767  attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
768  }
769  }
770  if (found)
771  ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
772 }
773 
775  const robot_state::AttachedBody* attached_body)
776 {
777  if (!octomap_monitor_)
778  return;
779 
780  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
781 
782  AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
783  if (it != attached_body_shape_handles_.end())
784  {
785  for (std::size_t k = 0; k < it->second.size(); ++k)
786  octomap_monitor_->forgetShape(it->second[k].first);
787  ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
789  }
790 }
791 
793  const collision_detection::World::ObjectConstPtr& obj)
794 {
795  if (!octomap_monitor_)
796  return;
797 
798  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
799 
800  bool found = false;
801  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
802  {
803  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
804  continue;
805  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
806  if (h)
807  {
808  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
809  found = true;
810  }
811  }
812  if (found)
813  ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
814 }
815 
817  const collision_detection::World::ObjectConstPtr& obj)
818 {
819  if (!octomap_monitor_)
820  return;
821 
822  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
823 
824  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
825  if (it != collision_body_shape_handles_.end())
826  {
827  for (std::size_t k = 0; k < it->second.size(); ++k)
828  octomap_monitor_->forgetShape(it->second[k].first);
829  ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
831  }
832 }
833 
835  robot_state::AttachedBody* attached_body, bool just_attached)
836 {
837  if (!octomap_monitor_)
838  return;
839 
840  if (just_attached)
841  excludeAttachedBodyFromOctree(attached_body);
842  else
843  includeAttachedBodyInOctree(attached_body);
844 }
845 
847  const collision_detection::World::ObjectConstPtr& obj, collision_detection::World::Action action)
848 {
849  if (!octomap_monitor_)
850  return;
852  return;
853 
856  else if (action & collision_detection::World::DESTROY)
858  else
859  {
862  }
863 }
864 
866 {
867  if (t.isZero())
868  return false;
870  ros::WallDuration timeout(wait_time);
871 
872  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
873 
875  {
876  // Wait for next robot update in state monitor.
877  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
878 
879  /* As robot updates are passed to the planning scene only in throttled fashion, there might
880  be still an update pending. If so, explicitly update the planning scene here.
881  If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
882  if (success)
883  {
884  boost::mutex::scoped_lock lock(state_pending_mutex_);
885  if (state_update_pending_) // enforce state update
886  {
887  state_update_pending_ = false;
889  lock.unlock();
891  }
892  return true;
893  }
894 
895  ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
896  return false;
897  }
898 
899  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
900  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
901  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
902  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
903  ros::Time prev_robot_motion_time = last_robot_motion_time_;
904  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
905  timeout > ros::WallDuration())
906  {
907  ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
908  new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
909  timeout -= ros::WallTime::now() - start; // compute remaining wait_time
910  }
911  bool success = last_robot_motion_time_ >= t;
912  // suppress warning if we received an update at all
913  if (!success && prev_robot_motion_time != last_robot_motion_time_)
914  ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
915  (t - last_robot_motion_time_).toSec());
916 
917  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
918  << " scene update: " << (t - last_update_time_).toSec());
919  return success;
920 }
921 
923 {
924  scene_update_mutex_.lock_shared();
925  if (octomap_monitor_)
926  octomap_monitor_->getOcTreePtr()->lockRead();
927 }
928 
930 {
931  if (octomap_monitor_)
932  octomap_monitor_->getOcTreePtr()->unlockRead();
933  scene_update_mutex_.unlock_shared();
934 }
935 
937 {
938  scene_update_mutex_.lock();
939  if (octomap_monitor_)
940  octomap_monitor_->getOcTreePtr()->lockWrite();
941 }
942 
944 {
945  if (octomap_monitor_)
946  octomap_monitor_->getOcTreePtr()->unlockWrite();
947  scene_update_mutex_.unlock();
948 }
949 
951 {
953 
954  ROS_INFO_NAMED(LOGNAME, "Starting scene monitor");
955  // listen for planning scene updates; these messages include transforms, so no need for filters
956  if (!scene_topic.empty())
957  {
960  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
961  }
962 }
963 
965 {
967  {
968  ROS_INFO_NAMED(LOGNAME, "Stopping scene monitor");
970  }
971 }
972 
974  const std::string& target_frame, const ros::Time& target_time,
976 {
977  if (!tf_)
978  return false;
979  try
980  {
981  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
982 
983  for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
984  {
986  tf_->waitForTransform(target_frame, it->first->getName(), target_time, shape_transform_cache_lookup_wait_time_);
987  tf_->lookupTransform(target_frame, it->first->getName(), target_time, tr);
988  Eigen::Affine3d ttr;
989  tf::transformTFToEigen(tr, ttr);
990  for (std::size_t j = 0; j < it->second.size(); ++j)
991  cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
992  }
993  for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin();
994  it != attached_body_shape_handles_.end(); ++it)
995  {
997  tf_->waitForTransform(target_frame, it->first->getAttachedLinkName(), target_time,
999  tf_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time, tr);
1000  Eigen::Affine3d transform;
1001  tf::transformTFToEigen(tr, transform);
1002  for (std::size_t k = 0; k < it->second.size(); ++k)
1003  cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
1004  }
1005  {
1007  tf_->waitForTransform(target_frame, scene_->getPlanningFrame(), target_time,
1009  tf_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time, tr);
1010  Eigen::Affine3d transform;
1011  tf::transformTFToEigen(tr, transform);
1012  for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin();
1013  it != collision_body_shape_handles_.end(); ++it)
1014  for (std::size_t k = 0; k < it->second.size(); ++k)
1015  cache[it->second[k].first] = transform * (*it->second[k].second);
1016  }
1017  }
1018  catch (tf::TransformException& ex)
1019  {
1020  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
1021  return false;
1022  }
1023  return true;
1024 }
1025 
1027  const std::string& collision_objects_topic, const std::string& planning_scene_world_topic,
1028  const bool load_octomap_monitor)
1029 {
1031  ROS_INFO_NAMED(LOGNAME, "Starting world geometry monitor");
1032 
1033  // listen for world geometry updates using message filters
1034  if (!collision_objects_topic.empty())
1035  {
1037  new message_filters::Subscriber<moveit_msgs::CollisionObject>(root_nh_, collision_objects_topic, 1024));
1038  if (tf_)
1039  {
1041  *collision_object_subscriber_, *tf_, scene_->getPlanningFrame(), 1024));
1042  collision_object_filter_->registerCallback(boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
1043  collision_object_filter_->registerFailureCallback(
1044  boost::bind(&PlanningSceneMonitor::collisionObjectFailTFCallback, this, _1, _2));
1045  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message notifier with target frame '%s'",
1046  root_nh_.resolveName(collision_objects_topic).c_str(),
1047  collision_object_filter_->getTargetFramesString().c_str());
1048  }
1049  else
1050  {
1051  collision_object_subscriber_->registerCallback(
1052  boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
1053  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
1054  }
1055  }
1056 
1057  if (!planning_scene_world_topic.empty())
1058  {
1060  root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
1061  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
1062  root_nh_.resolveName(planning_scene_world_topic).c_str());
1063  }
1064 
1065  // Ocotomap monitor is optional
1066  if (load_octomap_monitor)
1067  {
1068  if (!octomap_monitor_)
1069  {
1070  octomap_monitor_.reset(new occupancy_map_monitor::OccupancyMapMonitor(tf_, scene_->getPlanningFrame()));
1074 
1075  octomap_monitor_->setTransformCacheCallback(
1076  boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
1077  octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
1078  }
1079  octomap_monitor_->startMonitor();
1080  }
1081 }
1082 
1084 {
1086  {
1087  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1088  collision_object_filter_.reset();
1091  }
1093  {
1094  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1096  }
1097  if (octomap_monitor_)
1098  octomap_monitor_->stopMonitor();
1099 }
1100 
1101 void planning_scene_monitor::PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1102  const std::string& attached_objects_topic)
1103 {
1104  stopStateMonitor();
1105  if (scene_)
1106  {
1109  current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
1110  current_state_monitor_->startStateMonitor(joint_states_topic);
1111 
1112  {
1113  boost::mutex::scoped_lock lock(state_pending_mutex_);
1114  if (!dt_state_update_.isZero())
1116  }
1117 
1118  if (!attached_objects_topic.empty())
1119  {
1120  // using regular message filter as there's no header
1122  root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
1123  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
1124  root_nh_.resolveName(attached_objects_topic).c_str());
1125  }
1126  }
1127  else
1128  ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
1129 }
1130 
1132 {
1134  current_state_monitor_->stopStateMonitor();
1137 
1138  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1140  {
1141  boost::mutex::scoped_lock lock(state_pending_mutex_);
1142  state_update_pending_ = false;
1143  }
1144 }
1145 
1147  const sensor_msgs::JointStateConstPtr& /* joint_state */)
1148 {
1149  const ros::WallTime& n = ros::WallTime::now();
1151 
1152  bool update = false;
1153  {
1154  boost::mutex::scoped_lock lock(state_pending_mutex_);
1155 
1156  if (dt < dt_state_update_)
1157  {
1158  state_update_pending_ = true;
1159  }
1160  else
1161  {
1162  state_update_pending_ = false;
1163  last_robot_state_update_wall_time_ = n;
1164  update = true;
1165  }
1166  }
1167  // run the state update with state_pending_mutex_ unlocked
1168  if (update)
1170 }
1171 
1173 {
1175  {
1176  bool update = false;
1177 
1178  const ros::WallTime& n = ros::WallTime::now();
1180 
1181  {
1182  // lock for access to dt_state_update_ and state_update_pending_
1183  boost::mutex::scoped_lock lock(state_pending_mutex_);
1185  {
1186  state_update_pending_ = false;
1187  last_robot_state_update_wall_time_ = ros::WallTime::now();
1188  update = true;
1190  "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
1191  }
1192  }
1193 
1194  // run the state update with state_pending_mutex_ unlocked
1195  if (update)
1196  {
1198  ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
1199  }
1200  }
1201 }
1202 
1204 {
1205  if (!octomap_monitor_)
1206  return;
1207 
1209  {
1210  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1212  octomap_monitor_->getOcTreePtr()->lockRead();
1213  try
1214  {
1215  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Affine3d::Identity());
1216  octomap_monitor_->getOcTreePtr()->unlockRead();
1217  }
1218  catch (...)
1219  {
1220  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1221  throw;
1222  }
1223  }
1225 }
1226 
1228 {
1229  bool update = false;
1230  if (hz > std::numeric_limits<double>::epsilon())
1231  {
1232  boost::mutex::scoped_lock lock(state_pending_mutex_);
1233  dt_state_update_.fromSec(1.0 / hz);
1236  }
1237  else
1238  {
1239  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1241  boost::mutex::scoped_lock lock(state_pending_mutex_);
1244  update = true;
1245  }
1246  ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
1247 
1248  if (update)
1250 }
1251 
1253 {
1255  {
1256  std::vector<std::string> missing;
1257  if (!current_state_monitor_->haveCompleteState(missing) &&
1258  (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
1259  {
1260  std::string missing_str = boost::algorithm::join(missing, ", ");
1261  ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known. Missing %s",
1262  missing_str.c_str());
1263  }
1264 
1265  {
1266  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1268  ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
1269  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1270  scene_->getCurrentStateNonConst().update(); // compute all transforms
1271  }
1273  }
1274  else
1275  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
1276 }
1277 
1279 {
1280  boost::recursive_mutex::scoped_lock lock(update_lock_);
1281  if (fn)
1282  update_callbacks_.push_back(fn);
1283 }
1284 
1286 {
1287  boost::recursive_mutex::scoped_lock lock(update_lock_);
1288  update_callbacks_.clear();
1289 }
1290 
1292 {
1294  ROS_DEBUG_NAMED(LOGNAME, "Maximum frquency for publishing a planning scene is now %lf Hz",
1296 }
1297 
1299  std::vector<geometry_msgs::TransformStamped>& transforms)
1300 {
1301  const std::string& target = getRobotModel()->getModelFrame();
1302 
1303  std::vector<std::string> all_frame_names;
1304  tf_->getFrameStrings(all_frame_names);
1305  for (std::size_t i = 0; i < all_frame_names.size(); ++i)
1306  {
1307  const std::string& frame_no_slash = (!all_frame_names[i].empty() && all_frame_names[i][0] == '/') ?
1308  all_frame_names[i].substr(1) :
1309  all_frame_names[i];
1310  const std::string& frame_with_slash =
1311  (!all_frame_names[i].empty() && all_frame_names[i][0] != '/') ? '/' + all_frame_names[i] : all_frame_names[i];
1312 
1313  if (frame_with_slash == target || getRobotModel()->hasLinkModel(frame_no_slash))
1314  continue;
1315 
1316  ros::Time stamp(0);
1317  std::string err_string;
1318  if (tf_->getLatestCommonTime(target, all_frame_names[i], stamp, &err_string) != tf::NO_ERROR)
1319  {
1320  ROS_WARN_STREAM_NAMED(LOGNAME, "No transform available between frame '"
1321  << all_frame_names[i] << "' and planning frame '" << target << "' ("
1322  << err_string << ")");
1323  continue;
1324  }
1325 
1327  try
1328  {
1329  tf_->lookupTransform(target, all_frame_names[i], stamp, t);
1330  }
1331  catch (tf::TransformException& ex)
1332  {
1333  ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
1334  << all_frame_names[i] << "' to planning frame '" << target << "' ("
1335  << ex.what() << ")");
1336  continue;
1337  }
1338  geometry_msgs::TransformStamped f;
1339  f.header.frame_id = frame_with_slash;
1340  f.child_frame_id = target;
1341  f.transform.translation.x = t.getOrigin().x();
1342  f.transform.translation.y = t.getOrigin().y();
1343  f.transform.translation.z = t.getOrigin().z();
1344  const tf::Quaternion& q = t.getRotation();
1345  f.transform.rotation.x = q.x();
1346  f.transform.rotation.y = q.y();
1347  f.transform.rotation.z = q.z();
1348  f.transform.rotation.w = q.w();
1349  transforms.push_back(f);
1350  }
1351 }
1352 
1354 {
1355  if (!tf_)
1356  return;
1357 
1358  if (scene_)
1359  {
1360  std::vector<geometry_msgs::TransformStamped> transforms;
1361  getUpdatedFrameTransforms(transforms);
1362  {
1363  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1364  scene_->getTransformsNonConst().setTransforms(transforms);
1366  }
1368  }
1369 }
1370 
1372 {
1373  if (octomap_monitor_)
1374  octomap_monitor_->publishDebugInformation(flag);
1375 }
1376 
1378  const planning_scene::PlanningScenePtr& scene)
1379 {
1380  if (!scene || robot_description_.empty())
1381  return;
1382  collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
1383 
1384  // read overriding values from the param server
1385 
1386  // first we do default collision operations
1387  if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
1388  ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
1389  else
1390  {
1391  ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
1392 
1393  XmlRpc::XmlRpcValue coll_ops;
1394  nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
1395 
1396  if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
1397  {
1398  ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
1399  return;
1400  }
1401 
1402  if (coll_ops.size() == 0)
1403  {
1404  ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
1405  return;
1406  }
1407 
1408  for (int i = 0; i < coll_ops.size(); ++i)
1409  {
1410  if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
1411  {
1412  ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
1413  continue;
1414  }
1415  acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
1416  std::string(coll_ops[i]["operation"]) == "disable");
1417  }
1418  }
1419 }
1420 
1422 {
1423  if (robot_description_.empty())
1424  {
1425  default_robot_padd_ = 0.0;
1426  default_robot_scale_ = 1.0;
1427  default_object_padd_ = 0.0;
1428  default_attached_padd_ = 0.0;
1429  return;
1430  }
1431 
1432  // Ensure no leading slash creates a bad param server address
1433  static const std::string robot_description =
1434  (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
1435 
1436  nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
1437  nh_.param(robot_description + "_planning/default_robot_scale", default_robot_scale_, 1.0);
1438  nh_.param(robot_description + "_planning/default_object_padding", default_object_padd_, 0.0);
1439  nh_.param(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
1440  nh_.param(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_,
1441  std::map<std::string, double>());
1442  nh_.param(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_,
1443  std::map<std::string, double>());
1444 
1445  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
1446  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
1447 }
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor.
boost::recursive_mutex update_lock_
lock access to update_callbacks_
dynamic_reconfigure::Server< PlanningSceneMonitorDynamicReconfigureConfig > dynamic_reconfigure_server_
std::map< ShapeHandle, Eigen::Affine3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Affine3d > > > ShapeTransformCache
void clearUpdateCallbacks()
Clear the functions to be called when an update to the scene is received.
#define ROS_INFO_NAMED(name,...)
#define ROS_WARN_THROTTLE_NAMED(rate, name,...)
void mergeVertices(double threshold)
void getUpdatedFrameTransforms(std::vector< geometry_msgs::TransformStamped > &transforms)
#define ROS_DEBUG_STREAM_NAMED(name, args)
void publish(const boost::shared_ptr< M > &message) const
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
The geometry of the scene was updated. This includes receiving new octomaps, collision objects...
#define ROS_WARN_NAMED(name,...)
static const std::string LOGNAME
void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig &config, uint32_t level)
robot_model_loader::RobotModelLoaderPtr rm_loader_
f
void lockSceneWrite()
Lock the scene for writing (only one thread can lock for writing and no other thread can lock for rea...
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
static const std::string DEFAULT_PLANNING_SCENE_SERVICE
The name of the service used by default for requesting full planning scene state. ...
int size() const
void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state)
void setPeriod(const WallDuration &period, bool reset=true)
std::string resolveName(const std::string &name, bool remap=true) const
ros::Time last_robot_motion_time_
Last time the state was updated.
void addUpdateCallback(const boost::function< void(SceneUpdateType)> &fn)
Add a function to be called when an update to the scene is received.
boost::shared_lock< boost::shared_mutex > ReadLock
Monitors the joint_states topic and tf to maintain the current state of the robot.
bool call(MReq &req, MRes &res)
const robot_model::RobotModelConstPtr & getRobotModel() const
ros::Duration shape_transform_cache_lookup_wait_time_
the amount of time to wait when looking up transforms
std::string getName(void *handle)
void octomapUpdateCallback()
Callback for octomap updates.
void configureDefaultPadding()
Configure the default padding.
void notify_all() BOOST_NOEXCEPT
void triggerSceneUpdateEvent(SceneUpdateType update_type)
This function is called every time there is a change to the planning scene.
void stopPublishingPlanningScene()
Stop publishing the maintained planning scene.
void lockSceneRead()
Lock the scene for reading (multiple threads can lock for reading at the same time) ...
std::map< std::string, ObjectPtr >::const_iterator const_iterator
Type const & getType() const
static const std::string OCTOMAP_NS
void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr &obj)
Callback for a new collision object msg.
void unlockSceneWrite()
Lock the scene from writing (only one thread can lock for writing and no other thread can lock for re...
PlanningSceneMonitor(const std::string &robot_description, const boost::shared_ptr< tf::Transformer > &tf=boost::shared_ptr< tf::Transformer >(), const std::string &name="")
Constructor.
The state in the monitored scene was updated.
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
std::unique_ptr< message_filters::Subscriber< moveit_msgs::CollisionObject > > collision_object_subscriber_
void setStateUpdateFrequency(double hz)
Update the scene using the monitored state at a specified frequency, in Hz. This function has an effe...
void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody *attached_body, bool just_attached)
Callback for a change for an attached object of the current state of the planning scene...
bool updatesScene(const planning_scene::PlanningSceneConstPtr &scene) const
Return true if the scene scene can be updated directly or indirectly by this monitor. This function will return true if the pointer of the scene is the same as the one maintained, or if a parent of the scene is the one maintained.
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
TFSIMD_FORCE_INLINE const tfScalar & x() const
void excludeAttachedBodyFromOctree(const robot_state::AttachedBody *attached_body)
#define ROS_DEBUG_NAMED(name,...)
void reset()
void setupScene(ros::NodeHandle &nh, const planning_scene::PlanningScenePtr &scene)
This can be called on a new planning scene to setup the collision detector.
unsigned int ShapeHandle
std::unique_ptr< boost::thread > publish_planning_scene_
void getMonitoredTopics(std::vector< std::string > &topics) const
Get the topic names that the monitor is listening to.
void updateFrameTransforms()
Update the transforms for the frames that are not part of the kinematic model using tf...
ros::WallTime last_robot_state_update_wall_time_
Last time the state was updated from current_state_monitor_.
std::unique_ptr< tf::MessageFilter< moveit_msgs::CollisionObject > > collision_object_filter_
TFSIMD_FORCE_INLINE const tfScalar & z() const
void startWorldGeometryMonitor(const std::string &collision_objects_topic=DEFAULT_COLLISION_OBJECT_TOPIC, const std::string &planning_scene_world_topic=DEFAULT_PLANNING_SCENE_WORLD_TOPIC, const bool load_octomap_monitor=true)
Start listening for objects in the world, the collision map and attached collision objects...
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
WallDuration & fromSec(double t)
std::unique_ptr< occupancy_map_monitor::OccupancyMapMonitor > octomap_monitor_
PlanningSceneMonitor Subscribes to the topic planning_scene.
std::map< std::string, double > default_robot_link_scale_
default robot link scale
bool param(const std::string &param_name, T &param_val, const T &default_val) const
bool waitForCurrentRobotState(const ros::Time &t, double wait_time=1.)
Wait for robot state to become more recent than time t.
ros::NodeHandle nh_
Last time the robot has moved.
void initialize(const planning_scene::PlanningScenePtr &scene)
Initialize the planning scene monitor.
std::string getTopic() const
static const std::string DEFAULT_PLANNING_SCENE_TOPIC
The name of the topic used by default for receiving full planning scenes or planning scene diffs...
TFSIMD_FORCE_INLINE const tfScalar & y() const
void updateSceneWithCurrentState()
Update the scene using the monitored state. This function is automatically called when an update to t...
ros::WallTimer state_update_timer_
timer for state updates.
#define ROS_ERROR_THROTTLE_NAMED(rate, name,...)
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void startStateMonitor(const std::string &joint_states_topic=DEFAULT_JOINT_STATES_TOPIC, const std::string &attached_objects_topic=DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC)
Start the current state monitor.
void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene)
Configure the collision matrix for a particular scene.
boost::condition_variable_any new_scene_update_condition_
boost::shared_ptr< tf::Transformer > tf_
void startPublishingPlanningScene(SceneUpdateType event, const std::string &planning_scene_topic=MONITORED_PLANNING_SCENE_TOPIC)
Start publishing the maintained planning scene. The first message set out is a complete planning scen...
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
bool sleep()
void unlockSceneRead()
Unlock the scene from reading (multiple threads can lock for reading at the same time) ...
void stateUpdateTimerCallback(const ros::WallTimerEvent &event)
ros::Time last_update_time_
mutex for stored scene
static const std::string DEFAULT_COLLISION_OBJECT_TOPIC
The name of the topic used by default for receiving collision objects in the world.
bool hasMember(const std::string &name) const
std::vector< boost::function< void(SceneUpdateType)> > update_callbacks_
void collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr &obj, tf::filter_failure_reasons::FilterFailureReason reason)
Callback for a new collision object msg that failed to pass the TF filter.
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
static WallTime now()
Quaternion getRotation() const
boost::shared_mutex scene_update_mutex_
if diffs are monitored, this is the pointer to the parent scene
void monitorDiffs(bool flag)
By default, the maintained planning scene does not reason about diffs. When the flag passed in is tru...
planning_scene::PlanningScenePtr parent_scene_
bool getParam(const std::string &key, std::string &s) const
bool getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const
void stopWorldGeometryMonitor()
Stop the world geometry monitor.
void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj)
bool requestPlanningSceneState(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
Request planning scene state using a service call.
#define ROS_ERROR_NAMED(name,...)
void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr &obj)
The maintained set of fixed transforms in the monitored scene was updated.
static Time now()
double default_attached_padd_
default attached padding
std::string monitor_name_
The name of this scene monitor.
void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e)
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
bool newPlanningSceneMessage(const moveit_msgs::PlanningScene &scene)
bool hasParam(const std::string &key) const
void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr &object, collision_detection::World::Action action)
Callback for a change in the world maintained by the planning scene.
void setPlanningScenePublishingFrequency(double hz)
Set the maximum frequency at which planning scenes are being published.
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
The name of the topic used by default for attached collision objects.
std::map< std::string, double > default_robot_link_padd_
default robot link padding
volatile bool state_update_pending_
True when we need to update the RobotState from current_state_monitor_.
void includeAttachedBodyInOctree(const robot_state::AttachedBody *attached_body)
void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene)
void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr &obj)
Callback for a new attached object msg.
planning_scene::PlanningSceneConstPtr scene_const_
void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world)
Callback for a new planning scene world.
static const std::string DEFAULT_JOINT_STATES_TOPIC
The name of the topic used by default for receiving joint states.
ros::WallDuration dt_state_update_
the amount of time to wait in between updates to the robot state
collision_detection::CollisionPluginLoader collision_loader_
NO_ERROR
#define ROS_WARN_STREAM_NAMED(name, args)


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Thu Jul 12 2018 02:54:07