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;
195  if (!scene_)
196  {
197  try
198  {
199  scene_.reset(new planning_scene::PlanningScene(rm_loader_->getModel()));
204 
205  scene_->getCollisionRobotNonConst()->setPadding(default_robot_padd_);
206  scene_->getCollisionRobotNonConst()->setScale(default_robot_scale_);
207  for (std::map<std::string, double>::iterator it = default_robot_link_padd_.begin();
208  it != default_robot_link_padd_.end(); ++it)
209  {
210  scene_->getCollisionRobotNonConst()->setLinkPadding(it->first, it->second);
211  }
212  for (std::map<std::string, double>::iterator it = default_robot_link_scale_.begin();
213  it != default_robot_link_scale_.end(); ++it)
214  {
215  scene_->getCollisionRobotNonConst()->setLinkScale(it->first, it->second);
216  }
217  scene_->propogateRobotPadding();
218  }
219  catch (moveit::ConstructException& e)
220  {
221  ROS_ERROR_NAMED(LOGNAME, "Configuration of planning scene failed");
222  scene_.reset();
224  }
225  }
226  if (scene_)
227  {
228  scene_->setAttachedBodyUpdateCallback(
230  scene_->setCollisionObjectUpdateCallback(
231  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
232  }
233  }
234  else
235  {
236  ROS_ERROR_NAMED(LOGNAME, "Robot model not loaded");
237  }
238 
241 
245 
246  double temp_wait_time = 0.05;
247 
248  if (!robot_description_.empty())
249  nh_.param(robot_description_ + "_planning/shape_transform_cache_lookup_wait_time", temp_wait_time, temp_wait_time);
250 
252 
253  state_update_pending_ = false;
255  false, // not a oneshot timer
256  false); // do not start the timer yet
257 
259 }
260 
262 {
263  if (scene_)
264  {
265  if (flag)
266  {
267  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
268  if (scene_)
269  {
270  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
271  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
272  scene_->decoupleParent();
274  scene_ = parent_scene_->diff();
276  scene_->setAttachedBodyUpdateCallback(
278  scene_->setCollisionObjectUpdateCallback(
279  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
280  }
281  }
282  else
283  {
285  {
286  ROS_WARN_NAMED(LOGNAME, "Diff monitoring was stopped while publishing planning scene diffs. "
287  "Stopping planning scene diff publisher");
289  }
290  {
291  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
292  if (scene_)
293  {
294  scene_->decoupleParent();
295  parent_scene_.reset();
296  // remove the '+' added by .diff() at the end of the scene name
297  if (!scene_->getName().empty())
298  {
299  if (scene_->getName()[scene_->getName().length() - 1] == '+')
300  scene_->setName(scene_->getName().substr(0, scene_->getName().length() - 1));
301  }
302  }
303  }
304  }
305  }
306 }
307 
309 {
311  {
312  std::unique_ptr<boost::thread> copy;
313  copy.swap(publish_planning_scene_);
314  new_scene_update_condition_.notify_all();
315  copy->join();
316  monitorDiffs(false);
318  ROS_INFO_NAMED(LOGNAME, "Stopped publishing maintained planning scene.");
319  }
320 }
321 
323  const std::string& planning_scene_topic)
324 {
325  publish_update_types_ = update_type;
327  {
328  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>(planning_scene_topic, 100, false);
329  ROS_INFO_NAMED(LOGNAME, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
330  monitorDiffs(true);
331  publish_planning_scene_.reset(new boost::thread(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this)));
332  }
333 }
334 
336 {
337  ROS_DEBUG_NAMED(LOGNAME, "Started scene publishing thread ...");
338 
339  // publish the full planning scene once
340  {
341  moveit_msgs::PlanningScene msg;
342  {
344  if (octomap_monitor_)
345  lock = octomap_monitor_->getOcTreePtr()->reading();
346  scene_->getPlanningSceneMsg(msg);
347  }
349  ROS_DEBUG_NAMED(LOGNAME, "Published the full planning scene: '%s'", msg.name.c_str());
350  }
351 
352  do
353  {
354  moveit_msgs::PlanningScene msg;
355  bool publish_msg = false;
356  bool is_full = false;
358  {
359  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
361  new_scene_update_condition_.wait(ulock);
363  {
364  if ((publish_update_types_ & new_scene_update_) || new_scene_update_ == UPDATE_SCENE)
365  {
366  if (new_scene_update_ == UPDATE_SCENE)
367  is_full = true;
368  else
369  {
371  if (octomap_monitor_)
372  lock = octomap_monitor_->getOcTreePtr()->reading();
373  scene_->getPlanningSceneDiffMsg(msg);
374  if (new_scene_update_ == UPDATE_STATE)
375  {
376  msg.robot_state.attached_collision_objects.clear();
377  msg.robot_state.is_diff = true;
378  }
379  }
380  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the
381  // transform cache to
382  // update while we are
383  // potentially changing
384  // attached bodies
385  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
386  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
387  scene_->pushDiffs(parent_scene_);
388  scene_->clearDiffs();
389  scene_->setAttachedBodyUpdateCallback(
391  scene_->setCollisionObjectUpdateCallback(
392  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
393  if (octomap_monitor_)
394  {
395  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
396  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
397  }
398  if (is_full)
399  {
401  if (octomap_monitor_)
402  lock = octomap_monitor_->getOcTreePtr()->reading();
403  scene_->getPlanningSceneMsg(msg);
404  }
405  // also publish timestamp of this robot_state
406  msg.robot_state.joint_state.header.stamp = last_robot_motion_time_;
407  publish_msg = true;
408  }
409  new_scene_update_ = UPDATE_NONE;
410  }
411  }
412  if (publish_msg)
413  {
415  if (is_full)
416  ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str());
417  rate.sleep();
418  }
419  } while (publish_planning_scene_);
420 }
421 
422 void PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string>& topics) const
423 {
424  topics.clear();
426  {
427  const std::string& t = current_state_monitor_->getMonitoredTopic();
428  if (!t.empty())
429  topics.push_back(t);
430  }
432  topics.push_back(planning_scene_subscriber_.getTopic());
434  topics.push_back(collision_object_subscriber_.getTopic());
436  topics.push_back(planning_scene_world_subscriber_.getTopic());
437 }
438 
439 namespace
440 {
441 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr& scene,
442  const planning_scene::PlanningScene* possible_parent)
443 {
444  if (scene && scene.get() == possible_parent)
445  return true;
446  if (scene)
447  return sceneIsParentOf(scene->getParent(), possible_parent);
448  return false;
449 }
450 } // namespace
451 
452 bool PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr& scene) const
453 {
454  return sceneIsParentOf(scene_const_, scene.get());
455 }
456 
457 bool PlanningSceneMonitor::updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const
458 {
459  return sceneIsParentOf(scene_const_, scene.get());
460 }
461 
463 {
464  // do not modify update functions while we are calling them
465  boost::recursive_mutex::scoped_lock lock(update_lock_);
466 
467  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
468  update_callbacks_[i](update_type);
469  new_scene_update_ = (SceneUpdateType)((int)new_scene_update_ | (int)update_type);
470  new_scene_update_condition_.notify_all();
471 }
472 
473 bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_name)
474 {
475  if (get_scene_service_.getService() == service_name)
476  {
477  ROS_FATAL_STREAM_NAMED(LOGNAME, "requestPlanningSceneState() to self-provided service '" << service_name << "'");
478  throw std::runtime_error("requestPlanningSceneState() to self-provided service: " + service_name);
479  }
480  // use global namespace for service
481  ros::ServiceClient client = ros::NodeHandle().serviceClient<moveit_msgs::GetPlanningScene>(service_name);
482  moveit_msgs::GetPlanningScene srv;
483  srv.request.components.components =
484  srv.request.components.SCENE_SETTINGS | srv.request.components.ROBOT_STATE |
485  srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS | srv.request.components.WORLD_OBJECT_NAMES |
486  srv.request.components.WORLD_OBJECT_GEOMETRY | srv.request.components.OCTOMAP |
487  srv.request.components.TRANSFORMS | srv.request.components.ALLOWED_COLLISION_MATRIX |
488  srv.request.components.LINK_PADDING_AND_SCALING | srv.request.components.OBJECT_COLORS;
489 
490  // Make sure client is connected to server
491  if (!client.exists())
492  {
493  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for service `" << service_name << "` to exist.");
494  client.waitForExistence(ros::Duration(5.0));
495  }
496 
497  if (client.call(srv))
498  {
499  newPlanningSceneMessage(srv.response.scene);
500  }
501  else
502  {
504  LOGNAME, "Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?",
505  service_name.c_str());
506  return false;
507  }
508  return true;
509 }
510 
511 void PlanningSceneMonitor::providePlanningSceneService(const std::string& service_name)
512 {
513  // Load the service
516 }
517 
518 bool PlanningSceneMonitor::getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req,
519  moveit_msgs::GetPlanningScene::Response& res)
520 {
521  if (req.components.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
523 
524  moveit_msgs::PlanningSceneComponents all_components;
525  all_components.components = UINT_MAX; // Return all scene components if nothing is specified.
526 
527  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
528  scene_->getPlanningSceneMsg(res.scene, req.components.components ? req.components : all_components);
529 
530  return true;
531 }
532 
533 void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene)
534 {
535  newPlanningSceneMessage(*scene);
536 }
537 
539 {
540  if (scene_->getWorldNonConst()->removeObject(scene_->OCTOMAP_NS))
542  if (octomap_monitor_)
543  {
544  octomap_monitor_->getOcTreePtr()->lockWrite();
545  octomap_monitor_->getOcTreePtr()->clear();
546  octomap_monitor_->getOcTreePtr()->unlockWrite();
547  }
548  else
549  {
550  ROS_WARN_NAMED(LOGNAME, "Unable to clear octomap since no octomap monitor has been initialized");
551  }
552 }
553 
554 bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
555 {
556  if (!scene_)
557  return false;
558 
559  bool result;
560 
562  std::string old_scene_name;
563  {
564  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
565  // we don't want the transform cache to update while we are potentially changing attached bodies
566  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
567 
569  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
570  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
571  "scene update " << fmod(last_update_time_.toSec(), 10.)
572  << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
573  old_scene_name = scene_->getName();
574  result = scene_->usePlanningSceneMsg(scene);
575  if (octomap_monitor_)
576  {
577  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
578  {
579  octomap_monitor_->getOcTreePtr()->lockWrite();
580  octomap_monitor_->getOcTreePtr()->clear();
581  octomap_monitor_->getOcTreePtr()->unlockWrite();
582  }
583  }
584  robot_model_ = scene_->getRobotModel();
585 
586  // if we just reset the scene completely but we were maintaining diffs, we need to fix that
587  if (!scene.is_diff && parent_scene_)
588  {
589  // the scene is now decoupled from the parent, since we just reset it
590  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
591  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
593  scene_ = parent_scene_->diff();
595  scene_->setAttachedBodyUpdateCallback(
597  scene_->setCollisionObjectUpdateCallback(
598  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
599  }
600  if (octomap_monitor_)
601  {
602  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
603  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
604  }
605  }
606 
607  // if we have a diff, try to more accuratelly determine the update type
608  if (scene.is_diff)
609  {
610  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
611  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
612  scene.link_scale.empty();
613  if (no_other_scene_upd)
614  {
615  upd = UPDATE_NONE;
616  if (!planning_scene::PlanningScene::isEmpty(scene.world))
617  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
618 
619  if (!scene.fixed_frame_transforms.empty())
620  upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
621 
622  if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
623  {
624  upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
625  if (!scene.robot_state.attached_collision_objects.empty() || !static_cast<bool>(scene.robot_state.is_diff))
626  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
627  }
628  }
629  }
631  return result;
632 }
633 
634 void PlanningSceneMonitor::newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world)
635 {
636  if (scene_)
637  {
639  {
640  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
642  scene_->getWorldNonConst()->clearObjects();
643  scene_->processPlanningSceneWorldMsg(*world);
644  if (octomap_monitor_)
645  {
646  if (world->octomap.octomap.data.empty())
647  {
648  octomap_monitor_->getOcTreePtr()->lockWrite();
649  octomap_monitor_->getOcTreePtr()->clear();
650  octomap_monitor_->getOcTreePtr()->unlockWrite();
651  }
652  }
653  }
655  }
656 }
657 
658 void PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj)
659 {
660  if (!scene_)
661  return;
662 
664  {
665  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
667  if (!scene_->processCollisionObjectMsg(*obj))
668  return;
669  }
671 }
672 
673 void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
674 {
675  if (scene_)
676  {
678  {
679  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
681  scene_->processAttachedCollisionObjectMsg(*obj);
682  }
684  }
685 }
686 
688 {
689  if (!octomap_monitor_)
690  return;
691 
692  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
693 
695  const std::vector<const robot_model::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
697  bool warned = false;
698  for (std::size_t i = 0; i < links.size(); ++i)
699  {
700  std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes(); // copy shared ptrs on purpuse
701  for (std::size_t j = 0; j < shapes.size(); ++j)
702  {
703  // merge mesh vertices up to 0.1 mm apart
704  if (shapes[j]->type == shapes::MESH)
705  {
706  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
707  m->mergeVertices(1e-4);
708  shapes[j].reset(m);
709  }
710 
711  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
712  if (h)
713  link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
714  }
715  if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
716  {
717  ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
718  warned = true;
719  }
720  }
721 }
722 
724 {
725  if (!octomap_monitor_)
726  return;
727 
728  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
729 
730  for (LinkShapeHandles::iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
731  for (std::size_t i = 0; i < it->second.size(); ++i)
732  octomap_monitor_->forgetShape(it->second[i].first);
733  link_shape_handles_.clear();
734 }
735 
737 {
738  if (!octomap_monitor_)
739  return;
740 
741  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
742 
743  // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
744  for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin();
745  it != attached_body_shape_handles_.end(); ++it)
746  for (std::size_t k = 0; k < it->second.size(); ++k)
747  octomap_monitor_->forgetShape(it->second[k].first);
749 }
750 
752 {
753  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
754 
756  // add attached objects again
757  std::vector<const robot_state::AttachedBody*> ab;
758  scene_->getCurrentState().getAttachedBodies(ab);
759  for (std::size_t i = 0; i < ab.size(); ++i)
761 }
762 
764 {
765  if (!octomap_monitor_)
766  return;
767 
768  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
769 
770  // clear information about any attached object
771  for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin();
772  it != collision_body_shape_handles_.end(); ++it)
773  for (std::size_t k = 0; k < it->second.size(); ++k)
774  octomap_monitor_->forgetShape(it->second[k].first);
776 }
777 
779 {
780  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
781 
783  for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end();
784  ++it)
785  excludeWorldObjectFromOctree(it->second);
786 }
787 
788 void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body)
789 {
790  if (!octomap_monitor_)
791  return;
792 
793  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
794  bool found = false;
795  for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
796  {
797  if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
798  continue;
799  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
800  if (h)
801  {
802  found = true;
803  attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
804  }
805  }
806  if (found)
807  ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
808 }
809 
810 void PlanningSceneMonitor::includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body)
811 {
812  if (!octomap_monitor_)
813  return;
814 
815  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
816 
817  AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
818  if (it != attached_body_shape_handles_.end())
819  {
820  for (std::size_t k = 0; k < it->second.size(); ++k)
821  octomap_monitor_->forgetShape(it->second[k].first);
822  ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
824  }
825 }
826 
827 void PlanningSceneMonitor::excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj)
828 {
829  if (!octomap_monitor_)
830  return;
831 
832  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
833 
834  bool found = false;
835  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
836  {
837  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
838  continue;
839  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
840  if (h)
841  {
842  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
843  found = true;
844  }
845  }
846  if (found)
847  ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
848 }
849 
850 void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj)
851 {
852  if (!octomap_monitor_)
853  return;
854 
855  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
856 
857  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
858  if (it != collision_body_shape_handles_.end())
859  {
860  for (std::size_t k = 0; k < it->second.size(); ++k)
861  octomap_monitor_->forgetShape(it->second[k].first);
862  ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
864  }
865 }
866 
867 void PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body,
868  bool just_attached)
869 {
870  if (!octomap_monitor_)
871  return;
872 
873  if (just_attached)
874  excludeAttachedBodyFromOctree(attached_body);
875  else
876  includeAttachedBodyInOctree(attached_body);
877 }
878 
879 void PlanningSceneMonitor::currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& obj,
881 {
882  if (!octomap_monitor_)
883  return;
885  return;
886 
889  else if (action & collision_detection::World::DESTROY)
891  else
892  {
895  }
896 }
897 
899 {
900  if (t.isZero())
901  return false;
903  ros::WallDuration timeout(wait_time);
904 
905  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
906 
908  {
909  // Wait for next robot update in state monitor.
910  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
911 
912  /* As robot updates are passed to the planning scene only in throttled fashion, there might
913  be still an update pending. If so, explicitly update the planning scene here.
914  If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
915  if (success)
916  {
917  boost::mutex::scoped_lock lock(state_pending_mutex_);
918  if (state_update_pending_) // enforce state update
919  {
920  state_update_pending_ = false;
922  lock.unlock();
924  }
925  return true;
926  }
927 
928  ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
929  return false;
930  }
931 
932  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
933  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
934  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
935  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
936  ros::Time prev_robot_motion_time = last_robot_motion_time_;
937  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
938  timeout > ros::WallDuration())
939  {
940  ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
941  new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
942  timeout -= ros::WallTime::now() - start; // compute remaining wait_time
943  }
944  bool success = last_robot_motion_time_ >= t;
945  // suppress warning if we received an update at all
946  if (!success && prev_robot_motion_time != last_robot_motion_time_)
947  ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
948  (t - last_robot_motion_time_).toSec());
949 
950  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
951  << " scene update: " << (t - last_update_time_).toSec());
952  return success;
953 }
954 
956 {
957  scene_update_mutex_.lock_shared();
958  if (octomap_monitor_)
959  octomap_monitor_->getOcTreePtr()->lockRead();
960 }
961 
963 {
964  if (octomap_monitor_)
965  octomap_monitor_->getOcTreePtr()->unlockRead();
966  scene_update_mutex_.unlock_shared();
967 }
968 
970 {
971  scene_update_mutex_.lock();
972  if (octomap_monitor_)
973  octomap_monitor_->getOcTreePtr()->lockWrite();
974 }
975 
977 {
978  if (octomap_monitor_)
979  octomap_monitor_->getOcTreePtr()->unlockWrite();
980  scene_update_mutex_.unlock();
981 }
982 
983 void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
984 {
986 
987  ROS_INFO_NAMED(LOGNAME, "Starting planning scene monitor");
988  // listen for planning scene updates; these messages include transforms, so no need for filters
989  if (!scene_topic.empty())
990  {
993  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
994  }
995 }
996 
998 {
1000  {
1001  ROS_INFO_NAMED(LOGNAME, "Stopping planning scene monitor");
1003  }
1004 }
1005 
1006 bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
1008 {
1009  if (!tf_buffer_)
1010  return false;
1011  try
1012  {
1013  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
1014 
1015  for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
1016  {
1017  tf_buffer_->canTransform(target_frame, it->first->getName(), target_time, shape_transform_cache_lookup_wait_time_);
1018  Eigen::Isometry3d ttr =
1019  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, it->first->getName(), target_time));
1020  for (std::size_t j = 0; j < it->second.size(); ++j)
1021  cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
1022  }
1023  for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin();
1024  it != attached_body_shape_handles_.end(); ++it)
1025  {
1026  tf_buffer_->canTransform(target_frame, it->first->getAttachedLinkName(), target_time,
1028  Eigen::Isometry3d transform = tf2::transformToEigen(
1029  tf_buffer_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time));
1030  for (std::size_t k = 0; k < it->second.size(); ++k)
1031  cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
1032  }
1033  {
1034  tf_buffer_->canTransform(target_frame, scene_->getPlanningFrame(), target_time,
1036  Eigen::Isometry3d transform =
1037  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time));
1038  for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin();
1039  it != collision_body_shape_handles_.end(); ++it)
1040  for (std::size_t k = 0; k < it->second.size(); ++k)
1041  cache[it->second[k].first] = transform * (*it->second[k].second);
1042  }
1043  }
1044  catch (tf2::TransformException& ex)
1045  {
1046  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
1047  return false;
1048  }
1049  return true;
1050 }
1051 
1052 void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collision_objects_topic,
1053  const std::string& planning_scene_world_topic,
1054  const bool load_octomap_monitor)
1055 {
1057  ROS_INFO_NAMED(LOGNAME, "Starting world geometry update monitor for collision objects, attached objects, octomap "
1058  "updates.");
1059 
1060  // Listen to the /collision_objects topic to detect requests to add/remove/update collision objects to/from the world
1061  if (!collision_objects_topic.empty())
1062  {
1064  root_nh_.subscribe(collision_objects_topic, 1024, &PlanningSceneMonitor::collisionObjectCallback, this);
1065  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
1066  }
1067 
1068  if (!planning_scene_world_topic.empty())
1069  {
1071  root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
1072  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
1073  root_nh_.resolveName(planning_scene_world_topic).c_str());
1074  }
1075 
1076  // Ocotomap monitor is optional
1077  if (load_octomap_monitor)
1078  {
1079  if (!octomap_monitor_)
1080  {
1085 
1086  octomap_monitor_->setTransformCacheCallback(
1087  boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
1088  octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
1089  }
1090  octomap_monitor_->startMonitor();
1091  }
1092 }
1093 
1095 {
1097  {
1098  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1100  }
1102  {
1103  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1105  }
1106  if (octomap_monitor_)
1107  octomap_monitor_->stopMonitor();
1108 }
1109 
1110 void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1111  const std::string& attached_objects_topic)
1112 {
1113  stopStateMonitor();
1114  if (scene_)
1115  {
1118  current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
1119  current_state_monitor_->startStateMonitor(joint_states_topic);
1120 
1121  {
1122  boost::mutex::scoped_lock lock(state_pending_mutex_);
1123  if (!dt_state_update_.isZero())
1125  }
1126 
1127  if (!attached_objects_topic.empty())
1128  {
1129  // using regular message filter as there's no header
1131  root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
1132  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
1133  root_nh_.resolveName(attached_objects_topic).c_str());
1134  }
1135  }
1136  else
1137  ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
1138 }
1139 
1141 {
1143  current_state_monitor_->stopStateMonitor();
1146 
1147  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1149  {
1150  boost::mutex::scoped_lock lock(state_pending_mutex_);
1151  state_update_pending_ = false;
1152  }
1153 }
1154 
1155 void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr& /* joint_state */)
1156 {
1157  const ros::WallTime& n = ros::WallTime::now();
1159 
1160  bool update = false;
1161  {
1162  boost::mutex::scoped_lock lock(state_pending_mutex_);
1163 
1164  if (dt < dt_state_update_)
1165  {
1166  state_update_pending_ = true;
1167  }
1168  else
1169  {
1170  state_update_pending_ = false;
1171  last_robot_state_update_wall_time_ = n;
1172  update = true;
1173  }
1174  }
1175  // run the state update with state_pending_mutex_ unlocked
1176  if (update)
1178 }
1179 
1181 {
1183  {
1184  bool update = false;
1185 
1186  const ros::WallTime& n = ros::WallTime::now();
1188 
1189  {
1190  // lock for access to dt_state_update_ and state_update_pending_
1191  boost::mutex::scoped_lock lock(state_pending_mutex_);
1193  {
1194  state_update_pending_ = false;
1195  last_robot_state_update_wall_time_ = ros::WallTime::now();
1196  update = true;
1197  ROS_DEBUG_STREAM_NAMED(LOGNAME,
1198  "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
1199  }
1200  }
1201 
1202  // run the state update with state_pending_mutex_ unlocked
1203  if (update)
1204  {
1206  ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
1207  }
1208  }
1209 }
1210 
1212 {
1213  if (!octomap_monitor_)
1214  return;
1215 
1217  {
1218  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1220  octomap_monitor_->getOcTreePtr()->lockRead();
1221  try
1222  {
1223  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Isometry3d::Identity());
1224  octomap_monitor_->getOcTreePtr()->unlockRead();
1225  }
1226  catch (...)
1227  {
1228  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1229  throw;
1230  }
1231  }
1233 }
1234 
1236 {
1237  bool update = false;
1238  if (hz > std::numeric_limits<double>::epsilon())
1239  {
1240  boost::mutex::scoped_lock lock(state_pending_mutex_);
1241  dt_state_update_.fromSec(1.0 / hz);
1244  }
1245  else
1246  {
1247  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1249  boost::mutex::scoped_lock lock(state_pending_mutex_);
1252  update = true;
1253  }
1254  ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
1255 
1256  if (update)
1258 }
1259 
1261 {
1263  {
1264  std::vector<std::string> missing;
1265  if (!current_state_monitor_->haveCompleteState(missing) &&
1266  (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
1267  {
1268  std::string missing_str = boost::algorithm::join(missing, ", ");
1269  ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known. Missing %s",
1270  missing_str.c_str());
1271  }
1272 
1273  {
1274  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1276  ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
1277  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1278  scene_->getCurrentStateNonConst().update(); // compute all transforms
1279  }
1281  }
1282  else
1283  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
1284 }
1285 
1286 void PlanningSceneMonitor::addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn)
1287 {
1288  boost::recursive_mutex::scoped_lock lock(update_lock_);
1289  if (fn)
1290  update_callbacks_.push_back(fn);
1291 }
1292 
1294 {
1295  boost::recursive_mutex::scoped_lock lock(update_lock_);
1296  update_callbacks_.clear();
1297 }
1298 
1300 {
1302  ROS_DEBUG_NAMED(LOGNAME, "Maximum frquency for publishing a planning scene is now %lf Hz",
1304 }
1305 
1306 void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms)
1307 {
1308  const std::string& target = getRobotModel()->getModelFrame();
1309 
1310  std::vector<std::string> all_frame_names;
1311  tf_buffer_->_getFrameStrings(all_frame_names);
1312  for (std::size_t i = 0; i < all_frame_names.size(); ++i)
1313  {
1314  if (all_frame_names[i] == target || getRobotModel()->hasLinkModel(all_frame_names[i]))
1315  continue;
1316 
1317  geometry_msgs::TransformStamped f;
1318  try
1319  {
1320  f = tf_buffer_->lookupTransform(target, all_frame_names[i], ros::Time(0));
1321  }
1322  catch (tf2::TransformException& ex)
1323  {
1324  ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
1325  << all_frame_names[i] << "' to planning frame '" << target << "' ("
1326  << ex.what() << ")");
1327  continue;
1328  }
1329  f.header.frame_id = all_frame_names[i];
1330  f.child_frame_id = target;
1331  transforms.push_back(f);
1332  }
1333 }
1334 
1336 {
1337  if (!tf_buffer_)
1338  return;
1339 
1340  if (scene_)
1341  {
1342  std::vector<geometry_msgs::TransformStamped> transforms;
1343  getUpdatedFrameTransforms(transforms);
1344  {
1345  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1346  scene_->getTransformsNonConst().setTransforms(transforms);
1348  }
1350  }
1351 }
1352 
1354 {
1355  if (octomap_monitor_)
1356  octomap_monitor_->publishDebugInformation(flag);
1357 }
1358 
1359 void PlanningSceneMonitor::configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene)
1360 {
1361  if (!scene || robot_description_.empty())
1362  return;
1363  collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
1364 
1365  // read overriding values from the param server
1366 
1367  // first we do default collision operations
1368  if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
1369  ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
1370  else
1371  {
1372  ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
1373 
1374  XmlRpc::XmlRpcValue coll_ops;
1375  nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
1376 
1377  if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
1378  {
1379  ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
1380  return;
1381  }
1382 
1383  if (coll_ops.size() == 0)
1384  {
1385  ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
1386  return;
1387  }
1388 
1389  for (int i = 0; i < coll_ops.size(); ++i)
1390  {
1391  if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
1392  {
1393  ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
1394  continue;
1395  }
1396  acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
1397  std::string(coll_ops[i]["operation"]) == "disable");
1398  }
1399  }
1400 }
1401 
1403 {
1404  if (robot_description_.empty())
1405  {
1406  default_robot_padd_ = 0.0;
1407  default_robot_scale_ = 1.0;
1408  default_object_padd_ = 0.0;
1409  default_attached_padd_ = 0.0;
1410  return;
1411  }
1412 
1413  // Ensure no leading slash creates a bad param server address
1414  const std::string robot_description =
1415  (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
1416 
1417  nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
1418  nh_.param(robot_description + "_planning/default_robot_scale", default_robot_scale_, 1.0);
1419  nh_.param(robot_description + "_planning/default_object_padding", default_object_padd_, 0.0);
1420  nh_.param(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
1421  nh_.param(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_,
1422  std::map<std::string, double>());
1423  nh_.param(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_,
1424  std::map<std::string, double>());
1425 
1426  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
1427  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
1428 }
1429 } // 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 Mon Nov 23 2020 03:53:16