planning_scene_monitor.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
40 #include <moveit_msgs/GetPlanningScene.h>
41 
42 #include <dynamic_reconfigure/server.h>
43 #include <moveit_ros_planning/PlanningSceneMonitorDynamicReconfigureConfig.h>
44 #include <tf2/exceptions.h>
46 #include <tf2_eigen/tf2_eigen.h>
49 
50 #include <memory>
51 
52 namespace planning_scene_monitor
53 {
54 using namespace moveit_ros_planning;
55 
56 static const std::string LOGNAME = "planning_scene_monitor";
57 
59 {
60 public:
62  : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle(decideNamespace(owner->getName())))
63  {
64  dynamic_reconfigure_server_.setCallback(
65  boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
66  }
67 
68 private:
69  // make sure we do not advertise the same service multiple times, in case we use multiple PlanningSceneMonitor
70  // instances in a process
71  static std::string decideNamespace(const std::string& name)
72  {
73  std::string ns = "~/" + name;
74  std::replace(ns.begin(), ns.end(), ' ', '_');
75  std::transform(ns.begin(), ns.end(), ns.begin(), ::tolower);
76  if (ros::service::exists(ns + "/set_parameters", false))
77  {
78  unsigned int c = 1;
79  while (ros::service::exists(ns + boost::lexical_cast<std::string>(c) + "/set_parameters", false))
80  c++;
81  ns += boost::lexical_cast<std::string>(c);
82  }
83  return ns;
84  }
85 
86  void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig& config, uint32_t level)
87  {
89  if (config.publish_geometry_updates)
91  if (config.publish_state_updates)
93  if (config.publish_transforms_updates)
95  if (config.publish_planning_scene)
96  {
97  owner_->setPlanningScenePublishingFrequency(config.publish_planning_scene_hz);
98  owner_->startPublishingPlanningScene(event);
99  }
100  else
101  owner_->stopPublishingPlanningScene();
102  }
103 
105  dynamic_reconfigure::Server<PlanningSceneMonitorDynamicReconfigureConfig> dynamic_reconfigure_server_;
106 };
107 
108 const std::string PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC = "joint_states";
109 const std::string PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC = "attached_collision_object";
110 const std::string PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC = "collision_object";
111 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC = "planning_scene_world";
112 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC = "planning_scene";
113 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE = "get_planning_scene";
114 const std::string PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC = "monitored_planning_scene";
115 
116 PlanningSceneMonitor::PlanningSceneMonitor(const std::string& robot_description,
117  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
118  : PlanningSceneMonitor(planning_scene::PlanningScenePtr(), robot_description, tf_buffer, name)
119 {
120 }
121 
122 PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
123  const std::string& robot_description,
124  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
125  : PlanningSceneMonitor(scene, std::make_shared<robot_model_loader::RobotModelLoader>(robot_description), tf_buffer,
126  name)
127 {
128 }
129 
130 PlanningSceneMonitor::PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr& rm_loader,
131  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
132  : PlanningSceneMonitor(planning_scene::PlanningScenePtr(), rm_loader, tf_buffer, name)
133 {
134 }
135 
136 PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
137  const robot_model_loader::RobotModelLoaderPtr& rm_loader,
138  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const std::string& name)
139  : monitor_name_(name), nh_("~"), tf_buffer_(tf_buffer), rm_loader_(rm_loader)
140 {
143  spinner_.reset(new ros::AsyncSpinner(1, &queue_));
144  spinner_->start();
145  initialize(scene);
146 }
147 
148 PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
149  const robot_model_loader::RobotModelLoaderPtr& rm_loader,
150  const ros::NodeHandle& nh, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
151  const std::string& name)
152  : monitor_name_(name), nh_("~"), root_nh_(nh), tf_buffer_(tf_buffer), rm_loader_(rm_loader)
153 {
154  // use same callback queue as root_nh_
156  initialize(scene);
157 }
158 
160 {
161  if (scene_)
162  {
163  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
164  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
165  }
170 
171  spinner_.reset();
172  delete reconfigure_impl_;
173  current_state_monitor_.reset();
174  scene_const_.reset();
175  scene_.reset();
176  parent_scene_.reset();
177  robot_model_.reset();
178  rm_loader_.reset();
179 }
180 
181 void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& scene)
182 {
184  moveit::tools::Profiler::ScopedBlock prof_block("PlanningSceneMonitor::initialize");
185 
186  if (monitor_name_.empty())
187  monitor_name_ = "planning_scene_monitor";
188  robot_description_ = rm_loader_->getRobotDescription();
189  if (rm_loader_->getModel())
190  {
191  robot_model_ = rm_loader_->getModel();
192  scene_ = scene;
193  if (!scene_)
194  {
195  try
196  {
197  scene_.reset(new planning_scene::PlanningScene(rm_loader_->getModel()));
200 
201  scene_->getCollisionRobotNonConst()->setPadding(default_robot_padd_);
202  scene_->getCollisionRobotNonConst()->setScale(default_robot_scale_);
203  for (std::map<std::string, double>::iterator it = default_robot_link_padd_.begin();
204  it != default_robot_link_padd_.end(); ++it)
205  {
206  scene_->getCollisionRobotNonConst()->setLinkPadding(it->first, it->second);
207  }
208  for (std::map<std::string, double>::iterator it = default_robot_link_scale_.begin();
209  it != default_robot_link_scale_.end(); ++it)
210  {
211  scene_->getCollisionRobotNonConst()->setLinkScale(it->first, it->second);
212  }
213  scene_->propogateRobotPadding();
214  }
215  catch (moveit::ConstructException& e)
216  {
217  ROS_ERROR_NAMED(LOGNAME, "Configuration of planning scene failed");
218  scene_.reset();
219  }
220  }
221  // scene_const_ is set regardless if scene_ is null or not
223  if (scene_)
224  {
225  // The scene_ is loaded on the collision loader only if it was correctly instantiated
227  scene_->setAttachedBodyUpdateCallback(
229  scene_->setCollisionObjectUpdateCallback(
230  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
231  }
232  }
233  else
234  {
235  ROS_ERROR_NAMED(LOGNAME, "Robot model not loaded");
236  }
237 
240 
244 
245  double temp_wait_time = 0.05;
246 
247  if (!robot_description_.empty())
248  nh_.param(robot_description_ + "_planning/shape_transform_cache_lookup_wait_time", temp_wait_time, temp_wait_time);
249 
251 
252  state_update_pending_ = false;
254  false, // not a oneshot timer
255  false); // do not start the timer yet
256 
258 }
259 
261 {
262  if (scene_)
263  {
264  if (flag)
265  {
266  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
267  if (scene_)
268  {
269  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
270  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
271  scene_->decoupleParent();
273  scene_ = parent_scene_->diff();
275  scene_->setAttachedBodyUpdateCallback(
277  scene_->setCollisionObjectUpdateCallback(
278  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
279  }
280  }
281  else
282  {
284  {
285  ROS_WARN_NAMED(LOGNAME, "Diff monitoring was stopped while publishing planning scene diffs. "
286  "Stopping planning scene diff publisher");
288  }
289  {
290  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
291  if (scene_)
292  {
293  scene_->decoupleParent();
294  parent_scene_.reset();
295  // remove the '+' added by .diff() at the end of the scene name
296  if (!scene_->getName().empty())
297  {
298  if (scene_->getName()[scene_->getName().length() - 1] == '+')
299  scene_->setName(scene_->getName().substr(0, scene_->getName().length() - 1));
300  }
301  }
302  }
303  }
304  }
305 }
306 
308 {
310  {
311  std::unique_ptr<boost::thread> copy;
312  copy.swap(publish_planning_scene_);
313  new_scene_update_condition_.notify_all();
314  copy->join();
315  monitorDiffs(false);
317  ROS_INFO_NAMED(LOGNAME, "Stopped publishing maintained planning scene.");
318  }
319 }
320 
322  const std::string& planning_scene_topic)
323 {
324  publish_update_types_ = update_type;
326  {
327  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>(planning_scene_topic, 100, false);
328  ROS_INFO_NAMED(LOGNAME, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
329  monitorDiffs(true);
330  publish_planning_scene_.reset(new 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  {
363  if ((publish_update_types_ & new_scene_update_) || new_scene_update_ == UPDATE_SCENE)
364  {
365  if (new_scene_update_ == UPDATE_SCENE)
366  is_full = true;
367  else
368  {
370  if (octomap_monitor_)
371  lock = octomap_monitor_->getOcTreePtr()->reading();
372  scene_->getPlanningSceneDiffMsg(msg);
373  if (new_scene_update_ == UPDATE_STATE)
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(robot_state::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  }
408  new_scene_update_ = UPDATE_NONE;
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 
462 {
463  // do not modify update functions while we are calling them
464  boost::recursive_mutex::scoped_lock lock(update_lock_);
465 
466  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
467  update_callbacks_[i](update_type);
468  new_scene_update_ = (SceneUpdateType)((int)new_scene_update_ | (int)update_type);
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  moveit_msgs::GetPlanningScene srv;
482  srv.request.components.components =
483  srv.request.components.SCENE_SETTINGS | srv.request.components.ROBOT_STATE |
484  srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS | srv.request.components.WORLD_OBJECT_NAMES |
485  srv.request.components.WORLD_OBJECT_GEOMETRY | srv.request.components.OCTOMAP |
486  srv.request.components.TRANSFORMS | srv.request.components.ALLOWED_COLLISION_MATRIX |
487  srv.request.components.LINK_PADDING_AND_SCALING | srv.request.components.OBJECT_COLORS;
488 
489  // Make sure client is connected to server
490  if (!client.exists())
491  {
492  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for service `" << service_name << "` to exist.");
493  client.waitForExistence(ros::Duration(5.0));
494  }
495 
496  if (client.call(srv))
497  {
498  newPlanningSceneMessage(srv.response.scene);
499  }
500  else
501  {
503  LOGNAME, "Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?",
504  service_name.c_str());
505  return false;
506  }
507  return true;
508 }
509 
510 void PlanningSceneMonitor::providePlanningSceneService(const std::string& service_name)
511 {
512  // Load the service
515 }
516 
517 bool PlanningSceneMonitor::getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req,
518  moveit_msgs::GetPlanningScene::Response& res)
519 {
520  if (req.components.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
522 
523  moveit_msgs::PlanningSceneComponents all_components;
524  all_components.components = UINT_MAX; // Return all scene components if nothing is specified.
525 
526  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
527  scene_->getPlanningSceneMsg(res.scene, req.components.components ? req.components : all_components);
528 
529  return true;
530 }
531 
532 void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene)
533 {
534  newPlanningSceneMessage(*scene);
535 }
536 
538 {
539  bool removed = false;
540  {
541  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
542  removed = scene_->getWorldNonConst()->removeObject(scene_->OCTOMAP_NS);
543 
544  if (octomap_monitor_)
545  {
546  octomap_monitor_->getOcTreePtr()->lockWrite();
547  octomap_monitor_->getOcTreePtr()->clear();
548  octomap_monitor_->getOcTreePtr()->unlockWrite();
549  }
550  else
551  {
552  ROS_WARN_NAMED(LOGNAME, "Unable to clear octomap since no octomap monitor has been initialized");
553  } // Lift the scoped lock before calling triggerSceneUpdateEvent to avoid deadlock
554  }
555 
556  if (removed)
558 }
559 
560 bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
561 {
562  if (!scene_)
563  return false;
564 
565  bool result;
566 
568  std::string old_scene_name;
569  {
570  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
571  // we don't want the transform cache to update while we are potentially changing attached bodies
572  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
573 
575  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
576  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
577  "scene update " << fmod(last_update_time_.toSec(), 10.)
578  << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
579  old_scene_name = scene_->getName();
580  result = scene_->usePlanningSceneMsg(scene);
581  if (octomap_monitor_)
582  {
583  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
584  {
585  octomap_monitor_->getOcTreePtr()->lockWrite();
586  octomap_monitor_->getOcTreePtr()->clear();
587  octomap_monitor_->getOcTreePtr()->unlockWrite();
588  }
589  }
590  robot_model_ = scene_->getRobotModel();
591 
592  // if we just reset the scene completely but we were maintaining diffs, we need to fix that
593  if (!scene.is_diff && parent_scene_)
594  {
595  // the scene is now decoupled from the parent, since we just reset it
596  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
597  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
599  scene_ = parent_scene_->diff();
601  scene_->setAttachedBodyUpdateCallback(
603  scene_->setCollisionObjectUpdateCallback(
604  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
605  }
606  if (octomap_monitor_)
607  {
608  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
609  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
610  }
611  }
612 
613  // if we have a diff, try to more accuratelly determine the update type
614  if (scene.is_diff)
615  {
616  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
617  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
618  scene.link_scale.empty();
619  if (no_other_scene_upd)
620  {
621  upd = UPDATE_NONE;
622  if (!planning_scene::PlanningScene::isEmpty(scene.world))
623  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
624 
625  if (!scene.fixed_frame_transforms.empty())
626  upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
627 
628  if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
629  {
630  upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
631  if (!scene.robot_state.attached_collision_objects.empty() || !static_cast<bool>(scene.robot_state.is_diff))
632  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
633  }
634  }
635  }
637  return result;
638 }
639 
640 void PlanningSceneMonitor::newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world)
641 {
642  if (scene_)
643  {
645  {
646  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
648  scene_->getWorldNonConst()->clearObjects();
649  scene_->processPlanningSceneWorldMsg(*world);
650  if (octomap_monitor_)
651  {
652  if (world->octomap.octomap.data.empty())
653  {
654  octomap_monitor_->getOcTreePtr()->lockWrite();
655  octomap_monitor_->getOcTreePtr()->clear();
656  octomap_monitor_->getOcTreePtr()->unlockWrite();
657  }
658  }
659  }
661  }
662 }
663 
664 void PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj)
665 {
666  if (!scene_)
667  return;
668 
670  {
671  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
673  if (!scene_->processCollisionObjectMsg(*obj))
674  return;
675  }
677 }
678 
679 void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
680 {
681  if (scene_)
682  {
684  {
685  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
687  scene_->processAttachedCollisionObjectMsg(*obj);
688  }
690  }
691 }
692 
694 {
695  if (!octomap_monitor_)
696  return;
697 
698  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
699 
701  const std::vector<const robot_model::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
703  bool warned = false;
704  for (std::size_t i = 0; i < links.size(); ++i)
705  {
706  std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes(); // copy shared ptrs on purpuse
707  for (std::size_t j = 0; j < shapes.size(); ++j)
708  {
709  // merge mesh vertices up to 0.1 mm apart
710  if (shapes[j]->type == shapes::MESH)
711  {
712  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
713  m->mergeVertices(1e-4);
714  shapes[j].reset(m);
715  }
716 
717  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
718  if (h)
719  link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
720  }
721  if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
722  {
723  ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
724  warned = true;
725  }
726  }
727 }
728 
730 {
731  if (!octomap_monitor_)
732  return;
733 
734  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
735 
736  for (LinkShapeHandles::iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
737  for (std::size_t i = 0; i < it->second.size(); ++i)
738  octomap_monitor_->forgetShape(it->second[i].first);
739  link_shape_handles_.clear();
740 }
741 
743 {
744  if (!octomap_monitor_)
745  return;
746 
747  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
748 
749  // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
750  for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin();
751  it != attached_body_shape_handles_.end(); ++it)
752  for (std::size_t k = 0; k < it->second.size(); ++k)
753  octomap_monitor_->forgetShape(it->second[k].first);
755 }
756 
758 {
759  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
760 
762  // add attached objects again
763  std::vector<const robot_state::AttachedBody*> ab;
764  scene_->getCurrentState().getAttachedBodies(ab);
765  for (std::size_t i = 0; i < ab.size(); ++i)
767 }
768 
770 {
771  if (!octomap_monitor_)
772  return;
773 
774  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
775 
776  // clear information about any attached object
777  for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin();
778  it != collision_body_shape_handles_.end(); ++it)
779  for (std::size_t k = 0; k < it->second.size(); ++k)
780  octomap_monitor_->forgetShape(it->second[k].first);
782 }
783 
785 {
786  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
787 
789  for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end();
790  ++it)
791  excludeWorldObjectFromOctree(it->second);
792 }
793 
794 void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body)
795 {
796  if (!octomap_monitor_)
797  return;
798 
799  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
800  bool found = false;
801  for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
802  {
803  if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
804  continue;
805  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
806  if (h)
807  {
808  found = true;
809  attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
810  }
811  }
812  if (found)
813  ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
814 }
815 
816 void PlanningSceneMonitor::includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body)
817 {
818  if (!octomap_monitor_)
819  return;
820 
821  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
822 
823  AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
824  if (it != attached_body_shape_handles_.end())
825  {
826  for (std::size_t k = 0; k < it->second.size(); ++k)
827  octomap_monitor_->forgetShape(it->second[k].first);
828  ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
830  }
831 }
832 
833 void PlanningSceneMonitor::excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj)
834 {
835  if (!octomap_monitor_)
836  return;
837 
838  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
839 
840  bool found = false;
841  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
842  {
843  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
844  continue;
845  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
846  if (h)
847  {
848  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
849  found = true;
850  }
851  }
852  if (found)
853  ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
854 }
855 
856 void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj)
857 {
858  if (!octomap_monitor_)
859  return;
860 
861  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
862 
863  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
864  if (it != collision_body_shape_handles_.end())
865  {
866  for (std::size_t k = 0; k < it->second.size(); ++k)
867  octomap_monitor_->forgetShape(it->second[k].first);
868  ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
870  }
871 }
872 
873 void PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body,
874  bool just_attached)
875 {
876  if (!octomap_monitor_)
877  return;
878 
879  if (just_attached)
880  excludeAttachedBodyFromOctree(attached_body);
881  else
882  includeAttachedBodyInOctree(attached_body);
883 }
884 
885 void PlanningSceneMonitor::currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& obj,
887 {
888  if (!octomap_monitor_)
889  return;
891  return;
892 
895  else if (action & collision_detection::World::DESTROY)
897  else
898  {
901  }
902 }
903 
905 {
906  if (t.isZero())
907  return false;
909  ros::WallDuration timeout(wait_time);
910 
911  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
912 
914  {
915  // Wait for next robot update in state monitor.
916  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
917 
918  /* As robot updates are passed to the planning scene only in throttled fashion, there might
919  be still an update pending. If so, explicitly update the planning scene here.
920  If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
921  if (success)
922  {
923  boost::mutex::scoped_lock lock(state_pending_mutex_);
924  if (state_update_pending_) // enforce state update
925  {
926  state_update_pending_ = false;
928  lock.unlock();
930  }
931  return true;
932  }
933 
934  ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
935  return false;
936  }
937 
938  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
939  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
940  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
941  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
942  ros::Time prev_robot_motion_time = last_robot_motion_time_;
943  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
944  timeout > ros::WallDuration())
945  {
946  ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
947  new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
948  timeout -= ros::WallTime::now() - start; // compute remaining wait_time
949  }
950  bool success = last_robot_motion_time_ >= t;
951  // suppress warning if we received an update at all
952  if (!success && prev_robot_motion_time != last_robot_motion_time_)
953  ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
954  (t - last_robot_motion_time_).toSec());
955 
956  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
957  << " scene update: " << (t - last_update_time_).toSec());
958  return success;
959 }
960 
962 {
963  scene_update_mutex_.lock_shared();
964  if (octomap_monitor_)
965  octomap_monitor_->getOcTreePtr()->lockRead();
966 }
967 
969 {
970  if (octomap_monitor_)
971  octomap_monitor_->getOcTreePtr()->unlockRead();
972  scene_update_mutex_.unlock_shared();
973 }
974 
976 {
977  scene_update_mutex_.lock();
978  if (octomap_monitor_)
979  octomap_monitor_->getOcTreePtr()->lockWrite();
980 }
981 
983 {
984  if (octomap_monitor_)
985  octomap_monitor_->getOcTreePtr()->unlockWrite();
986  scene_update_mutex_.unlock();
987 }
988 
989 void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
990 {
992 
993  ROS_INFO_NAMED(LOGNAME, "Starting planning scene monitor");
994  // listen for planning scene updates; these messages include transforms, so no need for filters
995  if (!scene_topic.empty())
996  {
999  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
1000  }
1001 }
1002 
1004 {
1006  {
1007  ROS_INFO_NAMED(LOGNAME, "Stopping planning scene monitor");
1009  }
1010 }
1011 
1012 bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
1014 {
1015  if (!tf_buffer_)
1016  return false;
1017  try
1018  {
1019  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
1020 
1021  for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
1022  {
1023  tf_buffer_->canTransform(target_frame, it->first->getName(), target_time, shape_transform_cache_lookup_wait_time_);
1024  Eigen::Isometry3d ttr =
1025  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, it->first->getName(), target_time));
1026  for (std::size_t j = 0; j < it->second.size(); ++j)
1027  cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
1028  }
1029  for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin();
1030  it != attached_body_shape_handles_.end(); ++it)
1031  {
1032  tf_buffer_->canTransform(target_frame, it->first->getAttachedLinkName(), target_time,
1034  Eigen::Isometry3d transform = tf2::transformToEigen(
1035  tf_buffer_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time));
1036  for (std::size_t k = 0; k < it->second.size(); ++k)
1037  cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
1038  }
1039  {
1040  tf_buffer_->canTransform(target_frame, scene_->getPlanningFrame(), target_time,
1042  Eigen::Isometry3d transform =
1043  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time));
1044  for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin();
1045  it != collision_body_shape_handles_.end(); ++it)
1046  for (std::size_t k = 0; k < it->second.size(); ++k)
1047  cache[it->second[k].first] = transform * (*it->second[k].second);
1048  }
1049  }
1050  catch (tf2::TransformException& ex)
1051  {
1052  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
1053  return false;
1054  }
1055  return true;
1056 }
1057 
1058 void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collision_objects_topic,
1059  const std::string& planning_scene_world_topic,
1060  const bool load_octomap_monitor)
1061 {
1063  ROS_INFO_NAMED(LOGNAME, "Starting world geometry update monitor for collision objects, attached objects, octomap "
1064  "updates.");
1065 
1066  // Listen to the /collision_objects topic to detect requests to add/remove/update collision objects to/from the world
1067  if (!collision_objects_topic.empty())
1068  {
1070  root_nh_.subscribe(collision_objects_topic, 1024, &PlanningSceneMonitor::collisionObjectCallback, this);
1071  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
1072  }
1073 
1074  if (!planning_scene_world_topic.empty())
1075  {
1077  root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
1078  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
1079  root_nh_.resolveName(planning_scene_world_topic).c_str());
1080  }
1081 
1082  // Ocotomap monitor is optional
1083  if (load_octomap_monitor)
1084  {
1085  if (!octomap_monitor_)
1086  {
1091 
1092  octomap_monitor_->setTransformCacheCallback(
1093  boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
1094  octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
1095  }
1096  octomap_monitor_->startMonitor();
1097  }
1098 }
1099 
1101 {
1103  {
1104  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1106  }
1108  {
1109  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1111  }
1112  if (octomap_monitor_)
1113  octomap_monitor_->stopMonitor();
1114 }
1115 
1116 void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1117  const std::string& attached_objects_topic)
1118 {
1119  stopStateMonitor();
1120  if (scene_)
1121  {
1124  current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
1125  current_state_monitor_->startStateMonitor(joint_states_topic);
1126 
1127  {
1128  boost::mutex::scoped_lock lock(state_pending_mutex_);
1129  if (!dt_state_update_.isZero())
1131  }
1132 
1133  if (!attached_objects_topic.empty())
1134  {
1135  // using regular message filter as there's no header
1137  root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
1138  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
1139  root_nh_.resolveName(attached_objects_topic).c_str());
1140  }
1141  }
1142  else
1143  ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
1144 }
1145 
1147 {
1149  current_state_monitor_->stopStateMonitor();
1152 
1153  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1155  {
1156  boost::mutex::scoped_lock lock(state_pending_mutex_);
1157  state_update_pending_ = false;
1158  }
1159 }
1160 
1161 void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr& /* joint_state */)
1162 {
1163  const ros::WallTime& n = ros::WallTime::now();
1165 
1166  bool update = false;
1167  {
1168  boost::mutex::scoped_lock lock(state_pending_mutex_);
1169 
1170  if (dt < dt_state_update_)
1171  {
1172  state_update_pending_ = true;
1173  }
1174  else
1175  {
1176  state_update_pending_ = false;
1177  last_robot_state_update_wall_time_ = n;
1178  update = true;
1179  }
1180  }
1181  // run the state update with state_pending_mutex_ unlocked
1182  if (update)
1184 }
1185 
1187 {
1189  {
1190  bool update = false;
1191 
1192  const ros::WallTime& n = ros::WallTime::now();
1194 
1195  {
1196  // lock for access to dt_state_update_ and state_update_pending_
1197  boost::mutex::scoped_lock lock(state_pending_mutex_);
1199  {
1200  state_update_pending_ = false;
1201  last_robot_state_update_wall_time_ = ros::WallTime::now();
1202  update = true;
1203  ROS_DEBUG_STREAM_NAMED(LOGNAME,
1204  "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
1205  }
1206  }
1207 
1208  // run the state update with state_pending_mutex_ unlocked
1209  if (update)
1210  {
1212  ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
1213  }
1214  }
1215 }
1216 
1218 {
1219  if (!octomap_monitor_)
1220  return;
1221 
1223  {
1224  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1226  octomap_monitor_->getOcTreePtr()->lockRead();
1227  try
1228  {
1229  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Isometry3d::Identity());
1230  octomap_monitor_->getOcTreePtr()->unlockRead();
1231  }
1232  catch (...)
1233  {
1234  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1235  throw;
1236  }
1237  }
1239 }
1240 
1242 {
1243  bool update = false;
1244  if (hz > std::numeric_limits<double>::epsilon())
1245  {
1246  boost::mutex::scoped_lock lock(state_pending_mutex_);
1247  dt_state_update_.fromSec(1.0 / hz);
1250  }
1251  else
1252  {
1253  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1255  boost::mutex::scoped_lock lock(state_pending_mutex_);
1258  update = true;
1259  }
1260  ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
1261 
1262  if (update)
1264 }
1265 
1267 {
1269  {
1270  std::vector<std::string> missing;
1271  if (!current_state_monitor_->haveCompleteState(missing) &&
1272  (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
1273  {
1274  std::string missing_str = boost::algorithm::join(missing, ", ");
1275  ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known. Missing %s",
1276  missing_str.c_str());
1277  }
1278 
1279  {
1280  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1282  ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
1283  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1284  scene_->getCurrentStateNonConst().update(); // compute all transforms
1285  }
1287  }
1288  else
1289  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
1290 }
1291 
1292 void PlanningSceneMonitor::addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn)
1293 {
1294  boost::recursive_mutex::scoped_lock lock(update_lock_);
1295  if (fn)
1296  update_callbacks_.push_back(fn);
1297 }
1298 
1300 {
1301  boost::recursive_mutex::scoped_lock lock(update_lock_);
1302  update_callbacks_.clear();
1303 }
1304 
1306 {
1308  ROS_DEBUG_NAMED(LOGNAME, "Maximum frquency for publishing a planning scene is now %lf Hz",
1310 }
1311 
1312 void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms)
1313 {
1314  const std::string& target = getRobotModel()->getModelFrame();
1315 
1316  std::vector<std::string> all_frame_names;
1317  tf_buffer_->_getFrameStrings(all_frame_names);
1318  for (std::size_t i = 0; i < all_frame_names.size(); ++i)
1319  {
1320  if (all_frame_names[i] == target || getRobotModel()->hasLinkModel(all_frame_names[i]))
1321  continue;
1322 
1323  geometry_msgs::TransformStamped f;
1324  try
1325  {
1326  f = tf_buffer_->lookupTransform(target, all_frame_names[i], ros::Time(0));
1327  }
1328  catch (tf2::TransformException& ex)
1329  {
1330  ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
1331  << all_frame_names[i] << "' to planning frame '" << target << "' ("
1332  << ex.what() << ")");
1333  continue;
1334  }
1335  f.header.frame_id = all_frame_names[i];
1336  f.child_frame_id = target;
1337  transforms.push_back(f);
1338  }
1339 }
1340 
1342 {
1343  if (!tf_buffer_)
1344  return;
1345 
1346  if (scene_)
1347  {
1348  std::vector<geometry_msgs::TransformStamped> transforms;
1349  getUpdatedFrameTransforms(transforms);
1350  {
1351  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1352  scene_->getTransformsNonConst().setTransforms(transforms);
1354  }
1356  }
1357 }
1358 
1360 {
1361  if (octomap_monitor_)
1362  octomap_monitor_->publishDebugInformation(flag);
1363 }
1364 
1365 void PlanningSceneMonitor::configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene)
1366 {
1367  if (!scene || robot_description_.empty())
1368  return;
1369  collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
1370 
1371  // read overriding values from the param server
1372 
1373  // first we do default collision operations
1374  if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
1375  ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
1376  else
1377  {
1378  ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
1379 
1380  XmlRpc::XmlRpcValue coll_ops;
1381  nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
1382 
1383  if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
1384  {
1385  ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
1386  return;
1387  }
1388 
1389  if (coll_ops.size() == 0)
1390  {
1391  ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
1392  return;
1393  }
1394 
1395  for (int i = 0; i < coll_ops.size(); ++i)
1396  {
1397  if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
1398  {
1399  ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
1400  continue;
1401  }
1402  acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
1403  std::string(coll_ops[i]["operation"]) == "disable");
1404  }
1405  }
1406 }
1407 
1409 {
1410  if (robot_description_.empty())
1411  {
1412  default_robot_padd_ = 0.0;
1413  default_robot_scale_ = 1.0;
1414  default_object_padd_ = 0.0;
1415  default_attached_padd_ = 0.0;
1416  return;
1417  }
1418 
1419  // Ensure no leading slash creates a bad param server address
1420  const std::string robot_description =
1421  (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
1422 
1423  nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
1424  nh_.param(robot_description + "_planning/default_robot_scale", default_robot_scale_, 1.0);
1425  nh_.param(robot_description + "_planning/default_object_padding", default_object_padd_, 0.0);
1426  nh_.param(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
1427  nh_.param(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_,
1428  std::map<std::string, double>());
1429  nh_.param(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_,
1430  std::map<std::string, double>());
1431 
1432  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
1433  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
1434 }
1435 } // namespace planning_scene_monitor
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor (ROS topic-based)
boost::recursive_mutex update_lock_
lock access to update_callbacks_
dynamic_reconfigure::Server< PlanningSceneMonitorDynamicReconfigureConfig > dynamic_reconfigure_server_
void clearUpdateCallbacks()
Clear the functions to be called when an update to the scene is received.
std::string getTopic() const
#define ROS_INFO_NAMED(name,...)
void mergeVertices(double threshold)
void getUpdatedFrameTransforms(std::vector< geometry_msgs::TransformStamped > &transforms)
#define ROS_DEBUG_STREAM_NAMED(name, args)
ROSCPP_DECL void start()
#define ROS_WARN_THROTTLE_NAMED(period, name,...)
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
The geometry of the scene was updated. This includes receiving new octomaps, collision objects...
#define ROS_WARN_NAMED(name,...)
static const std::string LOGNAME
void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig &config, uint32_t level)
robot_model_loader::RobotModelLoaderPtr rm_loader_
f
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
void lockSceneWrite()
Lock the scene for writing (only one thread can lock for writing and no other thread can lock for rea...
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
static const std::string DEFAULT_PLANNING_SCENE_SERVICE
The name of the service used by default for requesting full planning scene state. ...
void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state)
void setPeriod(const WallDuration &period, bool reset=true)
ros::Time last_robot_motion_time_
Last time the state was updated.
void addUpdateCallback(const boost::function< void(SceneUpdateType)> &fn)
Add a function to be called when an update to the scene is received.
boost::shared_lock< boost::shared_mutex > ReadLock
Monitors the joint_states topic and tf to maintain the current state of the robot.
bool call(MReq &req, MRes &res)
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.
ros::Duration shape_transform_cache_lookup_wait_time_
the amount of time to wait when looking up transforms
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
void octomapUpdateCallback()
Callback for octomap updates.
const robot_model::RobotModelConstPtr & getRobotModel() const
void configureDefaultPadding()
Configure the default padding.
std::shared_ptr< ros::AsyncSpinner > spinner_
void triggerSceneUpdateEvent(SceneUpdateType update_type)
This function is called every time there is a change to the planning scene.
void stopPublishingPlanningScene()
Stop publishing the maintained planning scene.
void lockSceneRead()
Lock the scene for reading (multiple threads can lock for reading at the same time) ...
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::map< std::string, ObjectPtr >::const_iterator const_iterator
static const std::string OCTOMAP_NS
geometry_msgs::TransformStamped t
bool updatesScene(const planning_scene::PlanningSceneConstPtr &scene) const
Return true if the scene scene can be updated directly or indirectly by this monitor. This function will return true if the pointer of the scene is the same as the one maintained, or if a parent of the scene is the one maintained.
std::map< ShapeHandle, Eigen::Isometry3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Isometry3d > > > ShapeTransformCache
void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr &obj)
Callback for a new collision object msg.
void unlockSceneWrite()
Lock the scene from writing (only one thread can lock for writing and no other thread can lock for re...
The state in the monitored scene was updated.
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
void setStateUpdateFrequency(double hz)
Update the scene using the monitored state at a specified frequency, in Hz. This function has an effe...
bool getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request &req, moveit_msgs::GetPlanningScene::Response &res)
void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody *attached_body, bool just_attached)
Callback for a change for an attached object of the current state of the planning scene...
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
void excludeAttachedBodyFromOctree(const robot_state::AttachedBody *attached_body)
#define ROS_DEBUG_NAMED(name,...)
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
void setupScene(ros::NodeHandle &nh, const planning_scene::PlanningScenePtr &scene)
This can be called on a new planning scene to setup the collision detector.
unsigned int ShapeHandle
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Type const & getType() const
void publish(const boost::shared_ptr< M > &message) const
std::unique_ptr< boost::thread > publish_planning_scene_
void updateFrameTransforms()
Update the transforms for the frames that are not part of the kinematic model using tf...
ros::WallTime last_robot_state_update_wall_time_
Last time the state was updated from current_state_monitor_.
#define ROS_ERROR_THROTTLE_NAMED(period, name,...)
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:
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
WallDuration & fromSec(double t)
#define ROS_FATAL_STREAM_NAMED(name, args)
std::unique_ptr< occupancy_map_monitor::OccupancyMapMonitor > octomap_monitor_
bool getParam(const std::string &key, std::string &s) const
PlanningSceneMonitor Subscribes to the topic planning_scene.
std::map< std::string, double > default_robot_link_scale_
default robot link scale
void setCallbackQueue(CallbackQueueInterface *queue)
bool waitForCurrentRobotState(const ros::Time &t, double wait_time=1.)
Wait for robot state to become more recent than time t.
ros::NodeHandle nh_
Last time the robot has moved.
void initialize(const planning_scene::PlanningScenePtr &scene)
Initialize the planning scene monitor.
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...
void updateSceneWithCurrentState()
Update the scene using the monitored state. This function is automatically called when an update to t...
void getMonitoredTopics(std::vector< std::string > &topics) const
Get the topic names that the monitor is listening to.
ros::WallTimer state_update_timer_
timer for state updates.
std::string resolveName(const std::string &name, bool remap=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
CallbackQueueInterface * getCallbackQueue() const
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.
std::string getService() const
void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene)
Configure the collision matrix for a particular scene.
boost::condition_variable_any new_scene_update_condition_
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...
bool hasParam(const std::string &key) const
bool sleep()
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 ...
void unlockSceneRead()
Unlock the scene from reading (multiple threads can lock for reading at the same time) ...
void stateUpdateTimerCallback(const ros::WallTimerEvent &event)
ros::Time last_update_time_
mutex for stored scene
static const std::string DEFAULT_COLLISION_OBJECT_TOPIC
The name of the topic used by default for receiving collision objects in the world.
std::vector< boost::function< void(SceneUpdateType)> > update_callbacks_
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
static WallTime now()
boost::shared_mutex scene_update_mutex_
if diffs are monitored, this is the pointer to the parent scene
void monitorDiffs(bool flag)
By default, the maintained planning scene does not reason about diffs. When the flag passed in is tru...
planning_scene::PlanningScenePtr parent_scene_
void stopWorldGeometryMonitor()
Stop the world geometry monitor.
void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj)
bool requestPlanningSceneState(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
Request a full planning scene state using a service call Be careful not to use this in conjunction wi...
#define ROS_ERROR_NAMED(name,...)
void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr &obj)
The maintained set of fixed transforms in the monitored scene was updated.
static Time now()
double default_attached_padd_
default attached padding
std::string monitor_name_
The name of this scene monitor.
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
bool newPlanningSceneMessage(const moveit_msgs::PlanningScene &scene)
void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr &object, collision_detection::World::Action action)
Callback for a change in the world maintained by the planning scene.
void setPlanningScenePublishingFrequency(double hz)
Set the maximum frequency at which planning scenes are being published.
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
The name of the topic used by default for attached collision objects.
std::map< std::string, double > default_robot_link_padd_
default robot link padding
volatile bool state_update_pending_
True when we need to update the RobotState from current_state_monitor_.
void includeAttachedBodyInOctree(const robot_state::AttachedBody *attached_body)
void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene)
void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr &obj)
Callback for a new attached object msg.
planning_scene::PlanningSceneConstPtr scene_const_
bool hasMember(const std::string &name) const
void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world)
Callback for a new planning scene world.
static const std::string DEFAULT_JOINT_STATES_TOPIC
The name of the topic used by default for receiving joint states.
ros::WallDuration dt_state_update_
the amount of time to wait in between updates to the robot state
collision_detection::CollisionPluginLoader collision_loader_
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
bool getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const
#define ROS_WARN_STREAM_NAMED(name, args)


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Fri Aug 20 2021 03:00:58