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 <boost/algorithm/string/join.hpp>
52 
53 #include <memory>
54 
55 namespace planning_scene_monitor
56 {
57 using namespace moveit_ros_planning;
58 
59 static const std::string LOGNAME = "planning_scene_monitor";
60 
61 class PlanningSceneMonitor::DynamicReconfigureImpl
62 {
63 public:
64  DynamicReconfigureImpl(PlanningSceneMonitor* owner)
65  : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle(decideNamespace(owner->getName())))
66  {
67  dynamic_reconfigure_server_.setCallback(
68  [this](const auto& config, uint32_t level) { dynamicReconfigureCallback(config, level); });
69  }
70 
71 private:
72  // make sure we do not advertise the same service multiple times, in case we use multiple PlanningSceneMonitor
73  // instances in a process
74  static std::string decideNamespace(const std::string& name)
75  {
76  std::string ns = "~/" + name;
77  std::replace(ns.begin(), ns.end(), ' ', '_');
78  std::transform(ns.begin(), ns.end(), ns.begin(), ::tolower);
79  if (ros::service::exists(ns + "/set_parameters", false))
80  {
81  unsigned int c = 1;
82  while (ros::service::exists(ns + boost::lexical_cast<std::string>(c) + "/set_parameters", false))
83  c++;
84  ns += boost::lexical_cast<std::string>(c);
85  }
86  return ns;
87  }
88 
89  void dynamicReconfigureCallback(const PlanningSceneMonitorDynamicReconfigureConfig& config, uint32_t /*level*/)
90  {
92  if (config.publish_geometry_updates)
94  if (config.publish_state_updates)
96  if (config.publish_transforms_updates)
98  if (config.publish_planning_scene)
99  {
100  owner_->setPlanningScenePublishingFrequency(config.publish_planning_scene_hz);
101  owner_->startPublishingPlanningScene(event);
102  }
103  else
104  owner_->stopPublishingPlanningScene();
105  }
106 
107  PlanningSceneMonitor* owner_;
108  dynamic_reconfigure::Server<PlanningSceneMonitorDynamicReconfigureConfig> dynamic_reconfigure_server_;
109 };
110 
111 const std::string PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC = "joint_states";
112 const std::string PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC = "attached_collision_object";
113 const std::string PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC = "collision_object";
114 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC = "planning_scene_world";
115 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC = "planning_scene";
116 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE = "get_planning_scene";
117 const std::string PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC = "monitored_planning_scene";
118 
119 PlanningSceneMonitor::PlanningSceneMonitor(const std::string& robot_description,
120  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
121  : PlanningSceneMonitor(planning_scene::PlanningScenePtr(), robot_description, tf_buffer, name)
122 {
123 }
124 
125 PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
126  const std::string& robot_description,
127  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
128  : PlanningSceneMonitor(scene, std::make_shared<robot_model_loader::RobotModelLoader>(robot_description), tf_buffer,
129  name)
130 {
131 }
132 
133 PlanningSceneMonitor::PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr& rm_loader,
134  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
135  : PlanningSceneMonitor(planning_scene::PlanningScenePtr(), rm_loader, tf_buffer, name)
136 {
137 }
138 
139 PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
140  const robot_model_loader::RobotModelLoaderPtr& rm_loader,
141  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
142  : monitor_name_(name), nh_("~"), tf_buffer_(tf_buffer), rm_loader_(rm_loader)
143 {
146  spinner_ = std::make_shared<ros::AsyncSpinner>(1, &queue_);
147  spinner_->start();
148  initialize(scene);
149 }
150 
151 PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
152  const robot_model_loader::RobotModelLoaderPtr& rm_loader,
153  const ros::NodeHandle& nh, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
154  const std::string& name)
155  : monitor_name_(name), nh_("~"), root_nh_(nh), tf_buffer_(tf_buffer), rm_loader_(rm_loader)
156 {
157  // use same callback queue as root_nh_
159  initialize(scene);
160 }
161 
163 {
164  if (scene_)
165  {
166  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
167  scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback());
168  }
173 
174  spinner_.reset();
175  delete reconfigure_impl_;
176  current_state_monitor_.reset();
177  scene_const_.reset();
178  scene_.reset();
179  parent_scene_.reset();
180  robot_model_.reset();
181  rm_loader_.reset();
182 }
183 
184 void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& scene)
185 {
187  moveit::tools::Profiler::ScopedBlock prof_block("PlanningSceneMonitor::initialize");
188 
189  if (monitor_name_.empty())
190  monitor_name_ = "planning_scene_monitor";
191  robot_description_ = rm_loader_->getRobotDescription();
192  if (rm_loader_->getModel())
193  {
194  robot_model_ = rm_loader_->getModel();
195  scene_ = scene;
196  if (!scene_)
197  {
198  try
199  {
200  scene_ = std::make_shared<planning_scene::PlanningScene>(rm_loader_->getModel());
203 
204  scene_->getCollisionEnvNonConst()->setPadding(default_robot_padd_);
205  scene_->getCollisionEnvNonConst()->setScale(default_robot_scale_);
206  for (const std::pair<const std::string, double>& it : default_robot_link_padd_)
207  {
208  scene_->getCollisionEnvNonConst()->setLinkPadding(it.first, it.second);
209  }
210  for (const std::pair<const std::string, double>& it : default_robot_link_scale_)
211  {
212  scene_->getCollisionEnvNonConst()->setLinkScale(it.first, it.second);
213  }
214  scene_->propogateRobotPadding();
215  }
217  {
218  ROS_ERROR_NAMED(LOGNAME, "Configuration of planning scene failed");
219  scene_.reset();
220  }
221  }
222  // scene_const_ is set regardless if scene_ is null or not
224  if (scene_)
225  {
226  // The scene_ is loaded on the collision loader only if it was correctly instantiated
228  scene_->setAttachedBodyUpdateCallback([this](moveit::core::AttachedBody* body, bool attached) {
230  });
231  scene_->setCollisionObjectUpdateCallback(
232  [this](const collision_detection::World::ObjectConstPtr& object, collision_detection::World::Action action) {
233  currentWorldObjectUpdateCallback(object, action);
234  });
235  }
236  }
237  else
238  {
239  ROS_ERROR_NAMED(LOGNAME, "Robot model not loaded");
240  }
241 
244 
248 
249  double temp_wait_time = 0.05;
250 
251  if (!robot_description_.empty())
252  nh_.param(robot_description_ + "_planning/shape_transform_cache_lookup_wait_time", temp_wait_time, temp_wait_time);
253 
255 
256  state_update_pending_.store(false);
258  false, // not a oneshot timer
259  false); // do not start the timer yet
260 
261  reconfigure_impl_ = new DynamicReconfigureImpl(this);
262 
263  std::vector<std::string> ignored_frames_vector;
264  nh_.param("planning_scene_monitor_options/ignored_frames", ignored_frames_vector, std::vector<std::string>());
265  ignored_frames_ = std::set<std::string>(ignored_frames_vector.begin(), ignored_frames_vector.end());
266 }
267 
269 {
270  if (scene_)
271  {
272  if (flag)
273  {
274  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
275  if (scene_)
276  {
277  scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback());
278  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
279  scene_->decoupleParent();
281  scene_ = parent_scene_->diff();
283  scene_->setAttachedBodyUpdateCallback([this](moveit::core::AttachedBody* body, bool attached) {
285  });
286  scene_->setCollisionObjectUpdateCallback(
287  [this](const collision_detection::World::ObjectConstPtr& object,
289  }
290  }
291  else
292  {
294  {
295  ROS_WARN_NAMED(LOGNAME, "Diff monitoring was stopped while publishing planning scene diffs. "
296  "Stopping planning scene diff publisher");
298  }
299  {
300  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
301  if (scene_)
302  {
303  scene_->decoupleParent();
304  parent_scene_.reset();
305  // remove the '+' added by .diff() at the end of the scene name
306  if (!scene_->getName().empty())
307  {
308  if (scene_->getName()[scene_->getName().length() - 1] == '+')
309  scene_->setName(scene_->getName().substr(0, scene_->getName().length() - 1));
310  }
311  }
312  }
313  }
314  }
315 }
316 
318 {
320  {
321  std::unique_ptr<boost::thread> copy;
322  copy.swap(publish_planning_scene_);
323  new_scene_update_condition_.notify_all();
324  copy->join();
325  monitorDiffs(false);
327  ROS_INFO_NAMED(LOGNAME, "Stopped publishing maintained planning scene.");
328  }
329 }
330 
331 void PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_type,
332  const std::string& planning_scene_topic)
333 {
334  publish_update_types_ = update_type;
336  {
337  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>(planning_scene_topic, 100, false);
338  ROS_INFO_NAMED(LOGNAME, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
339  monitorDiffs(true);
340  publish_planning_scene_ = std::make_unique<boost::thread>([this] { scenePublishingThread(); });
341  }
342 }
343 
345 {
346  ROS_DEBUG_NAMED(LOGNAME, "Started scene publishing thread ...");
347 
348  // publish the full planning scene once
349  {
350  moveit_msgs::PlanningScene msg;
351  {
353  if (octomap_monitor_)
354  lock = octomap_monitor_->getOcTreePtr()->reading();
355  scene_->getPlanningSceneMsg(msg);
356  }
358  ROS_DEBUG_NAMED(LOGNAME, "Published the full planning scene: '%s'", msg.name.c_str());
359  }
360 
361  do
362  {
363  moveit_msgs::PlanningScene msg;
364  bool publish_msg = false;
365  bool is_full = false;
367  {
368  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
370  new_scene_update_condition_.wait(ulock);
372  {
374  {
376  is_full = true;
377  else
378  {
380  if (octomap_monitor_)
381  lock = octomap_monitor_->getOcTreePtr()->reading();
382  scene_->getPlanningSceneDiffMsg(msg);
384  {
385  msg.robot_state.attached_collision_objects.clear();
386  msg.robot_state.is_diff = true;
387  }
388  }
389  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the
390  // transform cache to
391  // update while we are
392  // potentially changing
393  // attached bodies
394  scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback());
395  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
396  scene_->pushDiffs(parent_scene_);
397  scene_->clearDiffs();
398  scene_->setAttachedBodyUpdateCallback([this](moveit::core::AttachedBody* body, bool attached) {
400  });
401  scene_->setCollisionObjectUpdateCallback(
402  [this](const collision_detection::World::ObjectConstPtr& object,
404  if (octomap_monitor_)
405  {
406  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
407  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
408  }
409  if (is_full)
410  {
412  if (octomap_monitor_)
413  lock = octomap_monitor_->getOcTreePtr()->reading();
414  scene_->getPlanningSceneMsg(msg);
415  }
416  // also publish timestamp of this robot_state
417  msg.robot_state.joint_state.header.stamp = last_robot_motion_time_;
418  publish_msg = true;
419  }
421  }
422  }
423  if (publish_msg)
424  {
426  if (is_full)
427  ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str());
428  rate.sleep();
429  }
430  } while (publish_planning_scene_);
431 }
432 
433 void PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string>& topics) const
434 {
435  topics.clear();
437  {
438  const std::string& t = current_state_monitor_->getMonitoredTopic();
439  if (!t.empty())
440  topics.push_back(t);
441  }
443  topics.push_back(planning_scene_subscriber_.getTopic());
445  topics.push_back(collision_object_subscriber_.getTopic());
447  topics.push_back(planning_scene_world_subscriber_.getTopic());
448 }
449 
450 namespace
451 {
452 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr& scene,
453  const planning_scene::PlanningScene* possible_parent)
454 {
455  if (scene && scene.get() == possible_parent)
456  return true;
457  if (scene)
458  return sceneIsParentOf(scene->getParent(), possible_parent);
459  return false;
460 }
461 } // namespace
462 
463 bool PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr& scene) const
464 {
465  return sceneIsParentOf(scene_const_, scene.get());
466 }
467 
468 bool PlanningSceneMonitor::updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const
469 {
470  return sceneIsParentOf(scene_const_, scene.get());
471 }
472 
473 void PlanningSceneMonitor::triggerSceneUpdateEvent(SceneUpdateType update_type)
474 {
475  // do not modify update functions while we are calling them
476  boost::recursive_mutex::scoped_lock lock(update_lock_);
477 
478  for (boost::function<void(SceneUpdateType)>& update_callback : update_callbacks_)
479  update_callback(update_type);
480  new_scene_update_ = (SceneUpdateType)((int)new_scene_update_ | (int)update_type);
481  new_scene_update_condition_.notify_all();
482 }
483 
484 bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_name)
485 {
486  if (get_scene_service_.getService() == service_name)
487  {
488  ROS_FATAL_STREAM_NAMED(LOGNAME, "requestPlanningSceneState() to self-provided service '" << service_name << "'");
489  throw std::runtime_error("requestPlanningSceneState() to self-provided service: " + service_name);
490  }
491  // use global namespace for service
492  ros::ServiceClient client = ros::NodeHandle().serviceClient<moveit_msgs::GetPlanningScene>(service_name);
493  // all scene components are returned if none are specified
494  moveit_msgs::GetPlanningScene srv;
495 
496  // Make sure client is connected to server
497  if (!client.exists())
498  {
499  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for service `" << service_name << "` to exist.");
501  }
502 
503  if (client.call(srv))
504  {
505  newPlanningSceneMessage(srv.response.scene);
506  }
507  else
508  {
510  LOGNAME, "Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?",
511  service_name.c_str());
512  return false;
513  }
514  return true;
515 }
516 
517 void PlanningSceneMonitor::providePlanningSceneService(const std::string& service_name)
518 {
519  // Load the service
522 }
523 
524 bool PlanningSceneMonitor::getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req,
525  moveit_msgs::GetPlanningScene::Response& res)
526 {
527  if (req.components.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
529 
530  moveit_msgs::PlanningSceneComponents all_components;
531  all_components.components = UINT_MAX; // Return all scene components if nothing is specified.
532 
533  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
534  scene_->getPlanningSceneMsg(res.scene, req.components.components ? req.components : all_components);
535 
536  return true;
537 }
538 
539 void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene)
540 {
541  newPlanningSceneMessage(*scene);
542 }
543 
545 {
546  bool removed = false;
547  {
548  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
549  removed = scene_->getWorldNonConst()->removeObject(scene_->OCTOMAP_NS);
550 
551  if (octomap_monitor_)
552  {
553  octomap_monitor_->getOcTreePtr()->lockWrite();
554  octomap_monitor_->getOcTreePtr()->clear();
555  octomap_monitor_->getOcTreePtr()->unlockWrite();
556  }
557  else
558  {
559  ROS_WARN_NAMED(LOGNAME, "Unable to clear octomap since no octomap monitor has been initialized");
560  } // Lift the scoped lock before calling triggerSceneUpdateEvent to avoid deadlock
561  }
562 
563  if (removed)
565 }
566 
567 bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
568 {
569  if (!scene_)
570  return false;
571 
572  bool result;
573 
575  std::string old_scene_name;
576  {
577  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
578  // we don't want the transform cache to update while we are potentially changing attached bodies
579  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
580 
582  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
583  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
584  "scene update " << fmod(last_update_time_.toSec(), 10.)
585  << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
586  old_scene_name = scene_->getName();
587 
588  if (!scene.is_diff && parent_scene_)
589  {
590  // If there is no new robot_state, transfer RobotState from current scene to parent scene
591  if (scene.robot_state.is_diff)
592  parent_scene_->setCurrentState(scene_->getCurrentState());
593  scene_->clearDiffs();
594  result = parent_scene_->setPlanningSceneMsg(scene);
595  // There were no callbacks for individual object changes, so rebuild the octree masks
598  }
599  else
600  {
601  result = scene_->usePlanningSceneMsg(scene);
602  }
603 
604  if (octomap_monitor_)
605  {
606  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
607  {
608  octomap_monitor_->getOcTreePtr()->lockWrite();
609  octomap_monitor_->getOcTreePtr()->clear();
610  octomap_monitor_->getOcTreePtr()->unlockWrite();
611  }
612  }
613  robot_model_ = scene_->getRobotModel();
614 
615  if (octomap_monitor_)
616  {
617  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
618  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
619  }
620  }
621 
622  // if we have a diff, try to more accurately determine the update type
623  if (scene.is_diff)
624  {
625  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
626  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
627  scene.link_scale.empty();
628  if (no_other_scene_upd)
629  {
630  upd = UPDATE_NONE;
631  if (!moveit::core::isEmpty(scene.world))
632  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
633 
634  if (!scene.fixed_frame_transforms.empty())
635  upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
636 
637  if (!moveit::core::isEmpty(scene.robot_state))
638  {
639  upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
640  if (!scene.robot_state.attached_collision_objects.empty() || !static_cast<bool>(scene.robot_state.is_diff))
641  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
642  }
643  }
644  }
646  return result;
647 }
648 
649 void PlanningSceneMonitor::newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world)
650 {
651  if (scene_)
652  {
654  {
655  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
657  scene_->getWorldNonConst()->clearObjects();
658  scene_->processPlanningSceneWorldMsg(*world);
659  if (octomap_monitor_)
660  {
661  if (world->octomap.octomap.data.empty())
662  {
663  octomap_monitor_->getOcTreePtr()->lockWrite();
664  octomap_monitor_->getOcTreePtr()->clear();
665  octomap_monitor_->getOcTreePtr()->unlockWrite();
666  }
667  }
668  }
670  }
671 }
672 
673 void PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj)
674 {
675  if (!scene_)
676  return;
677 
679  {
680  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
682  if (!scene_->processCollisionObjectMsg(*obj))
683  return;
684  }
686 }
687 
688 void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
689 {
690  if (scene_)
691  {
693  {
694  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
696  scene_->processAttachedCollisionObjectMsg(*obj);
697  }
699  }
700 }
701 
703 {
704  if (!octomap_monitor_)
705  return;
706 
707  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
708 
710  const std::vector<const moveit::core::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
712  bool warned = false;
713  for (const moveit::core::LinkModel* link : links)
714  {
715  std::vector<shapes::ShapeConstPtr> shapes = link->getShapes(); // copy shared ptrs on purpuse
716  for (std::size_t j = 0; j < shapes.size(); ++j)
717  {
718  // merge mesh vertices up to 0.1 mm apart
719  if (shapes[j]->type == shapes::MESH)
720  {
721  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
722  m->mergeVertices(1e-4);
723  shapes[j].reset(m);
724  }
725 
727  if (h)
728  link_shape_handles_[link].push_back(std::make_pair(h, j));
729  }
730  if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
731  {
732  ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
733  warned = true;
734  }
735  }
736 }
737 
739 {
740  if (!octomap_monitor_)
741  return;
742 
743  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
744 
745  for (std::pair<const moveit::core::LinkModel* const,
746  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
748  for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : link_shape_handle.second)
749  octomap_monitor_->forgetShape(it.first);
750  link_shape_handles_.clear();
751 }
752 
754 {
755  if (!octomap_monitor_)
756  return;
757 
758  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
759 
760  // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
761  for (std::pair<const moveit::core::AttachedBody* const,
762  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& attached_body_shape_handle :
764  for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : attached_body_shape_handle.second)
765  octomap_monitor_->forgetShape(it.first);
767 }
768 
770 {
771  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
772 
774  // add attached objects again
775  std::vector<const moveit::core::AttachedBody*> ab;
776  scene_->getCurrentState().getAttachedBodies(ab);
777  for (const moveit::core::AttachedBody* body : ab)
779 }
780 
782 {
783  if (!octomap_monitor_)
784  return;
785 
786  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
787 
788  // clear information about any attached object
789  for (std::pair<const std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
790  collision_body_shape_handle : collision_body_shape_handles_)
791  for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
792  collision_body_shape_handle.second)
793  octomap_monitor_->forgetShape(it.first);
795 }
796 
798 {
799  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
800 
802  for (const std::pair<const std::string, collision_detection::World::ObjectPtr>& it : *scene_->getWorld())
803  excludeWorldObjectFromOctree(it.second);
804 }
805 
807 {
808  if (!octomap_monitor_)
809  return;
810 
811  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
812  bool found = false;
813  for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
814  {
815  if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
816  continue;
817  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
818  if (h)
819  {
820  found = true;
821  attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
822  }
823  }
824  if (found)
825  ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
826 }
827 
829 {
830  if (!octomap_monitor_)
831  return;
832 
833  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
834 
835  AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
836  if (it != attached_body_shape_handles_.end())
837  {
838  for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& shape_handle : it->second)
839  octomap_monitor_->forgetShape(shape_handle.first);
840  ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
842  }
843 }
844 
845 void PlanningSceneMonitor::excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj)
846 {
847  if (!octomap_monitor_)
848  return;
849 
850  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
851 
852  bool found = false;
853  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
854  {
855  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
856  continue;
857  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
858  if (h)
859  {
860  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->global_shape_poses_[i]));
861  found = true;
862  }
863  }
864  if (found)
865  ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
866 }
867 
868 void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj)
869 {
870  if (!octomap_monitor_)
871  return;
872 
873  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
874 
875  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
876  if (it != collision_body_shape_handles_.end())
877  {
878  for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& shape_handle : it->second)
879  octomap_monitor_->forgetShape(shape_handle.first);
880  ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
882  }
883 }
884 
886  bool just_attached)
887 {
888  if (!octomap_monitor_)
889  return;
890 
891  if (just_attached)
892  excludeAttachedBodyFromOctree(attached_body);
893  else
894  includeAttachedBodyInOctree(attached_body);
895 }
896 
897 void PlanningSceneMonitor::currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& obj,
899 {
901  return;
903  return;
904 
907  else if (action & collision_detection::World::DESTROY)
909  else
910  {
913  }
914 }
915 
916 bool PlanningSceneMonitor::waitForCurrentRobotState(const ros::Time& t, double wait_time)
917 {
918  if (t.isZero())
919  return false;
921  ros::WallDuration timeout(wait_time);
922 
923  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
924 
926  {
927  // Wait for next robot update in state monitor.
928  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
929 
930  /* As robot updates are passed to the planning scene only in throttled fashion, there might
931  be still an update pending. If so, explicitly update the planning scene here.
932  If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
933  if (success)
934  {
935  if (state_update_pending_.load()) // perform state update
937  return true;
938  }
939 
940  ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
941  return false;
942  }
943 
944  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
945  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
946  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
947  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
948  ros::Time prev_robot_motion_time = last_robot_motion_time_;
949  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
950  timeout > ros::WallDuration())
951  {
952  ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
953  new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
954  timeout -= ros::WallTime::now() - start; // compute remaining wait_time
955  }
956  bool success = last_robot_motion_time_ >= t;
957  // suppress warning if we received an update at all
958  if (!success && prev_robot_motion_time != last_robot_motion_time_)
959  ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
960  (t - last_robot_motion_time_).toSec());
961 
962  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
963  << " scene update: " << (t - last_update_time_).toSec());
964  return success;
965 }
966 
968 {
969  scene_update_mutex_.lock_shared();
970  if (octomap_monitor_)
971  octomap_monitor_->getOcTreePtr()->lockRead();
972 }
973 
975 {
976  if (octomap_monitor_)
977  octomap_monitor_->getOcTreePtr()->unlockRead();
978  scene_update_mutex_.unlock_shared();
979 }
980 
982 {
983  scene_update_mutex_.lock();
984  if (octomap_monitor_)
985  octomap_monitor_->getOcTreePtr()->lockWrite();
986 }
987 
989 {
990  if (octomap_monitor_)
991  octomap_monitor_->getOcTreePtr()->unlockWrite();
992  scene_update_mutex_.unlock();
993 }
994 
995 void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
996 {
998 
999  ROS_INFO_NAMED(LOGNAME, "Starting planning scene monitor");
1000  // listen for planning scene updates; these messages include transforms, so no need for filters
1001  if (!scene_topic.empty())
1002  {
1005  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
1006  }
1007 }
1008 
1010 {
1012  {
1013  ROS_INFO_NAMED(LOGNAME, "Stopping planning scene monitor");
1015  }
1016 }
1017 
1018 bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
1021  if (!tf_buffer_)
1022  return false;
1023  try
1024  {
1025  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
1026 
1027  for (const std::pair<const moveit::core::LinkModel* const,
1028  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
1030  {
1031  tf_buffer_->canTransform(target_frame, link_shape_handle.first->getName(), target_time,
1033  Eigen::Isometry3d ttr = tf2::transformToEigen(
1034  tf_buffer_->lookupTransform(target_frame, link_shape_handle.first->getName(), target_time));
1035  for (std::size_t j = 0; j < link_shape_handle.second.size(); ++j)
1036  cache[link_shape_handle.second[j].first] =
1037  ttr * link_shape_handle.first->getCollisionOriginTransforms()[link_shape_handle.second[j].second];
1038  }
1039  for (const std::pair<const moveit::core::AttachedBody* const,
1040  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>&
1041  attached_body_shape_handle : attached_body_shape_handles_)
1042  {
1043  tf_buffer_->canTransform(target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time,
1045  Eigen::Isometry3d transform = tf2::transformToEigen(tf_buffer_->lookupTransform(
1046  target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time));
1047  for (std::size_t k = 0; k < attached_body_shape_handle.second.size(); ++k)
1048  cache[attached_body_shape_handle.second[k].first] =
1049  transform *
1050  attached_body_shape_handle.first->getShapePosesInLinkFrame()[attached_body_shape_handle.second[k].second];
1051  }
1052  {
1053  tf_buffer_->canTransform(target_frame, scene_->getPlanningFrame(), target_time,
1055  Eigen::Isometry3d transform =
1056  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time));
1057  for (const std::pair<const std::string,
1058  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
1059  collision_body_shape_handle : collision_body_shape_handles_)
1060  for (const std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
1061  collision_body_shape_handle.second)
1062  cache[it.first] = transform * (*it.second);
1063  }
1064  }
1065  catch (tf2::TransformException& ex)
1066  {
1067  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
1068  return false;
1069  }
1070  return true;
1071 }
1072 
1073 void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collision_objects_topic,
1074  const std::string& planning_scene_world_topic,
1075  const bool load_octomap_monitor)
1076 {
1078  ROS_INFO_NAMED(LOGNAME, "Starting world geometry update monitor for collision objects, attached objects, octomap "
1079  "updates.");
1080 
1081  // Listen to the /collision_objects topic to detect requests to add/remove/update collision objects to/from the world
1082  if (!collision_objects_topic.empty())
1083  {
1085  root_nh_.subscribe(collision_objects_topic, 1024, &PlanningSceneMonitor::collisionObjectCallback, this);
1086  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
1087  }
1088 
1089  if (!planning_scene_world_topic.empty())
1090  {
1092  root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
1093  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
1094  root_nh_.resolveName(planning_scene_world_topic).c_str());
1095  }
1096 
1097  // Ocotomap monitor is optional
1098  if (load_octomap_monitor)
1099  {
1100  if (!octomap_monitor_)
1101  {
1103  std::make_unique<occupancy_map_monitor::OccupancyMapMonitor>(tf_buffer_, scene_->getPlanningFrame());
1107 
1108  octomap_monitor_->setTransformCacheCallback(
1109  [this](const std::string& frame, const ros::Time& stamp, occupancy_map_monitor::ShapeTransformCache& cache) {
1110  return getShapeTransformCache(frame, stamp, cache);
1111  });
1112  octomap_monitor_->setUpdateCallback([this] { octomapUpdateCallback(); });
1113  }
1114  octomap_monitor_->startMonitor();
1115  }
1116 }
1117 
1119 {
1121  {
1122  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1124  }
1126  {
1127  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1129  }
1130  if (octomap_monitor_)
1131  octomap_monitor_->stopMonitor();
1132 }
1133 
1134 void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1135  const std::string& attached_objects_topic)
1136 {
1137  stopStateMonitor();
1138  if (scene_)
1139  {
1141  current_state_monitor_ = std::make_shared<CurrentStateMonitor>(getRobotModel(), tf_buffer_, root_nh_);
1142  current_state_monitor_->addUpdateCallback(
1143  [this](const sensor_msgs::JointStateConstPtr& state) { onStateUpdate(state); });
1144  current_state_monitor_->startStateMonitor(joint_states_topic);
1145 
1146  {
1147  boost::mutex::scoped_lock lock(state_update_mutex_);
1148  if (!dt_state_update_.isZero())
1150  }
1151 
1152  if (!attached_objects_topic.empty())
1153  {
1154  // using regular message filter as there's no header
1156  root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
1157  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
1158  root_nh_.resolveName(attached_objects_topic).c_str());
1159  }
1160  }
1161  else
1162  ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
1163 }
1164 
1168  current_state_monitor_->stopStateMonitor();
1171 
1173  state_update_pending_.store(false);
1174 }
1175 
1176 void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr& /* joint_state */)
1177 {
1178  state_update_pending_.store(true);
1179 
1180  // Read access to last_robot_state_update_wall_time_ and dt_state_update_ is unprotected here
1181  // as reading invalid values is not critical (just postpones the next state update)
1182  // only update every dt_state_update_ seconds
1185 }
1186 
1188 {
1189  // Read access to last_robot_state_update_wall_time_ and dt_state_update_ is unprotected here
1190  // as reading invalid values is not critical (just postpones the next state update)
1193 }
1194 
1196 {
1198  return;
1199 
1201  {
1202  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1204  octomap_monitor_->getOcTreePtr()->lockRead();
1205  try
1206  {
1207  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Isometry3d::Identity());
1208  octomap_monitor_->getOcTreePtr()->unlockRead();
1209  }
1210  catch (...)
1211  {
1212  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1213  throw;
1214  }
1215  }
1217 }
1218 
1220 {
1221  bool update = false;
1222  if (hz > std::numeric_limits<double>::epsilon())
1223  {
1224  boost::mutex::scoped_lock lock(state_update_mutex_);
1225  dt_state_update_.fromSec(1.0 / hz);
1228  }
1229  else
1230  {
1231  // stop must be called with state_update_mutex_ unlocked to avoid deadlock
1233  boost::mutex::scoped_lock lock(state_update_mutex_);
1235  if (state_update_pending_.load())
1236  update = true;
1237  }
1238  ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
1239 
1240  if (update)
1242 }
1243 
1244 void PlanningSceneMonitor::updateSceneWithCurrentState(bool skip_update_if_locked)
1245 {
1247  {
1248  std::vector<std::string> missing;
1249  if (!current_state_monitor_->haveCompleteState(missing) &&
1250  (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
1251  {
1252  std::string missing_str = boost::algorithm::join(missing, ", ");
1253  ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known. Missing %s",
1254  missing_str.c_str());
1255  }
1256 
1257  {
1258  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_, boost::defer_lock);
1259  if (!skip_update_if_locked)
1260  ulock.lock();
1261  else if (!ulock.try_lock())
1262  // Return if we can't lock scene_update_mutex within 100ms, thus not blocking CurrentStateMonitor too long
1263  return;
1264 
1266  ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
1267  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1268  scene_->getCurrentStateNonConst().update(); // compute all transforms
1269  }
1270 
1271  // Update state_update_mutex_ and last_robot_state_update_wall_time_
1272  {
1273  boost::mutex::scoped_lock lock(state_update_mutex_);
1275  state_update_pending_.store(false);
1276  }
1277 
1279  }
1280  else
1281  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
1282 }
1283 
1284 void PlanningSceneMonitor::addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn)
1285 {
1286  boost::recursive_mutex::scoped_lock lock(update_lock_);
1287  if (fn)
1288  update_callbacks_.push_back(fn);
1289 }
1290 
1292 {
1293  boost::recursive_mutex::scoped_lock lock(update_lock_);
1294  update_callbacks_.clear();
1295 }
1296 
1298 {
1300  ROS_DEBUG_NAMED(LOGNAME, "Maximum frequency for publishing a planning scene is now %lf Hz",
1302 }
1303 
1304 void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms)
1305 {
1306  const std::string& target = getRobotModel()->getModelFrame();
1307 
1308  std::vector<std::string> all_frame_names;
1309  tf_buffer_->_getFrameStrings(all_frame_names);
1310  for (const std::string& all_frame_name : all_frame_names)
1311  {
1312  if (all_frame_name == target || getRobotModel()->hasLinkModel(all_frame_name) || checkFrameIgnored(all_frame_name))
1313  continue;
1314 
1315  geometry_msgs::TransformStamped f;
1316  try
1317  {
1318  f = tf_buffer_->lookupTransform(target, all_frame_name, ros::Time(0));
1319  }
1320  catch (tf2::TransformException& ex)
1321  {
1322  ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
1323  << all_frame_name << "' to planning frame '" << target << "' (" << ex.what()
1324  << ")");
1325  continue;
1326  }
1327  f.header.frame_id = all_frame_name;
1328  f.child_frame_id = target;
1329  transforms.push_back(f);
1330  }
1331 }
1332 
1334 {
1335  if (!tf_buffer_)
1336  return;
1337 
1338  if (scene_)
1339  {
1340  std::vector<geometry_msgs::TransformStamped> transforms;
1341  getUpdatedFrameTransforms(transforms);
1342  {
1343  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1344  scene_->getTransformsNonConst().setTransforms(transforms);
1346  }
1348  }
1349 }
1350 
1352 {
1353  if (octomap_monitor_)
1354  octomap_monitor_->publishDebugInformation(flag);
1355 }
1356 
1357 void PlanningSceneMonitor::configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene)
1358 {
1359  if (!scene || robot_description_.empty())
1360  return;
1361  collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
1362 
1363  // read overriding values from the param server
1364 
1365  // first we do default collision operations
1366  if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
1367  ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
1368  else
1369  {
1370  ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
1371 
1372  XmlRpc::XmlRpcValue coll_ops;
1373  nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
1374 
1375  if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
1376  {
1377  ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
1378  return;
1379  }
1380 
1381  if (coll_ops.size() == 0)
1382  {
1383  ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
1384  return;
1385  }
1386 
1387  for (int i = 0; i < coll_ops.size(); ++i) // NOLINT(modernize-loop-convert)
1388  {
1389  if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
1390  {
1391  ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
1392  continue;
1393  }
1394  acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
1395  std::string(coll_ops[i]["operation"]) == "disable");
1396  }
1397  }
1398 }
1399 
1401 {
1402  if (robot_description_.empty())
1403  {
1404  default_robot_padd_ = 0.0;
1405  default_robot_scale_ = 1.0;
1406  default_object_padd_ = 0.0;
1407  default_attached_padd_ = 0.0;
1408  return;
1409  }
1410 
1411  // print deprecation warning if necessary
1412  // TODO: remove this warning after 06/2022
1413  const std::string old_robot_description =
1414  (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
1415  if (nh_.resolveName(old_robot_description) != nh_.resolveName(robot_description_))
1416  {
1417  if (nh_.hasParam(old_robot_description + "_planning/default_robot_padding") ||
1418  nh_.hasParam(old_robot_description + "_planning/default_robot_scale") ||
1419  nh_.hasParam(old_robot_description + "_planning/default_object_padding") ||
1420  nh_.hasParam(old_robot_description + "_planning/default_attached_padding") ||
1421  nh_.hasParam(old_robot_description + "_planning/default_robot_link_padding") ||
1422  nh_.hasParam(old_robot_description + "_planning/default_robot_link_scale"))
1423  {
1424  ROS_WARN_STREAM_NAMED(LOGNAME, "The path for the padding parameters has changed!\n"
1425  "Old parameter path: '"
1426  << nh_.resolveName(old_robot_description + "_planning/")
1427  << "'\n"
1428  "New parameter path: '"
1429  << nh_.resolveName(robot_description_ + "_planning/")
1430  << "'\n"
1431  "Ignoring old parameters. Please update your moveit config!");
1432  }
1433  }
1434 
1435  nh_.param(robot_description_ + "_planning/default_robot_padding", default_robot_padd_, 0.0);
1436  nh_.param(robot_description_ + "_planning/default_robot_scale", default_robot_scale_, 1.0);
1437  nh_.param(robot_description_ + "_planning/default_object_padding", default_object_padd_, 0.0);
1438  nh_.param(robot_description_ + "_planning/default_attached_padding", default_attached_padd_, 0.0);
1439  nh_.param(robot_description_ + "_planning/default_robot_link_padding", default_robot_link_padd_,
1440  std::map<std::string, double>());
1441  nh_.param(robot_description_ + "_planning/default_robot_link_scale", default_robot_link_scale_,
1442  std::map<std::string, double>());
1443 
1444  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
1445  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
1446 }
1447 
1448 bool PlanningSceneMonitor::checkFrameIgnored(const std::string& frame)
1449 {
1450  return (ignored_frames_.find(frame) != ignored_frames_.end());
1451 }
1452 } // namespace planning_scene_monitor
planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectFromOctree
void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj)
Definition: planning_scene_monitor.cpp:877
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:537
moveit::core::LinkModel
planning_scene_monitor::PlanningSceneMonitor::collision_body_shape_handles_
CollisionBodyShapeHandles collision_body_shape_handles_
Definition: planning_scene_monitor.h:562
planning_scene_monitor::PlanningSceneMonitor::publishDebugInformation
void publishDebugInformation(bool flag)
Definition: planning_scene_monitor.cpp:1383
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:1208
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:929
planning_scene_monitor::PlanningSceneMonitor::default_attached_padd_
double default_attached_padd_
default attached padding
Definition: planning_scene_monitor.h:521
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:523
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:1432
shapes
planning_scene_monitor::PlanningSceneMonitor::attached_body_shape_handles_
AttachedBodyShapeHandles attached_body_shape_handles_
Definition: planning_scene_monitor.h:561
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:505
planning_scene_monitor::PlanningSceneMonitor::~PlanningSceneMonitor
~PlanningSceneMonitor()
Definition: planning_scene_monitor.cpp:194
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:1050
planning_scene_monitor::PlanningSceneMonitor::stopStateMonitor
void stopStateMonitor()
Stop the state monitor.
Definition: planning_scene_monitor.cpp:1197
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:500
planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage
bool newPlanningSceneMessage(const moveit_msgs::PlanningScene &scene)
Definition: planning_scene_monitor.cpp:599
planning_scene_monitor::PlanningSceneMonitor::octomapUpdateCallback
void octomapUpdateCallback()
Callback for octomap updates.
Definition: planning_scene_monitor.cpp:1227
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:1316
planning_scene_monitor::PlanningSceneMonitor::spinner_
std::shared_ptr< ros::AsyncSpinner > spinner_
Definition: planning_scene_monitor.h:508
ros::ServiceClient::exists
bool exists()
shapes::PLANE
PLANE
planning_scene_monitor::PlanningSceneMonitor::updateSceneWithCurrentState
void updateSceneWithCurrentState(bool skip_update_if_locked=false)
Update the scene using the monitored state. This function is automatically called when an update to t...
Definition: planning_scene_monitor.cpp:1276
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:617
planning_scene_monitor::PlanningSceneMonitor::scenePublishingThread
void scenePublishingThread()
Definition: planning_scene_monitor.cpp:376
planning_scene_monitor::PlanningSceneMonitor::default_robot_padd_
double default_robot_padd_
default robot padding
Definition: planning_scene_monitor.h:515
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:539
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:1251
planning_scene_monitor::PlanningSceneMonitor::collisionObjectCallback
void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr &obj)
Callback for a new collision object msg.
Definition: planning_scene_monitor.cpp:705
planning_scene_monitor::PlanningSceneMonitor::scene_const_
planning_scene::PlanningSceneConstPtr scene_const_
Definition: planning_scene_monitor.h:499
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:566
planning_scene_monitor::PlanningSceneMonitor::state_update_pending_
std::atomic< bool > state_update_pending_
True if current_state_monitor_ has a newer RobotState than scene_.
Definition: planning_scene_monitor.h:590
TimeBase< Time, Duration >::toSec
double toSec() const
planning_scene_monitor::PlanningSceneMonitor::collision_object_subscriber_
ros::Subscriber collision_object_subscriber_
Definition: planning_scene_monitor.h:540
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:1365
planning_scene_monitor::LOGNAME
static const std::string LOGNAME
Definition: planning_scene_monitor.cpp:91
planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectsFromOctree
void excludeWorldObjectsFromOctree()
Definition: planning_scene_monitor.cpp:829
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:503
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:614
planning_scene_monitor::PlanningSceneMonitor::new_scene_update_
SceneUpdateType new_scene_update_
Definition: planning_scene_monitor.h:532
planning_scene_monitor::PlanningSceneMonitor::tf_buffer_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: planning_scene_monitor.h:510
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:612
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:517
planning_scene_monitor::PlanningSceneMonitor::initialize
void initialize(const planning_scene::PlanningScenePtr &scene)
Initialize the planning scene monitor.
Definition: planning_scene_monitor.cpp:216
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:531
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:547
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:502
planning_scene_monitor::PlanningSceneMonitor::new_scene_update_condition_
boost::condition_variable_any new_scene_update_condition_
Definition: planning_scene_monitor.h:533
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:999
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:1006
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:506
planning_scene_monitor::PlanningSceneMonitor::current_state_monitor_
CurrentStateMonitorPtr current_state_monitor_
Definition: planning_scene_monitor.h:550
ros::WallTime::now
static WallTime now()
planning_scene_monitor::PlanningSceneMonitor::checkFrameIgnored
bool checkFrameIgnored(const std::string &frame)
Definition: planning_scene_monitor.cpp:1480
planning_scene_monitor::PlanningSceneMonitor::stopPublishingPlanningScene
void stopPublishingPlanningScene()
Stop publishing the maintained planning scene.
Definition: planning_scene_monitor.cpp:349
planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodyFromOctree
void excludeAttachedBodyFromOctree(const moveit::core::AttachedBody *attached_body)
Definition: planning_scene_monitor.cpp:838
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:619
planning_scene_monitor::PlanningSceneMonitor::queue_
ros::CallbackQueue queue_
Definition: planning_scene_monitor.h:507
planning_scene_monitor::PlanningSceneMonitor::stopSceneMonitor
void stopSceneMonitor()
Stop the scene monitor.
Definition: planning_scene_monitor.cpp:1041
planning_scene_monitor::PlanningSceneMonitor::stopWorldGeometryMonitor
void stopWorldGeometryMonitor()
Stop the world geometry monitor.
Definition: planning_scene_monitor.cpp:1150
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:1166
planning_scene_monitor::PlanningSceneMonitor::excludeRobotLinksFromOctree
void excludeRobotLinksFromOctree()
Definition: planning_scene_monitor.cpp:734
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:556
planning_scene_monitor::PlanningSceneMonitor::publish_planning_scene_
std::unique_ptr< boost::thread > publish_planning_scene_
Definition: planning_scene_monitor.h:529
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:598
planning_scene_monitor::PlanningSceneMonitor::get_scene_service_
ros::ServiceServer get_scene_service_
Definition: planning_scene_monitor.h:544
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:1020
planning_scene_monitor::PlanningSceneMonitor::robot_description_
std::string robot_description_
Definition: planning_scene_monitor.h:512
planning_scene_monitor::PlanningSceneMonitor::default_object_padd_
double default_object_padd_
default object padding
Definition: planning_scene_monitor.h:519
planning_scene_monitor::PlanningSceneMonitor::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: planning_scene_monitor.h:615
planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodyInOctree
void includeAttachedBodyInOctree(const moveit::core::AttachedBody *attached_body)
Definition: planning_scene_monitor.cpp:860
planning_scene_monitor::PlanningSceneMonitor::stateUpdateTimerCallback
void stateUpdateTimerCallback(const ros::WallTimerEvent &event)
Definition: planning_scene_monitor.cpp:1219
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:1329
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:525
planning_scene_monitor::PlanningSceneMonitor::monitor_name_
std::string monitor_name_
The name of this scene monitor.
Definition: planning_scene_monitor.h:496
collision_detection::OccMapTree::ReadLock
boost::shared_lock< boost::shared_mutex > ReadLock
planning_scene_monitor::PlanningSceneMonitor::publish_planning_scene_frequency_
double publish_planning_scene_frequency_
Definition: planning_scene_monitor.h:530
XmlRpc::XmlRpcValue::getType
const Type & getType() const
ROS_WARN_THROTTLE_NAMED
#define ROS_WARN_THROTTLE_NAMED(period, name,...)
planning_scene_monitor::PlanningSceneMonitor::state_update_mutex_
boost::mutex state_update_mutex_
Definition: planning_scene_monitor.h:593
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:563
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:1105
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:498
planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneWorldCallback
void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world)
Callback for a new planning scene world.
Definition: planning_scene_monitor.cpp:681
planning_scene_monitor::PlanningSceneMonitor::nh_
ros::NodeHandle nh_
Last time the robot has moved.
Definition: planning_scene_monitor.h:505
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:917
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:770
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:948
DurationBase< WallDuration >::toNSec
int64_t toNSec() const
planning_scene_monitor::PlanningSceneMonitor::attachObjectCallback
void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr &obj)
Callback for a new attached object msg.
Definition: planning_scene_monitor.cpp:720
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:1027
std
planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneCallback
void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene)
Definition: planning_scene_monitor.cpp:571
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:536
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:813
moveit::tools::Profiler::ScopedBlock
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
planning_scene_monitor::PlanningSceneMonitor::ignored_frames_
std::set< std::string > ignored_frames_
Definition: planning_scene_monitor.h:622
planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodiesFromOctree
void excludeAttachedBodiesFromOctree()
Definition: planning_scene_monitor.cpp:801
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:1389
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:602
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:549
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:465
planning_scene_monitor::PlanningSceneMonitor::getUpdatedFrameTransforms
void getUpdatedFrameTransforms(std::vector< geometry_msgs::TransformStamped > &transforms)
Definition: planning_scene_monitor.cpp:1336
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:1323
planning_scene_monitor::PlanningSceneMonitor::clearOctomap
void clearOctomap()
Definition: planning_scene_monitor.cpp:576
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:500
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:516
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:363
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:300
planning_scene_monitor::PlanningSceneMonitor::update_callbacks_
std::vector< boost::function< void(SceneUpdateType)> > update_callbacks_
Definition: planning_scene_monitor.h:567
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:528
t
geometry_msgs::TransformStamped t
planning_scene_monitor::PlanningSceneMonitor::link_shape_handles_
LinkShapeHandles link_shape_handles_
Definition: planning_scene_monitor.h:560
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:900
XmlRpc::XmlRpcValue
planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodiesInOctree
void includeAttachedBodiesInOctree()
Definition: planning_scene_monitor.cpp:785
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:501
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:151
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:607
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:1013


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sat Jan 18 2025 03:36:46