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 
41 #include <moveit_msgs/GetPlanningScene.h>
42 
43 #include <dynamic_reconfigure/server.h>
44 #include <moveit_ros_planning/PlanningSceneMonitorDynamicReconfigureConfig.h>
45 #include <tf2/exceptions.h>
47 #include <tf2_eigen/tf2_eigen.h>
50 
51 #include <memory>
52 
53 namespace planning_scene_monitor
54 {
55 using namespace moveit_ros_planning;
56 
57 static const std::string LOGNAME = "planning_scene_monitor";
58 
59 class PlanningSceneMonitor::DynamicReconfigureImpl
60 {
61 public:
62  DynamicReconfigureImpl(PlanningSceneMonitor* owner)
63  : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle(decideNamespace(owner->getName())))
64  {
65  dynamic_reconfigure_server_.setCallback(
66  boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
67  }
68 
69 private:
70  // make sure we do not advertise the same service multiple times, in case we use multiple PlanningSceneMonitor
71  // instances in a process
72  static std::string decideNamespace(const std::string& name)
73  {
74  std::string ns = "~/" + name;
75  std::replace(ns.begin(), ns.end(), ' ', '_');
76  std::transform(ns.begin(), ns.end(), ns.begin(), ::tolower);
77  if (ros::service::exists(ns + "/set_parameters", false))
78  {
79  unsigned int c = 1;
80  while (ros::service::exists(ns + boost::lexical_cast<std::string>(c) + "/set_parameters", false))
81  c++;
82  ns += boost::lexical_cast<std::string>(c);
83  }
84  return ns;
85  }
86 
87  void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig& config, uint32_t /*level*/)
88  {
90  if (config.publish_geometry_updates)
92  if (config.publish_state_updates)
94  if (config.publish_transforms_updates)
96  if (config.publish_planning_scene)
97  {
98  owner_->setPlanningScenePublishingFrequency(config.publish_planning_scene_hz);
99  owner_->startPublishingPlanningScene(event);
100  }
101  else
102  owner_->stopPublishingPlanningScene();
103  }
104 
105  PlanningSceneMonitor* owner_;
106  dynamic_reconfigure::Server<PlanningSceneMonitorDynamicReconfigureConfig> dynamic_reconfigure_server_;
107 };
108 
109 const std::string PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC = "joint_states";
110 const std::string PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC = "attached_collision_object";
111 const std::string PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC = "collision_object";
112 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC = "planning_scene_world";
113 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC = "planning_scene";
114 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE = "get_planning_scene";
115 const std::string PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC = "monitored_planning_scene";
116 
117 PlanningSceneMonitor::PlanningSceneMonitor(const std::string& robot_description,
118  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
119  : PlanningSceneMonitor(planning_scene::PlanningScenePtr(), robot_description, tf_buffer, name)
120 {
121 }
122 
123 PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
124  const std::string& robot_description,
125  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
126  : PlanningSceneMonitor(scene, std::make_shared<robot_model_loader::RobotModelLoader>(robot_description), tf_buffer,
127  name)
128 {
129 }
130 
131 PlanningSceneMonitor::PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr& rm_loader,
132  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
133  : PlanningSceneMonitor(planning_scene::PlanningScenePtr(), rm_loader, tf_buffer, name)
134 {
135 }
136 
137 PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
138  const robot_model_loader::RobotModelLoaderPtr& rm_loader,
139  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
140  : monitor_name_(name), nh_("~"), tf_buffer_(tf_buffer), rm_loader_(rm_loader)
141 {
144  spinner_ = std::make_shared<ros::AsyncSpinner>(1, &queue_);
145  spinner_->start();
146  initialize(scene);
147 }
148 
149 PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
150  const robot_model_loader::RobotModelLoaderPtr& rm_loader,
151  const ros::NodeHandle& nh, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
152  const std::string& name)
153  : monitor_name_(name), nh_("~"), root_nh_(nh), tf_buffer_(tf_buffer), rm_loader_(rm_loader)
154 {
155  // use same callback queue as root_nh_
157  initialize(scene);
158 }
159 
161 {
162  if (scene_)
163  {
164  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
165  scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback());
166  }
171 
172  spinner_.reset();
173  delete reconfigure_impl_;
174  current_state_monitor_.reset();
175  scene_const_.reset();
176  scene_.reset();
177  parent_scene_.reset();
178  robot_model_.reset();
179  rm_loader_.reset();
180 }
181 
182 void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& scene)
183 {
185  moveit::tools::Profiler::ScopedBlock prof_block("PlanningSceneMonitor::initialize");
186 
187  if (monitor_name_.empty())
188  monitor_name_ = "planning_scene_monitor";
189  robot_description_ = rm_loader_->getRobotDescription();
190  if (rm_loader_->getModel())
191  {
192  robot_model_ = rm_loader_->getModel();
193  scene_ = scene;
194  if (!scene_)
195  {
196  try
197  {
198  scene_ = std::make_shared<planning_scene::PlanningScene>(rm_loader_->getModel());
201 
202  scene_->getCollisionEnvNonConst()->setPadding(default_robot_padd_);
203  scene_->getCollisionEnvNonConst()->setScale(default_robot_scale_);
204  for (const std::pair<const std::string, double>& it : default_robot_link_padd_)
205  {
206  scene_->getCollisionEnvNonConst()->setLinkPadding(it.first, it.second);
207  }
208  for (const std::pair<const std::string, double>& it : default_robot_link_scale_)
209  {
210  scene_->getCollisionEnvNonConst()->setLinkScale(it.first, it.second);
211  }
212  scene_->propogateRobotPadding();
213  }
215  {
216  ROS_ERROR_NAMED(LOGNAME, "Configuration of planning scene failed");
217  scene_.reset();
218  }
219  }
220  // scene_const_ is set regardless if scene_ is null or not
222  if (scene_)
223  {
224  // The scene_ is loaded on the collision loader only if it was correctly instantiated
226  scene_->setAttachedBodyUpdateCallback(
228  scene_->setCollisionObjectUpdateCallback(
229  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
230  }
231  }
232  else
233  {
234  ROS_ERROR_NAMED(LOGNAME, "Robot model not loaded");
235  }
236 
239 
243 
244  double temp_wait_time = 0.05;
245 
246  if (!robot_description_.empty())
247  nh_.param(robot_description_ + "_planning/shape_transform_cache_lookup_wait_time", temp_wait_time, temp_wait_time);
248 
250 
251  state_update_pending_ = false;
253  false, // not a oneshot timer
254  false); // do not start the timer yet
255 
256  reconfigure_impl_ = new DynamicReconfigureImpl(this);
257 }
258 
260 {
261  if (scene_)
262  {
263  if (flag)
264  {
265  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
266  if (scene_)
267  {
268  scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback());
269  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
270  scene_->decoupleParent();
272  scene_ = parent_scene_->diff();
274  scene_->setAttachedBodyUpdateCallback(
276  scene_->setCollisionObjectUpdateCallback(
277  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
278  }
279  }
280  else
281  {
283  {
284  ROS_WARN_NAMED(LOGNAME, "Diff monitoring was stopped while publishing planning scene diffs. "
285  "Stopping planning scene diff publisher");
287  }
288  {
289  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
290  if (scene_)
291  {
292  scene_->decoupleParent();
293  parent_scene_.reset();
294  // remove the '+' added by .diff() at the end of the scene name
295  if (!scene_->getName().empty())
296  {
297  if (scene_->getName()[scene_->getName().length() - 1] == '+')
298  scene_->setName(scene_->getName().substr(0, scene_->getName().length() - 1));
299  }
300  }
301  }
302  }
303  }
304 }
305 
307 {
309  {
310  std::unique_ptr<boost::thread> copy;
311  copy.swap(publish_planning_scene_);
312  new_scene_update_condition_.notify_all();
313  copy->join();
314  monitorDiffs(false);
316  ROS_INFO_NAMED(LOGNAME, "Stopped publishing maintained planning scene.");
317  }
318 }
319 
320 void PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_type,
321  const std::string& planning_scene_topic)
322 {
323  publish_update_types_ = update_type;
325  {
326  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>(planning_scene_topic, 100, false);
327  ROS_INFO_NAMED(LOGNAME, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
328  monitorDiffs(true);
330  std::make_unique<boost::thread>(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this));
331  }
332 }
333 
335 {
336  ROS_DEBUG_NAMED(LOGNAME, "Started scene publishing thread ...");
337 
338  // publish the full planning scene once
339  {
340  moveit_msgs::PlanningScene msg;
341  {
343  if (octomap_monitor_)
344  lock = octomap_monitor_->getOcTreePtr()->reading();
345  scene_->getPlanningSceneMsg(msg);
346  }
348  ROS_DEBUG_NAMED(LOGNAME, "Published the full planning scene: '%s'", msg.name.c_str());
349  }
350 
351  do
352  {
353  moveit_msgs::PlanningScene msg;
354  bool publish_msg = false;
355  bool is_full = false;
357  {
358  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
360  new_scene_update_condition_.wait(ulock);
362  {
364  {
366  is_full = true;
367  else
368  {
370  if (octomap_monitor_)
371  lock = octomap_monitor_->getOcTreePtr()->reading();
372  scene_->getPlanningSceneDiffMsg(msg);
374  {
375  msg.robot_state.attached_collision_objects.clear();
376  msg.robot_state.is_diff = true;
377  }
378  }
379  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the
380  // transform cache to
381  // update while we are
382  // potentially changing
383  // attached bodies
384  scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback());
385  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
386  scene_->pushDiffs(parent_scene_);
387  scene_->clearDiffs();
388  scene_->setAttachedBodyUpdateCallback(
390  scene_->setCollisionObjectUpdateCallback(
391  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
392  if (octomap_monitor_)
393  {
394  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
395  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
396  }
397  if (is_full)
398  {
400  if (octomap_monitor_)
401  lock = octomap_monitor_->getOcTreePtr()->reading();
402  scene_->getPlanningSceneMsg(msg);
403  }
404  // also publish timestamp of this robot_state
405  msg.robot_state.joint_state.header.stamp = last_robot_motion_time_;
406  publish_msg = true;
407  }
409  }
410  }
411  if (publish_msg)
412  {
414  if (is_full)
415  ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str());
416  rate.sleep();
417  }
418  } while (publish_planning_scene_);
419 }
420 
421 void PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string>& topics) const
422 {
423  topics.clear();
425  {
426  const std::string& t = current_state_monitor_->getMonitoredTopic();
427  if (!t.empty())
428  topics.push_back(t);
429  }
431  topics.push_back(planning_scene_subscriber_.getTopic());
433  topics.push_back(collision_object_subscriber_.getTopic());
435  topics.push_back(planning_scene_world_subscriber_.getTopic());
436 }
437 
438 namespace
439 {
440 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr& scene,
441  const planning_scene::PlanningScene* possible_parent)
442 {
443  if (scene && scene.get() == possible_parent)
444  return true;
445  if (scene)
446  return sceneIsParentOf(scene->getParent(), possible_parent);
447  return false;
448 }
449 } // namespace
450 
451 bool PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr& scene) const
452 {
453  return sceneIsParentOf(scene_const_, scene.get());
454 }
455 
456 bool PlanningSceneMonitor::updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const
457 {
458  return sceneIsParentOf(scene_const_, scene.get());
459 }
460 
461 void PlanningSceneMonitor::triggerSceneUpdateEvent(SceneUpdateType update_type)
462 {
463  // do not modify update functions while we are calling them
464  boost::recursive_mutex::scoped_lock lock(update_lock_);
465 
466  for (boost::function<void(SceneUpdateType)>& update_callback : update_callbacks_)
467  update_callback(update_type);
468  new_scene_update_ = (SceneUpdateType)((int)new_scene_update_ | (int)update_type);
469  new_scene_update_condition_.notify_all();
470 }
471 
472 bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_name)
473 {
474  if (get_scene_service_.getService() == service_name)
475  {
476  ROS_FATAL_STREAM_NAMED(LOGNAME, "requestPlanningSceneState() to self-provided service '" << service_name << "'");
477  throw std::runtime_error("requestPlanningSceneState() to self-provided service: " + service_name);
478  }
479  // use global namespace for service
480  ros::ServiceClient client = ros::NodeHandle().serviceClient<moveit_msgs::GetPlanningScene>(service_name);
481  // all scene components are returned if none are specified
482  moveit_msgs::GetPlanningScene srv;
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.");
489  }
490 
491  if (client.call(srv))
492  {
493  newPlanningSceneMessage(srv.response.scene);
494  }
495  else
496  {
498  LOGNAME, "Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?",
499  service_name.c_str());
500  return false;
501  }
502  return true;
503 }
504 
505 void PlanningSceneMonitor::providePlanningSceneService(const std::string& service_name)
506 {
507  // Load the service
510 }
511 
512 bool PlanningSceneMonitor::getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req,
513  moveit_msgs::GetPlanningScene::Response& res)
514 {
515  if (req.components.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
517 
518  moveit_msgs::PlanningSceneComponents all_components;
519  all_components.components = UINT_MAX; // Return all scene components if nothing is specified.
520 
521  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
522  scene_->getPlanningSceneMsg(res.scene, req.components.components ? req.components : all_components);
523 
524  return true;
525 }
526 
527 void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene)
528 {
529  newPlanningSceneMessage(*scene);
530 }
531 
533 {
534  bool removed = false;
535  {
536  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
537  removed = scene_->getWorldNonConst()->removeObject(scene_->OCTOMAP_NS);
538 
539  if (octomap_monitor_)
540  {
541  octomap_monitor_->getOcTreePtr()->lockWrite();
542  octomap_monitor_->getOcTreePtr()->clear();
543  octomap_monitor_->getOcTreePtr()->unlockWrite();
544  }
545  else
546  {
547  ROS_WARN_NAMED(LOGNAME, "Unable to clear octomap since no octomap monitor has been initialized");
548  } // Lift the scoped lock before calling triggerSceneUpdateEvent to avoid deadlock
549  }
550 
551  if (removed)
553 }
554 
555 bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
556 {
557  if (!scene_)
558  return false;
559 
560  bool result;
561 
563  std::string old_scene_name;
564  {
565  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
566  // we don't want the transform cache to update while we are potentially changing attached bodies
567  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
568 
570  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
571  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
572  "scene update " << fmod(last_update_time_.toSec(), 10.)
573  << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
574  old_scene_name = scene_->getName();
575  result = scene_->usePlanningSceneMsg(scene);
576  if (octomap_monitor_)
577  {
578  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
579  {
580  octomap_monitor_->getOcTreePtr()->lockWrite();
581  octomap_monitor_->getOcTreePtr()->clear();
582  octomap_monitor_->getOcTreePtr()->unlockWrite();
583  }
584  }
585  robot_model_ = scene_->getRobotModel();
586 
587  // if we just reset the scene completely but we were maintaining diffs, we need to fix that
588  if (!scene.is_diff && parent_scene_)
589  {
590  // the scene is now decoupled from the parent, since we just reset it
591  scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback());
592  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
594  scene_ = parent_scene_->diff();
596  scene_->setAttachedBodyUpdateCallback(
598  scene_->setCollisionObjectUpdateCallback(
599  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
600  }
601  if (octomap_monitor_)
602  {
603  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
604  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
605  }
606  }
607 
608  // if we have a diff, try to more accurately determine the update type
609  if (scene.is_diff)
610  {
611  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
612  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
613  scene.link_scale.empty();
614  if (no_other_scene_upd)
615  {
616  upd = UPDATE_NONE;
617  if (!moveit::core::isEmpty(scene.world))
618  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
619 
620  if (!scene.fixed_frame_transforms.empty())
621  upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
622 
623  if (!moveit::core::isEmpty(scene.robot_state))
624  {
625  upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
626  if (!scene.robot_state.attached_collision_objects.empty() || !static_cast<bool>(scene.robot_state.is_diff))
627  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
628  }
629  }
630  }
632  return result;
633 }
634 
635 void PlanningSceneMonitor::newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world)
636 {
637  if (scene_)
638  {
640  {
641  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
643  scene_->getWorldNonConst()->clearObjects();
644  scene_->processPlanningSceneWorldMsg(*world);
645  if (octomap_monitor_)
646  {
647  if (world->octomap.octomap.data.empty())
648  {
649  octomap_monitor_->getOcTreePtr()->lockWrite();
650  octomap_monitor_->getOcTreePtr()->clear();
651  octomap_monitor_->getOcTreePtr()->unlockWrite();
652  }
653  }
654  }
656  }
657 }
658 
659 void PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj)
660 {
661  if (!scene_)
662  return;
663 
665  {
666  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
668  if (!scene_->processCollisionObjectMsg(*obj))
669  return;
670  }
672 }
673 
674 void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
675 {
676  if (scene_)
677  {
679  {
680  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
682  scene_->processAttachedCollisionObjectMsg(*obj);
683  }
685  }
686 }
687 
689 {
690  if (!octomap_monitor_)
691  return;
692 
693  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
694 
696  const std::vector<const moveit::core::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
698  bool warned = false;
699  for (const moveit::core::LinkModel* link : links)
700  {
701  std::vector<shapes::ShapeConstPtr> shapes = link->getShapes(); // copy shared ptrs on purpuse
702  for (std::size_t j = 0; j < shapes.size(); ++j)
703  {
704  // merge mesh vertices up to 0.1 mm apart
705  if (shapes[j]->type == shapes::MESH)
706  {
707  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
708  m->mergeVertices(1e-4);
709  shapes[j].reset(m);
710  }
711 
713  if (h)
714  link_shape_handles_[link].push_back(std::make_pair(h, j));
715  }
716  if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
717  {
718  ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
719  warned = true;
720  }
721  }
722 }
723 
725 {
726  if (!octomap_monitor_)
727  return;
728 
729  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
730 
731  for (std::pair<const moveit::core::LinkModel* const,
732  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
734  for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : link_shape_handle.second)
735  octomap_monitor_->forgetShape(it.first);
736  link_shape_handles_.clear();
737 }
738 
740 {
741  if (!octomap_monitor_)
742  return;
743 
744  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
745 
746  // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
747  for (std::pair<const moveit::core::AttachedBody* const,
748  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& attached_body_shape_handle :
750  for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : attached_body_shape_handle.second)
751  octomap_monitor_->forgetShape(it.first);
753 }
754 
756 {
757  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
758 
760  // add attached objects again
761  std::vector<const moveit::core::AttachedBody*> ab;
762  scene_->getCurrentState().getAttachedBodies(ab);
763  for (const moveit::core::AttachedBody* body : ab)
765 }
766 
768 {
769  if (!octomap_monitor_)
770  return;
771 
772  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
773 
774  // clear information about any attached object
775  for (std::pair<const std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
776  collision_body_shape_handle : collision_body_shape_handles_)
777  for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
778  collision_body_shape_handle.second)
779  octomap_monitor_->forgetShape(it.first);
781 }
782 
784 {
785  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
786 
788  for (const std::pair<const std::string, collision_detection::World::ObjectPtr>& it : *scene_->getWorld())
789  excludeWorldObjectFromOctree(it.second);
790 }
791 
793 {
794  if (!octomap_monitor_)
795  return;
796 
797  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
798  bool found = false;
799  for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
800  {
801  if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
802  continue;
803  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
804  if (h)
805  {
806  found = true;
807  attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
808  }
809  }
810  if (found)
811  ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
812 }
813 
815 {
816  if (!octomap_monitor_)
817  return;
818 
819  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
820 
821  AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
822  if (it != attached_body_shape_handles_.end())
823  {
824  for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& shape_handle : it->second)
825  octomap_monitor_->forgetShape(shape_handle.first);
826  ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
828  }
829 }
830 
831 void PlanningSceneMonitor::excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj)
832 {
833  if (!octomap_monitor_)
834  return;
835 
836  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
837 
838  bool found = false;
839  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
840  {
841  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
842  continue;
843  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
844  if (h)
845  {
846  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->global_shape_poses_[i]));
847  found = true;
848  }
849  }
850  if (found)
851  ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
852 }
853 
854 void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj)
855 {
856  if (!octomap_monitor_)
857  return;
858 
859  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
860 
861  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
862  if (it != collision_body_shape_handles_.end())
863  {
864  for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& shape_handle : it->second)
865  octomap_monitor_->forgetShape(shape_handle.first);
866  ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
868  }
869 }
870 
872  bool just_attached)
873 {
874  if (!octomap_monitor_)
875  return;
876 
877  if (just_attached)
878  excludeAttachedBodyFromOctree(attached_body);
879  else
880  includeAttachedBodyInOctree(attached_body);
881 }
882 
883 void PlanningSceneMonitor::currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& obj,
885 {
887  return;
889  return;
890 
893  else if (action & collision_detection::World::DESTROY)
895  else
896  {
899  }
900 }
901 
902 bool PlanningSceneMonitor::waitForCurrentRobotState(const ros::Time& t, double wait_time)
903 {
904  if (t.isZero())
905  return false;
907  ros::WallDuration timeout(wait_time);
908 
909  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
910 
912  {
913  // Wait for next robot update in state monitor.
914  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
915 
916  /* As robot updates are passed to the planning scene only in throttled fashion, there might
917  be still an update pending. If so, explicitly update the planning scene here.
918  If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
919  if (success)
920  {
921  boost::mutex::scoped_lock lock(state_pending_mutex_);
922  if (state_update_pending_) // enforce state update
923  {
924  state_update_pending_ = false;
926  lock.unlock();
928  }
929  return true;
930  }
931 
932  ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
933  return false;
934  }
935 
936  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
937  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
938  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
939  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
940  ros::Time prev_robot_motion_time = last_robot_motion_time_;
941  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
942  timeout > ros::WallDuration())
943  {
944  ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
945  new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
946  timeout -= ros::WallTime::now() - start; // compute remaining wait_time
947  }
948  bool success = last_robot_motion_time_ >= t;
949  // suppress warning if we received an update at all
950  if (!success && prev_robot_motion_time != last_robot_motion_time_)
951  ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
952  (t - last_robot_motion_time_).toSec());
953 
954  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
955  << " scene update: " << (t - last_update_time_).toSec());
956  return success;
957 }
958 
960 {
961  scene_update_mutex_.lock_shared();
962  if (octomap_monitor_)
963  octomap_monitor_->getOcTreePtr()->lockRead();
964 }
965 
967 {
968  if (octomap_monitor_)
969  octomap_monitor_->getOcTreePtr()->unlockRead();
970  scene_update_mutex_.unlock_shared();
971 }
972 
974 {
975  scene_update_mutex_.lock();
976  if (octomap_monitor_)
977  octomap_monitor_->getOcTreePtr()->lockWrite();
978 }
979 
981 {
982  if (octomap_monitor_)
983  octomap_monitor_->getOcTreePtr()->unlockWrite();
984  scene_update_mutex_.unlock();
985 }
986 
987 void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
988 {
990 
991  ROS_INFO_NAMED(LOGNAME, "Starting planning scene monitor");
992  // listen for planning scene updates; these messages include transforms, so no need for filters
993  if (!scene_topic.empty())
994  {
997  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
998  }
999 }
1000 
1002 {
1004  {
1005  ROS_INFO_NAMED(LOGNAME, "Stopping planning scene monitor");
1007  }
1008 }
1009 
1010 bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
1013  if (!tf_buffer_)
1014  return false;
1015  try
1016  {
1017  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
1018 
1019  for (const std::pair<const moveit::core::LinkModel* const,
1020  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
1022  {
1023  tf_buffer_->canTransform(target_frame, link_shape_handle.first->getName(), target_time,
1025  Eigen::Isometry3d ttr = tf2::transformToEigen(
1026  tf_buffer_->lookupTransform(target_frame, link_shape_handle.first->getName(), target_time));
1027  for (std::size_t j = 0; j < link_shape_handle.second.size(); ++j)
1028  cache[link_shape_handle.second[j].first] =
1029  ttr * link_shape_handle.first->getCollisionOriginTransforms()[link_shape_handle.second[j].second];
1030  }
1031  for (const std::pair<const moveit::core::AttachedBody* const,
1032  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>&
1033  attached_body_shape_handle : attached_body_shape_handles_)
1034  {
1035  tf_buffer_->canTransform(target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time,
1037  Eigen::Isometry3d transform = tf2::transformToEigen(tf_buffer_->lookupTransform(
1038  target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time));
1039  for (std::size_t k = 0; k < attached_body_shape_handle.second.size(); ++k)
1040  cache[attached_body_shape_handle.second[k].first] =
1041  transform *
1042  attached_body_shape_handle.first->getShapePosesInLinkFrame()[attached_body_shape_handle.second[k].second];
1043  }
1044  {
1045  tf_buffer_->canTransform(target_frame, scene_->getPlanningFrame(), target_time,
1047  Eigen::Isometry3d transform =
1048  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time));
1049  for (const std::pair<const std::string,
1050  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
1051  collision_body_shape_handle : collision_body_shape_handles_)
1052  for (const std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
1053  collision_body_shape_handle.second)
1054  cache[it.first] = transform * (*it.second);
1055  }
1056  }
1057  catch (tf2::TransformException& ex)
1058  {
1059  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
1060  return false;
1061  }
1062  return true;
1063 }
1064 
1065 void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collision_objects_topic,
1066  const std::string& planning_scene_world_topic,
1067  const bool load_octomap_monitor)
1068 {
1070  ROS_INFO_NAMED(LOGNAME, "Starting world geometry update monitor for collision objects, attached objects, octomap "
1071  "updates.");
1072 
1073  // Listen to the /collision_objects topic to detect requests to add/remove/update collision objects to/from the world
1074  if (!collision_objects_topic.empty())
1075  {
1077  root_nh_.subscribe(collision_objects_topic, 1024, &PlanningSceneMonitor::collisionObjectCallback, this);
1078  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
1079  }
1080 
1081  if (!planning_scene_world_topic.empty())
1082  {
1084  root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
1085  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
1086  root_nh_.resolveName(planning_scene_world_topic).c_str());
1087  }
1088 
1089  // Ocotomap monitor is optional
1090  if (load_octomap_monitor)
1091  {
1092  if (!octomap_monitor_)
1093  {
1095  std::make_unique<occupancy_map_monitor::OccupancyMapMonitor>(tf_buffer_, scene_->getPlanningFrame());
1099 
1100  octomap_monitor_->setTransformCacheCallback(
1101  boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
1102  octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
1103  }
1104  octomap_monitor_->startMonitor();
1105  }
1106 }
1107 
1109 {
1111  {
1112  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1114  }
1116  {
1117  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1119  }
1120  if (octomap_monitor_)
1121  octomap_monitor_->stopMonitor();
1122 }
1123 
1124 void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1125  const std::string& attached_objects_topic)
1126 {
1127  stopStateMonitor();
1128  if (scene_)
1129  {
1131  current_state_monitor_ = std::make_shared<CurrentStateMonitor>(getRobotModel(), tf_buffer_, root_nh_);
1132  current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
1133  current_state_monitor_->startStateMonitor(joint_states_topic);
1134 
1135  {
1136  boost::mutex::scoped_lock lock(state_pending_mutex_);
1137  if (!dt_state_update_.isZero())
1139  }
1141  if (!attached_objects_topic.empty())
1142  {
1143  // using regular message filter as there's no header
1145  root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
1146  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
1147  root_nh_.resolveName(attached_objects_topic).c_str());
1148  }
1149  }
1150  else
1151  ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
1152 }
1153 
1155 {
1157  current_state_monitor_->stopStateMonitor();
1160 
1161  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1163  {
1164  boost::mutex::scoped_lock lock(state_pending_mutex_);
1165  state_update_pending_ = false;
1166  }
1167 }
1168 
1169 void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr& /* joint_state */)
1170 {
1171  const ros::WallTime& n = ros::WallTime::now();
1173 
1174  bool update = false;
1175  {
1176  boost::mutex::scoped_lock lock(state_pending_mutex_);
1177 
1178  if (dt < dt_state_update_)
1179  {
1180  state_update_pending_ = true;
1181  }
1182  else
1183  {
1184  state_update_pending_ = false;
1186  update = true;
1187  }
1188  }
1189  // run the state update with state_pending_mutex_ unlocked
1190  if (update)
1192 }
1193 
1195 {
1197  {
1198  bool update = false;
1199 
1200  const ros::WallTime& n = ros::WallTime::now();
1202 
1203  {
1204  // lock for access to dt_state_update_ and state_update_pending_
1205  boost::mutex::scoped_lock lock(state_pending_mutex_);
1207  {
1208  state_update_pending_ = false;
1210  update = true;
1212  "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
1213  }
1214  }
1215 
1216  // run the state update with state_pending_mutex_ unlocked
1217  if (update)
1218  {
1220  ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
1221  }
1222  }
1223 }
1224 
1227  if (!octomap_monitor_)
1228  return;
1229 
1231  {
1232  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1234  octomap_monitor_->getOcTreePtr()->lockRead();
1235  try
1236  {
1237  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Isometry3d::Identity());
1238  octomap_monitor_->getOcTreePtr()->unlockRead();
1239  }
1240  catch (...)
1241  {
1242  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1243  throw;
1244  }
1245  }
1247 }
1248 
1250 {
1251  bool update = false;
1252  if (hz > std::numeric_limits<double>::epsilon())
1253  {
1254  boost::mutex::scoped_lock lock(state_pending_mutex_);
1255  dt_state_update_.fromSec(1.0 / hz);
1258  }
1259  else
1260  {
1261  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1263  boost::mutex::scoped_lock lock(state_pending_mutex_);
1266  update = true;
1267  }
1268  ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
1269 
1270  if (update)
1272 }
1273 
1275 {
1277  {
1278  std::vector<std::string> missing;
1279  if (!current_state_monitor_->haveCompleteState(missing) &&
1280  (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
1281  {
1282  std::string missing_str = boost::algorithm::join(missing, ", ");
1283  ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known. Missing %s",
1284  missing_str.c_str());
1285  }
1286 
1287  {
1288  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1290  ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
1291  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1292  scene_->getCurrentStateNonConst().update(); // compute all transforms
1293  }
1295  }
1296  else
1297  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
1298 }
1299 
1300 void PlanningSceneMonitor::addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn)
1301 {
1302  boost::recursive_mutex::scoped_lock lock(update_lock_);
1303  if (fn)
1304  update_callbacks_.push_back(fn);
1305 }
1308 {
1309  boost::recursive_mutex::scoped_lock lock(update_lock_);
1310  update_callbacks_.clear();
1311 }
1312 
1314 {
1316  ROS_DEBUG_NAMED(LOGNAME, "Maximum frequency for publishing a planning scene is now %lf Hz",
1318 }
1319 
1320 void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms)
1321 {
1322  const std::string& target = getRobotModel()->getModelFrame();
1323 
1324  std::vector<std::string> all_frame_names;
1325  tf_buffer_->_getFrameStrings(all_frame_names);
1326  for (const std::string& all_frame_name : all_frame_names)
1327  {
1328  if (all_frame_name == target || getRobotModel()->hasLinkModel(all_frame_name))
1329  continue;
1330 
1331  geometry_msgs::TransformStamped f;
1332  try
1333  {
1334  f = tf_buffer_->lookupTransform(target, all_frame_name, ros::Time(0));
1335  }
1336  catch (tf2::TransformException& ex)
1337  {
1338  ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
1339  << all_frame_name << "' to planning frame '" << target << "' (" << ex.what()
1340  << ")");
1341  continue;
1342  }
1343  f.header.frame_id = all_frame_name;
1344  f.child_frame_id = target;
1345  transforms.push_back(f);
1346  }
1347 }
1348 
1350 {
1351  if (!tf_buffer_)
1352  return;
1353 
1354  if (scene_)
1355  {
1356  std::vector<geometry_msgs::TransformStamped> transforms;
1357  getUpdatedFrameTransforms(transforms);
1358  {
1359  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1360  scene_->getTransformsNonConst().setTransforms(transforms);
1362  }
1364  }
1365 }
1366 
1368 {
1369  if (octomap_monitor_)
1370  octomap_monitor_->publishDebugInformation(flag);
1371 }
1372 
1373 void PlanningSceneMonitor::configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene)
1374 {
1375  if (!scene || robot_description_.empty())
1376  return;
1377  collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
1378 
1379  // read overriding values from the param server
1380 
1381  // first we do default collision operations
1382  if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
1383  ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
1384  else
1385  {
1386  ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
1387 
1388  XmlRpc::XmlRpcValue coll_ops;
1389  nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
1390 
1391  if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
1392  {
1393  ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
1394  return;
1395  }
1396 
1397  if (coll_ops.size() == 0)
1398  {
1399  ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
1400  return;
1401  }
1402 
1403  for (int i = 0; i < coll_ops.size(); ++i) // NOLINT(modernize-loop-convert)
1404  {
1405  if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
1406  {
1407  ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
1408  continue;
1409  }
1410  acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
1411  std::string(coll_ops[i]["operation"]) == "disable");
1412  }
1413  }
1414 }
1415 
1417 {
1418  if (robot_description_.empty())
1419  {
1420  default_robot_padd_ = 0.0;
1421  default_robot_scale_ = 1.0;
1422  default_object_padd_ = 0.0;
1423  default_attached_padd_ = 0.0;
1424  return;
1425  }
1426 
1427  // print deprecation warning if necessary
1428  // TODO: remove this warning after 06/2022
1429  const std::string old_robot_description =
1430  (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
1431  if (nh_.resolveName(old_robot_description) != nh_.resolveName(robot_description_))
1432  {
1433  if (nh_.hasParam(old_robot_description + "_planning/default_robot_padding") ||
1434  nh_.hasParam(old_robot_description + "_planning/default_robot_scale") ||
1435  nh_.hasParam(old_robot_description + "_planning/default_object_padding") ||
1436  nh_.hasParam(old_robot_description + "_planning/default_attached_padding") ||
1437  nh_.hasParam(old_robot_description + "_planning/default_robot_link_padding") ||
1438  nh_.hasParam(old_robot_description + "_planning/default_robot_link_scale"))
1439  {
1440  ROS_WARN_STREAM_NAMED(LOGNAME, "The path for the padding parameters has changed!\n"
1441  "Old parameter path: '"
1442  << nh_.resolveName(old_robot_description + "_planning/")
1443  << "'\n"
1444  "New parameter path: '"
1445  << nh_.resolveName(robot_description_ + "_planning/")
1446  << "'\n"
1447  "Ignoring old parameters. Please update your moveit config!");
1448  }
1449  }
1450 
1451  nh_.param(robot_description_ + "_planning/default_robot_padding", default_robot_padd_, 0.0);
1452  nh_.param(robot_description_ + "_planning/default_robot_scale", default_robot_scale_, 1.0);
1453  nh_.param(robot_description_ + "_planning/default_object_padding", default_object_padd_, 0.0);
1454  nh_.param(robot_description_ + "_planning/default_attached_padding", default_attached_padd_, 0.0);
1455  nh_.param(robot_description_ + "_planning/default_robot_link_padding", default_robot_link_padd_,
1456  std::map<std::string, double>());
1457  nh_.param(robot_description_ + "_planning/default_robot_link_scale", default_robot_link_scale_,
1458  std::map<std::string, double>());
1459 
1460  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
1461  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
1462 }
1463 } // namespace planning_scene_monitor
planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectFromOctree
void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj)
Definition: planning_scene_monitor.cpp:863
XmlRpc::XmlRpcValue::size
int size() const
planning_scene_monitor::PlanningSceneMonitor::planning_scene_world_subscriber_
ros::Subscriber planning_scene_world_subscriber_
Definition: planning_scene_monitor.h:536
moveit::core::LinkModel
planning_scene_monitor::PlanningSceneMonitor::collision_body_shape_handles_
CollisionBodyShapeHandles collision_body_shape_handles_
Definition: planning_scene_monitor.h:561
planning_scene_monitor::PlanningSceneMonitor::publishDebugInformation
void publishDebugInformation(bool flag)
Definition: planning_scene_monitor.cpp:1399
planning_scene_monitor
Definition: current_state_monitor.h:47
planning_scene_monitor::PlanningSceneMonitor::onStateUpdate
void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state)
Definition: planning_scene_monitor.cpp:1201
planning_scene_monitor::PlanningSceneMonitor::currentWorldObjectUpdateCallback
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.
Definition: planning_scene_monitor.cpp:915
planning_scene_monitor::PlanningSceneMonitor::default_attached_padd_
double default_attached_padd_
default attached padding
Definition: planning_scene_monitor.h:520
planning_scene_monitor::PlanningSceneMonitor::default_robot_link_padd_
std::map< std::string, double > default_robot_link_padd_
default robot link padding
Definition: planning_scene_monitor.h:522
ros::ServiceClient::waitForExistence
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
ROS_ERROR_THROTTLE_NAMED
#define ROS_ERROR_THROTTLE_NAMED(period, name,...)
collision_detection::World::DESTROY
DESTROY
ros::service::exists
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
planning_scene_monitor::PlanningSceneMonitor::configureDefaultPadding
void configureDefaultPadding()
Configure the default padding.
Definition: planning_scene_monitor.cpp:1448
shapes
planning_scene_monitor::PlanningSceneMonitor::attached_body_shape_handles_
AttachedBodyShapeHandles attached_body_shape_handles_
Definition: planning_scene_monitor.h:560
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE
static const std::string DEFAULT_PLANNING_SCENE_SERVICE
The name of the service used by default for requesting full planning scene state.
Definition: planning_scene_monitor.h:132
occupancy_map_monitor::ShapeTransformCache
std::map< ShapeHandle, Eigen::Isometry3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Isometry3d > > > ShapeTransformCache
DurationBase< WallDuration >::fromSec
WallDuration & fromSec(double t)
planning_scene_monitor::PlanningSceneMonitor::triggerSceneUpdateEvent
void triggerSceneUpdateEvent(SceneUpdateType update_type)
This function is called every time there is a change to the planning scene.
Definition: planning_scene_monitor.cpp:493
planning_scene_monitor::PlanningSceneMonitor::~PlanningSceneMonitor
~PlanningSceneMonitor()
Definition: planning_scene_monitor.cpp:192
planning_scene_monitor::PlanningSceneMonitor::getShapeTransformCache
bool getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const
Definition: planning_scene_monitor.cpp:1042
planning_scene_monitor::PlanningSceneMonitor::stopStateMonitor
void stopStateMonitor()
Stop the state monitor.
Definition: planning_scene_monitor.cpp:1186
planning_scene_monitor::PlanningSceneMonitor::updatesScene
bool updatesScene(const planning_scene::PlanningSceneConstPtr &scene) const
Return true if the scene scene can be updated directly or indirectly by this monitor....
Definition: planning_scene_monitor.cpp:488
planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage
bool newPlanningSceneMessage(const moveit_msgs::PlanningScene &scene)
Definition: planning_scene_monitor.cpp:587
planning_scene_monitor::PlanningSceneMonitor::octomapUpdateCallback
void octomapUpdateCallback()
Callback for octomap updates.
Definition: planning_scene_monitor.cpp:1257
planning_scene_monitor::PlanningSceneMonitor::addUpdateCallback
void addUpdateCallback(const boost::function< void(SceneUpdateType)> &fn)
Add a function to be called when an update to the scene is received.
Definition: planning_scene_monitor.cpp:1332
planning_scene_monitor::PlanningSceneMonitor::spinner_
std::shared_ptr< ros::AsyncSpinner > spinner_
Definition: planning_scene_monitor.h:507
ros::ServiceClient::exists
bool exists()
shapes::PLANE
PLANE
tf2_eigen.h
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
ros
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
planning_scene_monitor::PlanningSceneMonitor::collision_loader_
collision_detection::CollisionPluginLoader collision_loader_
Definition: planning_scene_monitor.h:616
planning_scene_monitor::PlanningSceneMonitor::scenePublishingThread
void scenePublishingThread()
Definition: planning_scene_monitor.cpp:366
planning_scene_monitor::PlanningSceneMonitor::default_robot_padd_
double default_robot_padd_
default robot padding
Definition: planning_scene_monitor.h:514
robot_model_loader
Definition: robot_model_loader.h:44
planning_scene::PlanningScene
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
planning_scene_monitor::PlanningSceneMonitor::attached_collision_object_subscriber_
ros::Subscriber attached_collision_object_subscriber_
Definition: planning_scene_monitor.h:538
planning_scene_monitor::PlanningSceneMonitor::setStateUpdateFrequency
void setStateUpdateFrequency(double hz)
Update the scene using the monitored state at a specified frequency, in Hz. This function has an effe...
Definition: planning_scene_monitor.cpp:1281
planning_scene_monitor::PlanningSceneMonitor::collisionObjectCallback
void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr &obj)
Callback for a new collision object msg.
Definition: planning_scene_monitor.cpp:691
planning_scene_monitor::PlanningSceneMonitor::scene_const_
planning_scene::PlanningSceneConstPtr scene_const_
Definition: planning_scene_monitor.h:498
ros::Subscriber::getTopic
std::string getTopic() const
planning_scene_monitor::PlanningSceneMonitor::update_lock_
boost::recursive_mutex update_lock_
lock access to update_callbacks_
Definition: planning_scene_monitor.h:565
TimeBase< Time, Duration >::toSec
double toSec() const
planning_scene_monitor::PlanningSceneMonitor::collision_object_subscriber_
ros::Subscriber collision_object_subscriber_
Definition: planning_scene_monitor.h:539
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
collision_detection::World::Action
ros::Subscriber::shutdown
void shutdown()
moveit::tools::Profiler::ScopedStart
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
shapes::Mesh::mergeVertices
void mergeVertices(double threshold)
shapes::Mesh
planning_scene_monitor::PlanningSceneMonitor::updateFrameTransforms
void updateFrameTransforms()
Update the transforms for the frames that are not part of the kinematic model using tf....
Definition: planning_scene_monitor.cpp:1381
planning_scene_monitor::LOGNAME
static const std::string LOGNAME
Definition: planning_scene_monitor.cpp:89
planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectsFromOctree
void excludeWorldObjectsFromOctree()
Definition: planning_scene_monitor.cpp:815
planning_scene_monitor::PlanningSceneMonitor::last_robot_motion_time_
ros::Time last_robot_motion_time_
Last time the state was updated.
Definition: planning_scene_monitor.h:502
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
obj
CollisionObject< S > * obj
planning_scene_monitor::PlanningSceneMonitor::rm_loader_
robot_model_loader::RobotModelLoaderPtr rm_loader_
Definition: planning_scene_monitor.h:613
planning_scene_monitor::PlanningSceneMonitor::new_scene_update_
SceneUpdateType new_scene_update_
Definition: planning_scene_monitor.h:531
planning_scene_monitor::PlanningSceneMonitor::tf_buffer_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: planning_scene_monitor.h:509
shapes::MESH
MESH
f
f
planning_scene_monitor::PlanningSceneMonitor::state_update_timer_
ros::WallTimer state_update_timer_
timer for state updates.
Definition: planning_scene_monitor.h:607
ros::WallTimer::start
void start()
planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType
SceneUpdateType
Definition: planning_scene_monitor.h:96
moveit::core::AttachedBody
planning_scene_monitor::PlanningSceneMonitor::default_robot_scale_
double default_robot_scale_
default robot scaling
Definition: planning_scene_monitor.h:516
planning_scene_monitor::PlanningSceneMonitor::state_pending_mutex_
boost::mutex state_pending_mutex_
Definition: planning_scene_monitor.h:589
planning_scene_monitor::PlanningSceneMonitor::initialize
void initialize(const planning_scene::PlanningScenePtr &scene)
Initialize the planning scene monitor.
Definition: planning_scene_monitor.cpp:214
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
planning_scene_monitor::PlanningSceneMonitor::publish_update_types_
SceneUpdateType publish_update_types_
Definition: planning_scene_monitor.h:530
collision_detection::AllowedCollisionMatrix::setEntry
void setEntry(bool allowed)
collision_detection::AllowedCollisionMatrix
message_checks.h
planning_scene_monitor::PlanningSceneMonitor::octomap_monitor_
std::unique_ptr< occupancy_map_monitor::OccupancyMapMonitor > octomap_monitor_
Definition: planning_scene_monitor.h:546
planning_scene_monitor::PlanningSceneMonitor::updateSceneWithCurrentState
void updateSceneWithCurrentState()
Update the scene using the monitored state. This function is automatically called when an update to t...
Definition: planning_scene_monitor.cpp:1306
moveit::core::isEmpty
bool isEmpty(const geometry_msgs::Pose &msg)
planning_scene_monitor::PlanningSceneMonitor::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Definition: planning_scene_monitor.h:205
planning_scene_monitor::PlanningSceneMonitor::last_update_time_
ros::Time last_update_time_
mutex for stored scene
Definition: planning_scene_monitor.h:501
planning_scene_monitor::PlanningSceneMonitor::new_scene_update_condition_
boost::condition_variable_any new_scene_update_condition_
Definition: planning_scene_monitor.h:532
name
std::string name
ros::Publisher::shutdown
void shutdown()
planning_scene_monitor::PlanningSceneMonitor::lockSceneRead
void lockSceneRead()
Lock the scene for reading (multiple threads can lock for reading at the same time)
Definition: planning_scene_monitor.cpp:991
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
update
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
collision_detection::World::ObserverCallbackFn
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
ROS_FATAL_STREAM_NAMED
#define ROS_FATAL_STREAM_NAMED(name, args)
planning_scene_monitor::PlanningSceneMonitor::unlockSceneRead
void unlockSceneRead()
Unlock the scene from reading (multiple threads can lock for reading at the same time)
Definition: planning_scene_monitor.cpp:998
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC
static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC
Definition: planning_scene_monitor.h:126
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
planning_scene_monitor::PlanningSceneMonitor::root_nh_
ros::NodeHandle root_nh_
Definition: planning_scene_monitor.h:505
planning_scene_monitor::PlanningSceneMonitor::current_state_monitor_
CurrentStateMonitorPtr current_state_monitor_
Definition: planning_scene_monitor.h:549
ros::WallTime::now
static WallTime now()
planning_scene_monitor::PlanningSceneMonitor::stopPublishingPlanningScene
void stopPublishingPlanningScene()
Stop publishing the maintained planning scene.
Definition: planning_scene_monitor.cpp:338
planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodyFromOctree
void excludeAttachedBodyFromOctree(const moveit::core::AttachedBody *attached_body)
Definition: planning_scene_monitor.cpp:824
planning_scene_monitor::PlanningSceneMonitor::UPDATE_STATE
@ UPDATE_STATE
The state in the monitored scene was updated.
Definition: planning_scene_monitor.h:102
planning_scene_monitor::PlanningSceneMonitor::reconfigure_impl_
DynamicReconfigureImpl * reconfigure_impl_
Definition: planning_scene_monitor.h:618
planning_scene_monitor::PlanningSceneMonitor::queue_
ros::CallbackQueue queue_
Definition: planning_scene_monitor.h:506
planning_scene_monitor::PlanningSceneMonitor::stopSceneMonitor
void stopSceneMonitor()
Stop the scene monitor.
Definition: planning_scene_monitor.cpp:1033
planning_scene_monitor::PlanningSceneMonitor::stopWorldGeometryMonitor
void stopWorldGeometryMonitor()
Stop the world geometry monitor.
Definition: planning_scene_monitor.cpp:1140
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC
static const std::string DEFAULT_COLLISION_OBJECT_TOPIC
The name of the topic used by default for receiving collision objects in the world.
Definition: planning_scene_monitor.h:122
planning_scene_monitor::PlanningSceneMonitor::startStateMonitor
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.
Definition: planning_scene_monitor.cpp:1156
planning_scene_monitor::PlanningSceneMonitor::excludeRobotLinksFromOctree
void excludeRobotLinksFromOctree()
Definition: planning_scene_monitor.cpp:720
ros::WallTimer::setPeriod
void setPeriod(const WallDuration &period, bool reset=true)
planning_scene_monitor::PlanningSceneMonitor::getPlanningSceneServiceCallback
bool getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request &req, moveit_msgs::GetPlanningScene::Response &res)
Definition: planning_scene_monitor.cpp:544
planning_scene_monitor::PlanningSceneMonitor::publish_planning_scene_
std::unique_ptr< boost::thread > publish_planning_scene_
Definition: planning_scene_monitor.h:528
ros::ServiceClient
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
planning_scene_monitor.h
planning_scene_monitor::PlanningSceneMonitor::last_robot_state_update_wall_time_
ros::WallTime last_robot_state_update_wall_time_
Last time the state was updated from current_state_monitor_.
Definition: planning_scene_monitor.h:611
planning_scene_monitor::PlanningSceneMonitor::get_scene_service_
ros::ServiceServer get_scene_service_
Definition: planning_scene_monitor.h:543
ros::NodeHandle::getCallbackQueue
CallbackQueueInterface * getCallbackQueue() const
moveit::core::AttachedBodyCallback
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
planning_scene_monitor::PlanningSceneMonitor::unlockSceneWrite
void unlockSceneWrite()
Lock the scene from writing (only one thread can lock for writing and no other thread can lock for re...
Definition: planning_scene_monitor.cpp:1012
planning_scene_monitor::PlanningSceneMonitor::robot_description_
std::string robot_description_
Definition: planning_scene_monitor.h:511
planning_scene_monitor::PlanningSceneMonitor::default_object_padd_
double default_object_padd_
default object padding
Definition: planning_scene_monitor.h:518
planning_scene_monitor::PlanningSceneMonitor::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: planning_scene_monitor.h:614
planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodyInOctree
void includeAttachedBodyInOctree(const moveit::core::AttachedBody *attached_body)
Definition: planning_scene_monitor.cpp:846
planning_scene_monitor::PlanningSceneMonitor::stateUpdateTimerCallback
void stateUpdateTimerCallback(const ros::WallTimerEvent &event)
Definition: planning_scene_monitor.cpp:1226
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
ros::WallTime
planning_scene_monitor::PlanningSceneMonitor::setPlanningScenePublishingFrequency
void setPlanningScenePublishingFrequency(double hz)
Set the maximum frequency at which planning scenes are being published.
Definition: planning_scene_monitor.cpp:1345
planning_scene_monitor::PlanningSceneMonitor::default_robot_link_scale_
std::map< std::string, double > default_robot_link_scale_
default robot link scale
Definition: planning_scene_monitor.h:524
planning_scene_monitor::PlanningSceneMonitor::monitor_name_
std::string monitor_name_
The name of this scene monitor.
Definition: planning_scene_monitor.h:495
collision_detection::OccMapTree::ReadLock
boost::shared_lock< boost::shared_mutex > ReadLock
ros::Rate::sleep
bool sleep()
planning_scene_monitor::PlanningSceneMonitor::publish_planning_scene_frequency_
double publish_planning_scene_frequency_
Definition: planning_scene_monitor.h:529
XmlRpc::XmlRpcValue::getType
const Type & getType() const
ROS_WARN_THROTTLE_NAMED
#define ROS_WARN_THROTTLE_NAMED(period, name,...)
planning_scene_monitor::PlanningSceneMonitor
PlanningSceneMonitor Subscribes to the topic planning_scene.
Definition: planning_scene_monitor.h:93
tf2::transformToEigen
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
ros::NodeHandle::setCallbackQueue
void setCallbackQueue(CallbackQueueInterface *queue)
shapes::OCTREE
OCTREE
planning_scene_monitor::PlanningSceneMonitor::shape_handles_lock_
boost::recursive_mutex shape_handles_lock_
Definition: planning_scene_monitor.h:562
planning_scene_monitor::PlanningSceneMonitor::startWorldGeometryMonitor
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 the OccupancyMapMonitor and listening for:
Definition: planning_scene_monitor.cpp:1097
exceptions.h
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
XmlRpc::XmlRpcValue::TypeArray
TypeArray
planning_scene_monitor::PlanningSceneMonitor::UPDATE_NONE
@ UPDATE_NONE
No update.
Definition: planning_scene_monitor.h:99
start
ROSCPP_DECL void start()
occupancy_map_monitor::ShapeHandle
unsigned int ShapeHandle
planning_scene_monitor::PlanningSceneMonitor::scene_
planning_scene::PlanningScenePtr scene_
Definition: planning_scene_monitor.h:497
planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneWorldCallback
void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world)
Callback for a new planning scene world.
Definition: planning_scene_monitor.cpp:667
planning_scene_monitor::PlanningSceneMonitor::nh_
ros::NodeHandle nh_
Last time the robot has moved.
Definition: planning_scene_monitor.h:504
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC
static const std::string DEFAULT_JOINT_STATES_TOPIC
The name of the topic used by default for receiving joint states.
Definition: planning_scene_monitor.h:116
planning_scene_monitor::PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback
void currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody *attached_body, bool just_attached)
Callback for a change for an attached object of the current state of the planning scene.
Definition: planning_scene_monitor.cpp:903
ros::WallTimerEvent
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC
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.
Definition: planning_scene_monitor.h:129
planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC
static const std::string MONITORED_PLANNING_SCENE_TOPIC
Definition: planning_scene_monitor.h:136
ros::Time
planning_scene_monitor::PlanningSceneMonitor::includeRobotLinksInOctree
void includeRobotLinksInOctree()
Definition: planning_scene_monitor.cpp:756
planning_scene_monitor::PlanningSceneMonitor::waitForCurrentRobotState
bool waitForCurrentRobotState(const ros::Time &t, double wait_time=1.)
Wait for robot state to become more recent than time t.
Definition: planning_scene_monitor.cpp:934
DurationBase< WallDuration >::toNSec
int64_t toNSec() const
planning_scene_monitor::PlanningSceneMonitor::state_update_pending_
volatile bool state_update_pending_
True when we need to update the RobotState from current_state_monitor_.
Definition: planning_scene_monitor.h:593
planning_scene_monitor::PlanningSceneMonitor::attachObjectCallback
void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr &obj)
Callback for a new attached object msg.
Definition: planning_scene_monitor.cpp:706
tf_buffer
tf2_ros::Buffer * tf_buffer
planning_scene_monitor::PlanningSceneMonitor::startSceneMonitor
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor (ROS topic-based)
Definition: planning_scene_monitor.cpp:1019
std
planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneCallback
void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene)
Definition: planning_scene_monitor.cpp:559
ros::ServiceClient::call
bool call(const MReq &req, MRes &resp, const std::string &service_md5sum)
moveit::core::AttachedBody::getShapes
const std::vector< shapes::ShapeConstPtr > & getShapes() const
planning_scene_monitor::PlanningSceneMonitor::planning_scene_subscriber_
ros::Subscriber planning_scene_subscriber_
Definition: planning_scene_monitor.h:535
planning_scene::PlanningScene::OCTOMAP_NS
static const MOVEIT_PLANNING_SCENE_EXPORT std::string OCTOMAP_NS
ros::ServiceServer::getService
std::string getService() const
planning_scene_monitor::PlanningSceneMonitor::includeWorldObjectsInOctree
void includeWorldObjectsInOctree()
Definition: planning_scene_monitor.cpp:799
moveit::tools::Profiler::ScopedBlock
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodiesFromOctree
void excludeAttachedBodiesFromOctree()
Definition: planning_scene_monitor.cpp:787
planning_scene_monitor::PlanningSceneMonitor::configureCollisionMatrix
void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene)
Configure the collision matrix for a particular scene.
Definition: planning_scene_monitor.cpp:1405
planning_scene_monitor::PlanningSceneMonitor::dt_state_update_
ros::WallDuration dt_state_update_
the amount of time to wait in between updates to the robot state
Definition: planning_scene_monitor.h:597
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY
@ UPDATE_GEOMETRY
The geometry of the scene was updated. This includes receiving new octomaps, collision objects,...
Definition: planning_scene_monitor.h:109
tf2_geometry_msgs.h
planning_scene_monitor::PlanningSceneMonitor::providePlanningSceneService
void providePlanningSceneService(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
Create an optional service for getting the complete planning scene This is useful for satisfying the ...
Definition: planning_scene_monitor.cpp:537
ros::Rate
planning_scene_monitor::PlanningSceneMonitor::getMonitoredTopics
void getMonitoredTopics(std::vector< std::string > &topics) const
Get the topic names that the monitor is listening to.
Definition: planning_scene_monitor.cpp:453
planning_scene_monitor::PlanningSceneMonitor::getUpdatedFrameTransforms
void getUpdatedFrameTransforms(std::vector< geometry_msgs::TransformStamped > &transforms)
Definition: planning_scene_monitor.cpp:1352
planning_scene_monitor::PlanningSceneMonitor::clearUpdateCallbacks
void clearUpdateCallbacks()
Clear the functions to be called when an update to the scene is received.
Definition: planning_scene_monitor.cpp:1339
planning_scene_monitor::PlanningSceneMonitor::clearOctomap
void clearOctomap()
Definition: planning_scene_monitor.cpp:564
planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE
@ UPDATE_SCENE
The entire scene was updated.
Definition: planning_scene_monitor.h:112
DurationBase< WallDuration >::toSec
double toSec() const
collision_detection::CollisionPluginLoader::setupScene
void setupScene(ros::NodeHandle &nh, const planning_scene::PlanningScenePtr &scene)
Fetch plugin name from parameter server and activate the plugin for the given scene.
Definition: collision_plugin_loader.cpp:40
moveit::ConstructException
planning_scene_monitor::PlanningSceneMonitor::parent_scene_
planning_scene::PlanningScenePtr parent_scene_
Definition: planning_scene_monitor.h:499
profiler.h
ros::WallDuration
collision_detection::World::CREATE
CREATE
tf2::TransformException
planning_scene
planning_scene_monitor::PlanningSceneMonitor::requestPlanningSceneState
bool requestPlanningSceneState(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
Request a full planning scene state using a service call Be careful not to use this in conjunction wi...
Definition: planning_scene_monitor.cpp:504
planning_scene_monitor::PlanningSceneMonitor::startPublishingPlanningScene
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...
Definition: planning_scene_monitor.cpp:352
planning_scene_monitor::PlanningSceneMonitor::monitorDiffs
void monitorDiffs(bool flag)
By default, the maintained planning scene does not reason about diffs. When the flag passed in is tru...
Definition: planning_scene_monitor.cpp:291
planning_scene_monitor::PlanningSceneMonitor::update_callbacks_
std::vector< boost::function< void(SceneUpdateType)> > update_callbacks_
Definition: planning_scene_monitor.h:566
planning_scene_monitor::PlanningSceneMonitor::UPDATE_TRANSFORMS
@ UPDATE_TRANSFORMS
The maintained set of fixed transforms in the monitored scene was updated.
Definition: planning_scene_monitor.h:105
ros::Duration
ros::NodeHandle::createWallTimer
WallTimer createWallTimer(WallDuration period, const WallTimerCallback &callback, bool oneshot=false, bool autostart=true) const
moveit::core::AttachedBody::getName
const std::string & getName() const
planning_scene_monitor::PlanningSceneMonitor::planning_scene_publisher_
ros::Publisher planning_scene_publisher_
Definition: planning_scene_monitor.h:527
t
geometry_msgs::TransformStamped t
planning_scene_monitor::PlanningSceneMonitor::link_shape_handles_
LinkShapeHandles link_shape_handles_
Definition: planning_scene_monitor.h:559
Transform.h
ros::WallTimer::stop
void stop()
planning_scene_monitor::PlanningSceneMonitor::includeWorldObjectInOctree
void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr &obj)
Definition: planning_scene_monitor.cpp:886
XmlRpc::XmlRpcValue
planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodiesInOctree
void includeAttachedBodiesInOctree()
Definition: planning_scene_monitor.cpp:771
planning_scene_monitor::PlanningSceneMonitor::scene_update_mutex_
boost::shared_mutex scene_update_mutex_
if diffs are monitored, this is the pointer to the parent scene
Definition: planning_scene_monitor.h:500
DurationBase< WallDuration >::isZero
bool isZero() const
ros::NodeHandle
planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor
PlanningSceneMonitor(const std::string &robot_description, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >(), const std::string &name="")
Constructor.
Definition: planning_scene_monitor.cpp:149
planning_scene_monitor::PlanningSceneMonitor::shape_transform_cache_lookup_wait_time_
ros::Duration shape_transform_cache_lookup_wait_time_
the amount of time to wait when looking up transforms
Definition: planning_scene_monitor.h:602
ros::Time::now
static Time now()
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
The name of the topic used by default for attached collision objects.
Definition: planning_scene_monitor.h:119
robot_model_loader.h
planning_scene_monitor::PlanningSceneMonitor::lockSceneWrite
void lockSceneWrite()
Lock the scene for writing (only one thread can lock for writing and no other thread can lock for rea...
Definition: planning_scene_monitor.cpp:1005


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Fri Sep 24 2021 02:24:16