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  : PlanningSceneMonitor(planning_scene::PlanningScenePtr(), robot_description, tf, name)
121 {
122 }
123 
124 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
125  const std::string& robot_description,
127  const std::string& name)
128  : PlanningSceneMonitor(scene, std::make_shared<robot_model_loader::RobotModelLoader>(robot_description), tf, name)
129 {
130 }
131 
133  const robot_model_loader::RobotModelLoaderPtr& rm_loader, const boost::shared_ptr<tf::Transformer>& tf,
134  const std::string& name)
135  : PlanningSceneMonitor(planning_scene::PlanningScenePtr(), rm_loader, tf, name)
136 {
137 }
138 
140  const planning_scene::PlanningScenePtr& scene, const robot_model_loader::RobotModelLoaderPtr& rm_loader,
141  const boost::shared_ptr<tf::Transformer>& tf, const std::string& name)
142  : monitor_name_(name), nh_("~"), tf_(tf), rm_loader_(rm_loader)
143 {
146  spinner_.reset(new ros::AsyncSpinner(1, &queue_));
147  spinner_->start();
148  initialize(scene);
149 }
150 
152  const planning_scene::PlanningScenePtr& scene, const robot_model_loader::RobotModelLoaderPtr& rm_loader,
153  const ros::NodeHandle& nh, const boost::shared_ptr<tf::Transformer>& tf, const std::string& name)
154  : monitor_name_(name), nh_("~"), root_nh_(nh), tf_(tf), rm_loader_(rm_loader)
155 {
156  // use same callback queue as root_nh_
158  initialize(scene);
159 }
160 
162 {
163  if (scene_)
164  {
165  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
166  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
167  }
172 
173  spinner_.reset();
174  delete reconfigure_impl_;
175  current_state_monitor_.reset();
176  scene_const_.reset();
177  scene_.reset();
178  parent_scene_.reset();
179  robot_model_.reset();
180  rm_loader_.reset();
181 }
182 
183 void planning_scene_monitor::PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& scene)
184 {
186  moveit::tools::Profiler::ScopedBlock prof_block("PlanningSceneMonitor::initialize");
187 
188  if (monitor_name_.empty())
189  monitor_name_ = "planning_scene_monitor";
190  robot_description_ = rm_loader_->getRobotDescription();
191  if (rm_loader_->getModel())
192  {
193  robot_model_ = rm_loader_->getModel();
194  scene_ = scene;
197  if (!scene_)
198  {
199  try
200  {
201  scene_.reset(new planning_scene::PlanningScene(rm_loader_->getModel()));
206 
207  scene_->getCollisionRobotNonConst()->setPadding(default_robot_padd_);
208  scene_->getCollisionRobotNonConst()->setScale(default_robot_scale_);
209  for (std::map<std::string, double>::iterator it = default_robot_link_padd_.begin();
210  it != default_robot_link_padd_.end(); ++it)
211  {
212  scene_->getCollisionRobotNonConst()->setLinkPadding(it->first, it->second);
213  }
214  for (std::map<std::string, double>::iterator it = default_robot_link_scale_.begin();
215  it != default_robot_link_scale_.end(); ++it)
216  {
217  scene_->getCollisionRobotNonConst()->setLinkScale(it->first, it->second);
218  }
219  scene_->propogateRobotPadding();
220  }
221  catch (moveit::ConstructException& e)
222  {
223  ROS_ERROR_NAMED(LOGNAME, "Configuration of planning scene failed");
224  scene_.reset();
226  }
227  }
228  if (scene_)
229  {
230  scene_->setAttachedBodyUpdateCallback(
232  scene_->setCollisionObjectUpdateCallback(
233  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
234  }
235  }
236  else
237  {
238  ROS_ERROR_NAMED(LOGNAME, "Robot model not loaded");
239  }
240 
243 
247 
248  double temp_wait_time = 0.05;
249 
250  if (!robot_description_.empty())
251  nh_.param(robot_description_ + "_planning/shape_transform_cache_lookup_wait_time", temp_wait_time, temp_wait_time);
252 
254 
255  state_update_pending_ = false;
257  false, // not a oneshot timer
258  false); // do not start the timer yet
259 
261 }
262 
264 {
265  if (scene_)
266  {
267  if (flag)
268  {
269  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
270  if (scene_)
271  {
272  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
273  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
274  scene_->decoupleParent();
276  scene_ = parent_scene_->diff();
278  scene_->setAttachedBodyUpdateCallback(
280  scene_->setCollisionObjectUpdateCallback(
281  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
282  }
283  }
284  else
285  {
287  {
288  ROS_WARN_NAMED(LOGNAME, "Diff monitoring was stopped while publishing planning scene diffs. "
289  "Stopping planning scene diff publisher");
291  }
292  {
293  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
294  if (scene_)
295  {
296  scene_->decoupleParent();
297  parent_scene_.reset();
298  // remove the '+' added by .diff() at the end of the scene name
299  if (!scene_->getName().empty())
300  {
301  if (scene_->getName()[scene_->getName().length() - 1] == '+')
302  scene_->setName(scene_->getName().substr(0, scene_->getName().length() - 1));
303  }
304  }
305  }
306  }
307  }
308 }
309 
311 {
313  {
314  std::unique_ptr<boost::thread> copy;
315  copy.swap(publish_planning_scene_);
317  copy->join();
318  monitorDiffs(false);
320  ROS_INFO_NAMED(LOGNAME, "Stopped publishing maintained planning scene.");
321  }
322 }
323 
325  const std::string& planning_scene_topic)
326 {
327  publish_update_types_ = update_type;
329  {
330  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>(planning_scene_topic, 100, false);
331  ROS_INFO_NAMED(LOGNAME, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
332  monitorDiffs(true);
333  publish_planning_scene_.reset(new boost::thread(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this)));
334  }
335 }
336 
338 {
339  ROS_DEBUG_NAMED(LOGNAME, "Started scene publishing thread ...");
340 
341  // publish the full planning scene once
342  {
343  moveit_msgs::PlanningScene msg;
344  {
346  if (octomap_monitor_)
347  lock = octomap_monitor_->getOcTreePtr()->reading();
348  scene_->getPlanningSceneMsg(msg);
349  }
351  ROS_DEBUG_NAMED(LOGNAME, "Published the full planning scene: '%s'", msg.name.c_str());
352  }
353 
354  do
355  {
356  moveit_msgs::PlanningScene msg;
357  bool publish_msg = false;
358  bool is_full = false;
360  {
361  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
365  {
366  if ((publish_update_types_ & new_scene_update_) || new_scene_update_ == UPDATE_SCENE)
367  {
368  if (new_scene_update_ == UPDATE_SCENE)
369  is_full = true;
370  else
371  {
373  if (octomap_monitor_)
374  lock = octomap_monitor_->getOcTreePtr()->reading();
375  scene_->getPlanningSceneDiffMsg(msg);
376  }
377  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the
378  // transform cache to
379  // update while we are
380  // potentially changing
381  // attached bodies
382  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
383  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
384  scene_->pushDiffs(parent_scene_);
385  scene_->clearDiffs();
386  scene_->setAttachedBodyUpdateCallback(
388  scene_->setCollisionObjectUpdateCallback(
389  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
390  if (octomap_monitor_)
391  {
392  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
393  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
394  }
395  if (is_full)
396  {
398  if (octomap_monitor_)
399  lock = octomap_monitor_->getOcTreePtr()->reading();
400  scene_->getPlanningSceneMsg(msg);
401  }
402  // also publish timestamp of this robot_state
403  msg.robot_state.joint_state.header.stamp = last_robot_motion_time_;
404  publish_msg = true;
405  }
406  new_scene_update_ = UPDATE_NONE;
407  }
408  }
409  if (publish_msg)
410  {
411  rate.reset();
413  if (is_full)
414  ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str());
415  rate.sleep();
416  }
417  } while (publish_planning_scene_);
418 }
419 
420 void planning_scene_monitor::PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string>& topics) const
421 {
422  topics.clear();
424  {
425  const std::string& t = current_state_monitor_->getMonitoredTopic();
426  if (!t.empty())
427  topics.push_back(t);
428  }
430  topics.push_back(planning_scene_subscriber_.getTopic());
432  topics.push_back(collision_object_subscriber_->getTopic());
434  topics.push_back(planning_scene_world_subscriber_.getTopic());
435 }
436 
437 namespace
438 {
439 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr& scene,
440  const planning_scene::PlanningScene* possible_parent)
441 {
442  if (scene && scene.get() == possible_parent)
443  return true;
444  if (scene)
445  return sceneIsParentOf(scene->getParent(), possible_parent);
446  return false;
447 }
448 }
449 
450 bool planning_scene_monitor::PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr& scene) const
451 {
452  return sceneIsParentOf(scene_const_, scene.get());
453 }
454 
456  const planning_scene::PlanningSceneConstPtr& scene) const
457 {
458  return sceneIsParentOf(scene_const_, scene.get());
459 }
460 
462 {
463  // do not modify update functions while we are calling them
464  boost::recursive_mutex::scoped_lock lock(update_lock_);
465 
466  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
467  update_callbacks_[i](update_type);
468  new_scene_update_ = (SceneUpdateType)((int)new_scene_update_ | (int)update_type);
470 }
471 
473 {
474  // use global namespace for service
475  ros::ServiceClient client = ros::NodeHandle().serviceClient<moveit_msgs::GetPlanningScene>(service_name);
476  moveit_msgs::GetPlanningScene srv;
477  srv.request.components.components =
478  srv.request.components.SCENE_SETTINGS | srv.request.components.ROBOT_STATE |
479  srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS | srv.request.components.WORLD_OBJECT_NAMES |
480  srv.request.components.WORLD_OBJECT_GEOMETRY | srv.request.components.OCTOMAP |
481  srv.request.components.TRANSFORMS | srv.request.components.ALLOWED_COLLISION_MATRIX |
482  srv.request.components.LINK_PADDING_AND_SCALING | srv.request.components.OBJECT_COLORS;
483 
484  // Make sure client is connected to server
485  if (!client.exists())
486  {
487  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for service `" << service_name << "` to exist.");
488  client.waitForExistence(ros::Duration(5.0));
489  }
490 
491  if (client.call(srv))
492  {
493  newPlanningSceneMessage(srv.response.scene);
494  }
495  else
496  {
497  ROS_INFO_NAMED(LOGNAME, "Failed to call service %s, have you launched move_group? at %s:%d", service_name.c_str(),
498  __FILE__, __LINE__);
499  return false;
500  }
501  return true;
502 }
503 
505  const moveit_msgs::PlanningSceneConstPtr& scene)
506 {
507  newPlanningSceneMessage(*scene);
508 }
509 
511 {
512  octomap_monitor_->getOcTreePtr()->lockWrite();
513  octomap_monitor_->getOcTreePtr()->clear();
514  octomap_monitor_->getOcTreePtr()->unlockWrite();
515 }
516 
518 {
519  if (!scene_)
520  return false;
521 
522  bool result;
523 
525  std::string old_scene_name;
526  {
527  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
528  // we don't want the transform cache to update while we are potentially changing attached bodies
529  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
530 
532  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
533  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
534  "scene update " << fmod(last_update_time_.toSec(), 10.)
535  << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
536  old_scene_name = scene_->getName();
537  result = scene_->usePlanningSceneMsg(scene);
538  if (octomap_monitor_)
539  {
540  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
541  {
542  octomap_monitor_->getOcTreePtr()->lockWrite();
543  octomap_monitor_->getOcTreePtr()->clear();
544  octomap_monitor_->getOcTreePtr()->unlockWrite();
545  }
546  }
547  robot_model_ = scene_->getRobotModel();
548 
549  // if we just reset the scene completely but we were maintaining diffs, we need to fix that
550  if (!scene.is_diff && parent_scene_)
551  {
552  // the scene is now decoupled from the parent, since we just reset it
553  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
554  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
556  scene_ = parent_scene_->diff();
558  scene_->setAttachedBodyUpdateCallback(
560  scene_->setCollisionObjectUpdateCallback(
561  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
562  }
563  if (octomap_monitor_)
564  {
565  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
566  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
567  }
568  }
569 
570  // if we have a diff, try to more accuratelly determine the update type
571  if (scene.is_diff)
572  {
573  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
574  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
575  scene.link_scale.empty();
576  if (no_other_scene_upd)
577  {
578  upd = UPDATE_NONE;
579  if (!planning_scene::PlanningScene::isEmpty(scene.world))
580  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
581 
582  if (!scene.fixed_frame_transforms.empty())
583  upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
584 
585  if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
586  {
587  upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
588  if (!scene.robot_state.attached_collision_objects.empty() || scene.robot_state.is_diff == false)
589  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
590  }
591  }
592  }
594  return result;
595 }
596 
598  const moveit_msgs::PlanningSceneWorldConstPtr& world)
599 {
600  if (scene_)
601  {
603  {
604  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
606  scene_->getWorldNonConst()->clearObjects();
607  scene_->processPlanningSceneWorldMsg(*world);
608  if (octomap_monitor_)
609  {
610  if (world->octomap.octomap.data.empty())
611  {
612  octomap_monitor_->getOcTreePtr()->lockWrite();
613  octomap_monitor_->getOcTreePtr()->clear();
614  octomap_monitor_->getOcTreePtr()->unlockWrite();
615  }
616  }
617  }
619  }
620 }
621 
623  const moveit_msgs::CollisionObjectConstPtr& obj, tf::filter_failure_reasons::FilterFailureReason reason)
624 {
625  // if we just want to remove objects, the frame does not matter
626  if (reason == tf::filter_failure_reasons::EmptyFrameID && obj->operation == moveit_msgs::CollisionObject::REMOVE)
628 }
629 
631  const moveit_msgs::CollisionObjectConstPtr& obj)
632 {
633  if (scene_)
634  {
636  {
637  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
639  scene_->processCollisionObjectMsg(*obj);
640  }
642  }
643 }
644 
646  const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
647 {
648  if (scene_)
649  {
651  {
652  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
654  scene_->processAttachedCollisionObjectMsg(*obj);
655  }
657  }
658 }
659 
661 {
662  if (!octomap_monitor_)
663  return;
664 
665  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
666 
668  const std::vector<const robot_model::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
670  bool warned = false;
671  for (std::size_t i = 0; i < links.size(); ++i)
672  {
673  std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes(); // copy shared ptrs on purpuse
674  for (std::size_t j = 0; j < shapes.size(); ++j)
675  {
676  // merge mesh vertices up to 0.1 mm apart
677  if (shapes[j]->type == shapes::MESH)
678  {
679  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
680  m->mergeVertices(1e-4);
681  shapes[j].reset(m);
682  }
683 
684  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
685  if (h)
686  link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
687  }
688  if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
689  {
690  ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
691  warned = true;
692  }
693  }
694 }
695 
697 {
698  if (!octomap_monitor_)
699  return;
700 
701  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
702 
703  for (LinkShapeHandles::iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
704  for (std::size_t i = 0; i < it->second.size(); ++i)
705  octomap_monitor_->forgetShape(it->second[i].first);
706  link_shape_handles_.clear();
707 }
708 
710 {
711  if (!octomap_monitor_)
712  return;
713 
714  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
715 
716  // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
717  for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin();
718  it != attached_body_shape_handles_.end(); ++it)
719  for (std::size_t k = 0; k < it->second.size(); ++k)
720  octomap_monitor_->forgetShape(it->second[k].first);
722 }
723 
725 {
726  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
727 
729  // add attached objects again
730  std::vector<const robot_state::AttachedBody*> ab;
731  scene_->getCurrentState().getAttachedBodies(ab);
732  for (std::size_t i = 0; i < ab.size(); ++i)
734 }
735 
737 {
738  if (!octomap_monitor_)
739  return;
740 
741  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
742 
743  // clear information about any attached object
744  for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin();
745  it != collision_body_shape_handles_.end(); ++it)
746  for (std::size_t k = 0; k < it->second.size(); ++k)
747  octomap_monitor_->forgetShape(it->second[k].first);
749 }
750 
752 {
753  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
754 
756  for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end();
757  ++it)
758  excludeWorldObjectFromOctree(it->second);
759 }
760 
762  const robot_state::AttachedBody* attached_body)
763 {
764  if (!octomap_monitor_)
765  return;
766 
767  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
768  bool found = false;
769  for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
770  {
771  if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
772  continue;
773  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
774  if (h)
775  {
776  found = true;
777  attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
778  }
779  }
780  if (found)
781  ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
782 }
783 
785  const robot_state::AttachedBody* attached_body)
786 {
787  if (!octomap_monitor_)
788  return;
789 
790  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
791 
792  AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
793  if (it != attached_body_shape_handles_.end())
794  {
795  for (std::size_t k = 0; k < it->second.size(); ++k)
796  octomap_monitor_->forgetShape(it->second[k].first);
797  ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
799  }
800 }
801 
803  const collision_detection::World::ObjectConstPtr& obj)
804 {
805  if (!octomap_monitor_)
806  return;
807 
808  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
809 
810  bool found = false;
811  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
812  {
813  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
814  continue;
815  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
816  if (h)
817  {
818  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
819  found = true;
820  }
821  }
822  if (found)
823  ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
824 }
825 
827  const collision_detection::World::ObjectConstPtr& obj)
828 {
829  if (!octomap_monitor_)
830  return;
831 
832  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
833 
834  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
835  if (it != collision_body_shape_handles_.end())
836  {
837  for (std::size_t k = 0; k < it->second.size(); ++k)
838  octomap_monitor_->forgetShape(it->second[k].first);
839  ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
841  }
842 }
843 
845  robot_state::AttachedBody* attached_body, bool just_attached)
846 {
847  if (!octomap_monitor_)
848  return;
849 
850  if (just_attached)
851  excludeAttachedBodyFromOctree(attached_body);
852  else
853  includeAttachedBodyInOctree(attached_body);
854 }
855 
857  const collision_detection::World::ObjectConstPtr& obj, collision_detection::World::Action action)
858 {
859  if (!octomap_monitor_)
860  return;
862  return;
863 
866  else if (action & collision_detection::World::DESTROY)
868  else
869  {
872  }
873 }
874 
876 {
877  if (t.isZero())
878  return false;
880  ros::WallDuration timeout(wait_time);
881 
882  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
883 
885  {
886  // Wait for next robot update in state monitor.
887  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
888 
889  /* As robot updates are passed to the planning scene only in throttled fashion, there might
890  be still an update pending. If so, explicitly update the planning scene here.
891  If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
892  if (success)
893  {
894  boost::mutex::scoped_lock lock(state_pending_mutex_);
895  if (state_update_pending_) // enforce state update
896  {
897  state_update_pending_ = false;
899  lock.unlock();
901  }
902  return true;
903  }
904 
905  ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
906  return false;
907  }
908 
909  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
910  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
911  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
912  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
913  ros::Time prev_robot_motion_time = last_robot_motion_time_;
914  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
915  timeout > ros::WallDuration())
916  {
917  ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
918  new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
919  timeout -= ros::WallTime::now() - start; // compute remaining wait_time
920  }
921  bool success = last_robot_motion_time_ >= t;
922  // suppress warning if we received an update at all
923  if (!success && prev_robot_motion_time != last_robot_motion_time_)
924  ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
925  (t - last_robot_motion_time_).toSec());
926 
927  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
928  << " scene update: " << (t - last_update_time_).toSec());
929  return success;
930 }
931 
933 {
934  scene_update_mutex_.lock_shared();
935  if (octomap_monitor_)
936  octomap_monitor_->getOcTreePtr()->lockRead();
937 }
938 
940 {
941  if (octomap_monitor_)
942  octomap_monitor_->getOcTreePtr()->unlockRead();
943  scene_update_mutex_.unlock_shared();
944 }
945 
947 {
948  scene_update_mutex_.lock();
949  if (octomap_monitor_)
950  octomap_monitor_->getOcTreePtr()->lockWrite();
951 }
952 
954 {
955  if (octomap_monitor_)
956  octomap_monitor_->getOcTreePtr()->unlockWrite();
957  scene_update_mutex_.unlock();
958 }
959 
961 {
963 
964  ROS_INFO_NAMED(LOGNAME, "Starting scene monitor");
965  // listen for planning scene updates; these messages include transforms, so no need for filters
966  if (!scene_topic.empty())
967  {
970  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
971  }
972 }
973 
975 {
977  {
978  ROS_INFO_NAMED(LOGNAME, "Stopping scene monitor");
980  }
981 }
982 
984  const std::string& target_frame, const ros::Time& target_time,
986 {
987  if (!tf_)
988  return false;
989  try
990  {
991  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
992 
993  for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
994  {
996  tf_->waitForTransform(target_frame, it->first->getName(), target_time, shape_transform_cache_lookup_wait_time_);
997  tf_->lookupTransform(target_frame, it->first->getName(), target_time, tr);
998  Eigen::Affine3d ttr;
999  tf::transformTFToEigen(tr, ttr);
1000  for (std::size_t j = 0; j < it->second.size(); ++j)
1001  cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
1002  }
1003  for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin();
1004  it != attached_body_shape_handles_.end(); ++it)
1005  {
1007  tf_->waitForTransform(target_frame, it->first->getAttachedLinkName(), target_time,
1009  tf_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time, tr);
1010  Eigen::Affine3d transform;
1011  tf::transformTFToEigen(tr, transform);
1012  for (std::size_t k = 0; k < it->second.size(); ++k)
1013  cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
1014  }
1015  {
1017  tf_->waitForTransform(target_frame, scene_->getPlanningFrame(), target_time,
1019  tf_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time, tr);
1020  Eigen::Affine3d transform;
1021  tf::transformTFToEigen(tr, transform);
1022  for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin();
1023  it != collision_body_shape_handles_.end(); ++it)
1024  for (std::size_t k = 0; k < it->second.size(); ++k)
1025  cache[it->second[k].first] = transform * (*it->second[k].second);
1026  }
1027  }
1028  catch (tf::TransformException& ex)
1029  {
1030  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
1031  return false;
1032  }
1033  return true;
1034 }
1035 
1037  const std::string& collision_objects_topic, const std::string& planning_scene_world_topic,
1038  const bool load_octomap_monitor)
1039 {
1041  ROS_INFO_NAMED(LOGNAME, "Starting world geometry monitor");
1042 
1043  // listen for world geometry updates using message filters
1044  if (!collision_objects_topic.empty())
1045  {
1047  new message_filters::Subscriber<moveit_msgs::CollisionObject>(root_nh_, collision_objects_topic, 1024));
1048  if (tf_)
1049  {
1051  *collision_object_subscriber_, *tf_, scene_->getPlanningFrame(), 1024));
1052  collision_object_filter_->registerCallback(boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
1053  collision_object_filter_->registerFailureCallback(
1054  boost::bind(&PlanningSceneMonitor::collisionObjectFailTFCallback, this, _1, _2));
1055  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message notifier with target frame '%s'",
1056  root_nh_.resolveName(collision_objects_topic).c_str(),
1057  collision_object_filter_->getTargetFramesString().c_str());
1058  }
1059  else
1060  {
1061  collision_object_subscriber_->registerCallback(
1062  boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
1063  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
1064  }
1065  }
1066 
1067  if (!planning_scene_world_topic.empty())
1068  {
1070  root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
1071  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
1072  root_nh_.resolveName(planning_scene_world_topic).c_str());
1073  }
1074 
1075  // Ocotomap monitor is optional
1076  if (load_octomap_monitor)
1077  {
1078  if (!octomap_monitor_)
1079  {
1080  octomap_monitor_.reset(new occupancy_map_monitor::OccupancyMapMonitor(tf_, scene_->getPlanningFrame()));
1084 
1085  octomap_monitor_->setTransformCacheCallback(
1086  boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
1087  octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
1088  }
1089  octomap_monitor_->startMonitor();
1090  }
1091 }
1092 
1094 {
1096  {
1097  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1098  collision_object_filter_.reset();
1101  }
1103  {
1104  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1106  }
1107  if (octomap_monitor_)
1108  octomap_monitor_->stopMonitor();
1109 }
1110 
1111 void planning_scene_monitor::PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1112  const std::string& attached_objects_topic)
1113 {
1114  stopStateMonitor();
1115  if (scene_)
1116  {
1119  current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
1120  current_state_monitor_->startStateMonitor(joint_states_topic);
1121 
1122  {
1123  boost::mutex::scoped_lock lock(state_pending_mutex_);
1124  if (!dt_state_update_.isZero())
1126  }
1127 
1128  if (!attached_objects_topic.empty())
1129  {
1130  // using regular message filter as there's no header
1132  root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
1133  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
1134  root_nh_.resolveName(attached_objects_topic).c_str());
1135  }
1136  }
1137  else
1138  ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
1139 }
1140 
1142 {
1144  current_state_monitor_->stopStateMonitor();
1147 
1148  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1150  {
1151  boost::mutex::scoped_lock lock(state_pending_mutex_);
1152  state_update_pending_ = false;
1153  }
1154 }
1155 
1157  const sensor_msgs::JointStateConstPtr& /* joint_state */)
1158 {
1159  const ros::WallTime& n = ros::WallTime::now();
1161 
1162  bool update = false;
1163  {
1164  boost::mutex::scoped_lock lock(state_pending_mutex_);
1165 
1166  if (dt < dt_state_update_)
1167  {
1168  state_update_pending_ = true;
1169  }
1170  else
1171  {
1172  state_update_pending_ = false;
1173  last_robot_state_update_wall_time_ = n;
1174  update = true;
1175  }
1176  }
1177  // run the state update with state_pending_mutex_ unlocked
1178  if (update)
1180 }
1181 
1183 {
1185  {
1186  bool update = false;
1187 
1188  const ros::WallTime& n = ros::WallTime::now();
1190 
1191  {
1192  // lock for access to dt_state_update_ and state_update_pending_
1193  boost::mutex::scoped_lock lock(state_pending_mutex_);
1195  {
1196  state_update_pending_ = false;
1197  last_robot_state_update_wall_time_ = ros::WallTime::now();
1198  update = true;
1200  "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
1201  }
1202  }
1203 
1204  // run the state update with state_pending_mutex_ unlocked
1205  if (update)
1206  {
1208  ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
1209  }
1210  }
1211 }
1212 
1214 {
1215  if (!octomap_monitor_)
1216  return;
1217 
1219  {
1220  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1222  octomap_monitor_->getOcTreePtr()->lockRead();
1223  try
1224  {
1225  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Affine3d::Identity());
1226  octomap_monitor_->getOcTreePtr()->unlockRead();
1227  }
1228  catch (...)
1229  {
1230  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1231  throw;
1232  }
1233  }
1235 }
1236 
1238 {
1239  bool update = false;
1240  if (hz > std::numeric_limits<double>::epsilon())
1241  {
1242  boost::mutex::scoped_lock lock(state_pending_mutex_);
1243  dt_state_update_.fromSec(1.0 / hz);
1246  }
1247  else
1248  {
1249  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1251  boost::mutex::scoped_lock lock(state_pending_mutex_);
1254  update = true;
1255  }
1256  ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
1257 
1258  if (update)
1260 }
1261 
1263 {
1265  {
1266  std::vector<std::string> missing;
1267  if (!current_state_monitor_->haveCompleteState(missing) &&
1268  (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
1269  {
1270  std::string missing_str = boost::algorithm::join(missing, ", ");
1271  ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known. Missing %s",
1272  missing_str.c_str());
1273  }
1274 
1275  {
1276  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1278  ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
1279  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1280  scene_->getCurrentStateNonConst().update(); // compute all transforms
1281  }
1283  }
1284  else
1285  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
1286 }
1287 
1289 {
1290  boost::recursive_mutex::scoped_lock lock(update_lock_);
1291  if (fn)
1292  update_callbacks_.push_back(fn);
1293 }
1294 
1296 {
1297  boost::recursive_mutex::scoped_lock lock(update_lock_);
1298  update_callbacks_.clear();
1299 }
1300 
1302 {
1304  ROS_DEBUG_NAMED(LOGNAME, "Maximum frquency for publishing a planning scene is now %lf Hz",
1306 }
1307 
1309  std::vector<geometry_msgs::TransformStamped>& transforms)
1310 {
1311  const std::string& target = getRobotModel()->getModelFrame();
1312 
1313  std::vector<std::string> all_frame_names;
1314  tf_->getFrameStrings(all_frame_names);
1315  for (std::size_t i = 0; i < all_frame_names.size(); ++i)
1316  {
1317  const std::string& frame_no_slash = (!all_frame_names[i].empty() && all_frame_names[i][0] == '/') ?
1318  all_frame_names[i].substr(1) :
1319  all_frame_names[i];
1320  const std::string& frame_with_slash =
1321  (!all_frame_names[i].empty() && all_frame_names[i][0] != '/') ? '/' + all_frame_names[i] : all_frame_names[i];
1322 
1323  if (frame_with_slash == target || getRobotModel()->hasLinkModel(frame_no_slash))
1324  continue;
1325 
1326  ros::Time stamp(0);
1327  std::string err_string;
1328  if (tf_->getLatestCommonTime(target, all_frame_names[i], stamp, &err_string) != tf::NO_ERROR)
1329  {
1330  ROS_WARN_STREAM_NAMED(LOGNAME, "No transform available between frame '"
1331  << all_frame_names[i] << "' and planning frame '" << target << "' ("
1332  << err_string << ")");
1333  continue;
1334  }
1335 
1337  try
1338  {
1339  tf_->lookupTransform(target, all_frame_names[i], stamp, t);
1340  }
1341  catch (tf::TransformException& ex)
1342  {
1343  ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
1344  << all_frame_names[i] << "' to planning frame '" << target << "' ("
1345  << ex.what() << ")");
1346  continue;
1347  }
1348  geometry_msgs::TransformStamped f;
1349  f.header.frame_id = frame_with_slash;
1350  f.child_frame_id = target;
1351  f.transform.translation.x = t.getOrigin().x();
1352  f.transform.translation.y = t.getOrigin().y();
1353  f.transform.translation.z = t.getOrigin().z();
1354  const tf::Quaternion& q = t.getRotation();
1355  f.transform.rotation.x = q.x();
1356  f.transform.rotation.y = q.y();
1357  f.transform.rotation.z = q.z();
1358  f.transform.rotation.w = q.w();
1359  transforms.push_back(f);
1360  }
1361 }
1362 
1364 {
1365  if (!tf_)
1366  return;
1367 
1368  if (scene_)
1369  {
1370  std::vector<geometry_msgs::TransformStamped> transforms;
1371  getUpdatedFrameTransforms(transforms);
1372  {
1373  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1374  scene_->getTransformsNonConst().setTransforms(transforms);
1376  }
1378  }
1379 }
1380 
1382 {
1383  if (octomap_monitor_)
1384  octomap_monitor_->publishDebugInformation(flag);
1385 }
1386 
1388  const planning_scene::PlanningScenePtr& scene)
1389 {
1390  if (!scene || robot_description_.empty())
1391  return;
1392  collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
1393 
1394  // read overriding values from the param server
1395 
1396  // first we do default collision operations
1397  if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
1398  ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
1399  else
1400  {
1401  ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
1402 
1403  XmlRpc::XmlRpcValue coll_ops;
1404  nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
1405 
1406  if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
1407  {
1408  ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
1409  return;
1410  }
1411 
1412  if (coll_ops.size() == 0)
1413  {
1414  ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
1415  return;
1416  }
1417 
1418  for (int i = 0; i < coll_ops.size(); ++i)
1419  {
1420  if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
1421  {
1422  ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
1423  continue;
1424  }
1425  acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
1426  std::string(coll_ops[i]["operation"]) == "disable");
1427  }
1428  }
1429 }
1430 
1432 {
1433  if (robot_description_.empty())
1434  {
1435  default_robot_padd_ = 0.0;
1436  default_robot_scale_ = 1.0;
1437  default_object_padd_ = 0.0;
1438  default_attached_padd_ = 0.0;
1439  return;
1440  }
1441 
1442  // Ensure no leading slash creates a bad param server address
1443  static const std::string robot_description =
1444  (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
1445 
1446  nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
1447  nh_.param(robot_description + "_planning/default_robot_scale", default_robot_scale_, 1.0);
1448  nh_.param(robot_description + "_planning/default_object_padding", default_object_padd_, 0.0);
1449  nh_.param(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
1450  nh_.param(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_,
1451  std::map<std::string, double>());
1452  nh_.param(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_,
1453  std::map<std::string, double>());
1454 
1455  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
1456  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
1457 }
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.
std::shared_ptr< ros::AsyncSpinner > spinner_
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
void setCallbackQueue(CallbackQueueInterface *queue)
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)
CallbackQueueInterface * getCallbackQueue() const
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 Mon Oct 15 2018 03:45:59