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  octomap_monitor_->getOcTreePtr()->lockWrite();
541  octomap_monitor_->getOcTreePtr()->clear();
542  octomap_monitor_->getOcTreePtr()->unlockWrite();
543 }
544 
545 bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
546 {
547  if (!scene_)
548  return false;
549 
550  bool result;
551 
553  std::string old_scene_name;
554  {
555  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
556  // we don't want the transform cache to update while we are potentially changing attached bodies
557  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
558 
560  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
561  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
562  "scene update " << fmod(last_update_time_.toSec(), 10.)
563  << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
564  old_scene_name = scene_->getName();
565  result = scene_->usePlanningSceneMsg(scene);
566  if (octomap_monitor_)
567  {
568  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
569  {
570  octomap_monitor_->getOcTreePtr()->lockWrite();
571  octomap_monitor_->getOcTreePtr()->clear();
572  octomap_monitor_->getOcTreePtr()->unlockWrite();
573  }
574  }
575  robot_model_ = scene_->getRobotModel();
576 
577  // if we just reset the scene completely but we were maintaining diffs, we need to fix that
578  if (!scene.is_diff && parent_scene_)
579  {
580  // the scene is now decoupled from the parent, since we just reset it
581  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
582  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
584  scene_ = parent_scene_->diff();
586  scene_->setAttachedBodyUpdateCallback(
588  scene_->setCollisionObjectUpdateCallback(
589  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
590  }
591  if (octomap_monitor_)
592  {
593  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
594  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
595  }
596  }
597 
598  // if we have a diff, try to more accuratelly determine the update type
599  if (scene.is_diff)
600  {
601  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
602  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
603  scene.link_scale.empty();
604  if (no_other_scene_upd)
605  {
606  upd = UPDATE_NONE;
607  if (!planning_scene::PlanningScene::isEmpty(scene.world))
608  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
609 
610  if (!scene.fixed_frame_transforms.empty())
611  upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
612 
613  if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
614  {
615  upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
616  if (!scene.robot_state.attached_collision_objects.empty() || !static_cast<bool>(scene.robot_state.is_diff))
617  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
618  }
619  }
620  }
622  return result;
623 }
624 
625 void PlanningSceneMonitor::newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world)
626 {
627  if (scene_)
628  {
630  {
631  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
633  scene_->getWorldNonConst()->clearObjects();
634  scene_->processPlanningSceneWorldMsg(*world);
635  if (octomap_monitor_)
636  {
637  if (world->octomap.octomap.data.empty())
638  {
639  octomap_monitor_->getOcTreePtr()->lockWrite();
640  octomap_monitor_->getOcTreePtr()->clear();
641  octomap_monitor_->getOcTreePtr()->unlockWrite();
642  }
643  }
644  }
646  }
647 }
648 
649 void PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj)
650 {
651  if (!scene_)
652  return;
653 
655  {
656  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
658  if (!scene_->processCollisionObjectMsg(*obj))
659  return;
660  }
662 }
663 
664 void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
665 {
666  if (scene_)
667  {
669  {
670  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
672  scene_->processAttachedCollisionObjectMsg(*obj);
673  }
675  }
676 }
677 
679 {
680  if (!octomap_monitor_)
681  return;
682 
683  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
684 
686  const std::vector<const robot_model::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
688  bool warned = false;
689  for (std::size_t i = 0; i < links.size(); ++i)
690  {
691  std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes(); // copy shared ptrs on purpuse
692  for (std::size_t j = 0; j < shapes.size(); ++j)
693  {
694  // merge mesh vertices up to 0.1 mm apart
695  if (shapes[j]->type == shapes::MESH)
696  {
697  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
698  m->mergeVertices(1e-4);
699  shapes[j].reset(m);
700  }
701 
702  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
703  if (h)
704  link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
705  }
706  if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
707  {
708  ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
709  warned = true;
710  }
711  }
712 }
713 
715 {
716  if (!octomap_monitor_)
717  return;
718 
719  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
720 
721  for (LinkShapeHandles::iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
722  for (std::size_t i = 0; i < it->second.size(); ++i)
723  octomap_monitor_->forgetShape(it->second[i].first);
724  link_shape_handles_.clear();
725 }
726 
728 {
729  if (!octomap_monitor_)
730  return;
731 
732  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
733 
734  // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
735  for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin();
736  it != attached_body_shape_handles_.end(); ++it)
737  for (std::size_t k = 0; k < it->second.size(); ++k)
738  octomap_monitor_->forgetShape(it->second[k].first);
740 }
741 
743 {
744  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
745 
747  // add attached objects again
748  std::vector<const robot_state::AttachedBody*> ab;
749  scene_->getCurrentState().getAttachedBodies(ab);
750  for (std::size_t i = 0; i < ab.size(); ++i)
752 }
753 
755 {
756  if (!octomap_monitor_)
757  return;
758 
759  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
760 
761  // clear information about any attached object
762  for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin();
763  it != collision_body_shape_handles_.end(); ++it)
764  for (std::size_t k = 0; k < it->second.size(); ++k)
765  octomap_monitor_->forgetShape(it->second[k].first);
767 }
768 
770 {
771  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
772 
774  for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end();
775  ++it)
776  excludeWorldObjectFromOctree(it->second);
777 }
778 
779 void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body)
780 {
781  if (!octomap_monitor_)
782  return;
783 
784  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
785  bool found = false;
786  for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
787  {
788  if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
789  continue;
790  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
791  if (h)
792  {
793  found = true;
794  attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
795  }
796  }
797  if (found)
798  ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
799 }
800 
801 void PlanningSceneMonitor::includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body)
802 {
803  if (!octomap_monitor_)
804  return;
805 
806  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
807 
808  AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
809  if (it != attached_body_shape_handles_.end())
810  {
811  for (std::size_t k = 0; k < it->second.size(); ++k)
812  octomap_monitor_->forgetShape(it->second[k].first);
813  ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
815  }
816 }
817 
818 void PlanningSceneMonitor::excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj)
819 {
820  if (!octomap_monitor_)
821  return;
822 
823  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
824 
825  bool found = false;
826  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
827  {
828  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
829  continue;
830  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
831  if (h)
832  {
833  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
834  found = true;
835  }
836  }
837  if (found)
838  ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
839 }
840 
841 void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj)
842 {
843  if (!octomap_monitor_)
844  return;
845 
846  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
847 
848  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
849  if (it != collision_body_shape_handles_.end())
850  {
851  for (std::size_t k = 0; k < it->second.size(); ++k)
852  octomap_monitor_->forgetShape(it->second[k].first);
853  ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
855  }
856 }
857 
858 void PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body,
859  bool just_attached)
860 {
861  if (!octomap_monitor_)
862  return;
863 
864  if (just_attached)
865  excludeAttachedBodyFromOctree(attached_body);
866  else
867  includeAttachedBodyInOctree(attached_body);
868 }
869 
870 void PlanningSceneMonitor::currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& obj,
872 {
873  if (!octomap_monitor_)
874  return;
876  return;
877 
880  else if (action & collision_detection::World::DESTROY)
882  else
883  {
886  }
887 }
888 
890 {
891  if (t.isZero())
892  return false;
894  ros::WallDuration timeout(wait_time);
895 
896  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
897 
899  {
900  // Wait for next robot update in state monitor.
901  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
902 
903  /* As robot updates are passed to the planning scene only in throttled fashion, there might
904  be still an update pending. If so, explicitly update the planning scene here.
905  If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
906  if (success)
907  {
908  boost::mutex::scoped_lock lock(state_pending_mutex_);
909  if (state_update_pending_) // enforce state update
910  {
911  state_update_pending_ = false;
913  lock.unlock();
915  }
916  return true;
917  }
918 
919  ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
920  return false;
921  }
922 
923  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
924  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
925  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
926  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
927  ros::Time prev_robot_motion_time = last_robot_motion_time_;
928  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
929  timeout > ros::WallDuration())
930  {
931  ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
932  new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
933  timeout -= ros::WallTime::now() - start; // compute remaining wait_time
934  }
935  bool success = last_robot_motion_time_ >= t;
936  // suppress warning if we received an update at all
937  if (!success && prev_robot_motion_time != last_robot_motion_time_)
938  ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
939  (t - last_robot_motion_time_).toSec());
940 
941  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
942  << " scene update: " << (t - last_update_time_).toSec());
943  return success;
944 }
945 
947 {
948  scene_update_mutex_.lock_shared();
949  if (octomap_monitor_)
950  octomap_monitor_->getOcTreePtr()->lockRead();
951 }
952 
954 {
955  if (octomap_monitor_)
956  octomap_monitor_->getOcTreePtr()->unlockRead();
957  scene_update_mutex_.unlock_shared();
958 }
959 
961 {
962  scene_update_mutex_.lock();
963  if (octomap_monitor_)
964  octomap_monitor_->getOcTreePtr()->lockWrite();
965 }
966 
968 {
969  if (octomap_monitor_)
970  octomap_monitor_->getOcTreePtr()->unlockWrite();
971  scene_update_mutex_.unlock();
972 }
973 
974 void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
975 {
977 
978  ROS_INFO_NAMED(LOGNAME, "Starting planning scene monitor");
979  // listen for planning scene updates; these messages include transforms, so no need for filters
980  if (!scene_topic.empty())
981  {
984  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
985  }
986 }
987 
989 {
991  {
992  ROS_INFO_NAMED(LOGNAME, "Stopping planning scene monitor");
994  }
995 }
996 
997 bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
999 {
1000  if (!tf_buffer_)
1001  return false;
1002  try
1003  {
1004  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
1005 
1006  for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
1007  {
1008  tf_buffer_->canTransform(target_frame, it->first->getName(), target_time,
1010  Eigen::Isometry3d ttr =
1011  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, it->first->getName(), target_time));
1012  for (std::size_t j = 0; j < it->second.size(); ++j)
1013  cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
1014  }
1015  for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin();
1016  it != attached_body_shape_handles_.end(); ++it)
1017  {
1018  tf_buffer_->canTransform(target_frame, it->first->getAttachedLinkName(), target_time,
1020  Eigen::Isometry3d transform = tf2::transformToEigen(
1021  tf_buffer_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time));
1022  for (std::size_t k = 0; k < it->second.size(); ++k)
1023  cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
1024  }
1025  {
1026  tf_buffer_->canTransform(target_frame, scene_->getPlanningFrame(), target_time,
1028  Eigen::Isometry3d transform =
1029  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time));
1030  for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin();
1031  it != collision_body_shape_handles_.end(); ++it)
1032  for (std::size_t k = 0; k < it->second.size(); ++k)
1033  cache[it->second[k].first] = transform * (*it->second[k].second);
1034  }
1035  }
1036  catch (tf2::TransformException& ex)
1037  {
1038  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
1039  return false;
1040  }
1041  return true;
1042 }
1043 
1044 void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collision_objects_topic,
1045  const std::string& planning_scene_world_topic,
1046  const bool load_octomap_monitor)
1047 {
1049  ROS_INFO_NAMED(LOGNAME, "Starting world geometry update monitor for collision objects, attached objects, octomap "
1050  "updates.");
1051 
1052  // Listen to the /collision_objects topic to detect requests to add/remove/update collision objects to/from the world
1053  if (!collision_objects_topic.empty())
1054  {
1056  root_nh_.subscribe(collision_objects_topic, 1024, &PlanningSceneMonitor::collisionObjectCallback, this);
1057  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
1058  }
1059 
1060  if (!planning_scene_world_topic.empty())
1061  {
1063  root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
1064  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
1065  root_nh_.resolveName(planning_scene_world_topic).c_str());
1066  }
1067 
1068  // Ocotomap monitor is optional
1069  if (load_octomap_monitor)
1070  {
1071  if (!octomap_monitor_)
1072  {
1077 
1078  octomap_monitor_->setTransformCacheCallback(
1079  boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
1080  octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
1081  }
1082  octomap_monitor_->startMonitor();
1083  }
1084 }
1085 
1087 {
1089  {
1090  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1092  }
1094  {
1095  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1097  }
1098  if (octomap_monitor_)
1099  octomap_monitor_->stopMonitor();
1100 }
1101 
1102 void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1103  const std::string& attached_objects_topic)
1104 {
1105  stopStateMonitor();
1106  if (scene_)
1107  {
1110  current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
1111  current_state_monitor_->startStateMonitor(joint_states_topic);
1112 
1113  {
1114  boost::mutex::scoped_lock lock(state_pending_mutex_);
1115  if (!dt_state_update_.isZero())
1117  }
1118 
1119  if (!attached_objects_topic.empty())
1120  {
1121  // using regular message filter as there's no header
1123  root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
1124  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
1125  root_nh_.resolveName(attached_objects_topic).c_str());
1126  }
1127  }
1128  else
1129  ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
1130 }
1131 
1133 {
1135  current_state_monitor_->stopStateMonitor();
1138 
1139  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1141  {
1142  boost::mutex::scoped_lock lock(state_pending_mutex_);
1143  state_update_pending_ = false;
1144  }
1145 }
1146 
1147 void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr& /* joint_state */)
1148 {
1149  const ros::WallTime& n = ros::WallTime::now();
1151 
1152  bool update = false;
1153  {
1154  boost::mutex::scoped_lock lock(state_pending_mutex_);
1155 
1156  if (dt < dt_state_update_)
1157  {
1158  state_update_pending_ = true;
1159  }
1160  else
1161  {
1162  state_update_pending_ = false;
1163  last_robot_state_update_wall_time_ = n;
1164  update = true;
1165  }
1166  }
1167  // run the state update with state_pending_mutex_ unlocked
1168  if (update)
1170 }
1171 
1173 {
1175  {
1176  bool update = false;
1177 
1178  const ros::WallTime& n = ros::WallTime::now();
1180 
1181  {
1182  // lock for access to dt_state_update_ and state_update_pending_
1183  boost::mutex::scoped_lock lock(state_pending_mutex_);
1185  {
1186  state_update_pending_ = false;
1187  last_robot_state_update_wall_time_ = ros::WallTime::now();
1188  update = true;
1189  ROS_DEBUG_STREAM_NAMED(LOGNAME,
1190  "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
1191  }
1192  }
1193 
1194  // run the state update with state_pending_mutex_ unlocked
1195  if (update)
1196  {
1198  ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
1199  }
1200  }
1201 }
1202 
1204 {
1205  if (!octomap_monitor_)
1206  return;
1207 
1209  {
1210  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1212  octomap_monitor_->getOcTreePtr()->lockRead();
1213  try
1214  {
1215  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Isometry3d::Identity());
1216  octomap_monitor_->getOcTreePtr()->unlockRead();
1217  }
1218  catch (...)
1219  {
1220  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1221  throw;
1222  }
1223  }
1225 }
1226 
1228 {
1229  bool update = false;
1230  if (hz > std::numeric_limits<double>::epsilon())
1231  {
1232  boost::mutex::scoped_lock lock(state_pending_mutex_);
1233  dt_state_update_.fromSec(1.0 / hz);
1236  }
1237  else
1238  {
1239  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1241  boost::mutex::scoped_lock lock(state_pending_mutex_);
1244  update = true;
1245  }
1246  ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
1247 
1248  if (update)
1250 }
1251 
1253 {
1255  {
1256  std::vector<std::string> missing;
1257  if (!current_state_monitor_->haveCompleteState(missing) &&
1258  (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
1259  {
1260  std::string missing_str = boost::algorithm::join(missing, ", ");
1261  ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known. Missing %s",
1262  missing_str.c_str());
1263  }
1264 
1265  {
1266  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1268  ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
1269  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1270  scene_->getCurrentStateNonConst().update(); // compute all transforms
1271  }
1273  }
1274  else
1275  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
1276 }
1277 
1278 void PlanningSceneMonitor::addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn)
1279 {
1280  boost::recursive_mutex::scoped_lock lock(update_lock_);
1281  if (fn)
1282  update_callbacks_.push_back(fn);
1283 }
1284 
1286 {
1287  boost::recursive_mutex::scoped_lock lock(update_lock_);
1288  update_callbacks_.clear();
1289 }
1290 
1292 {
1294  ROS_DEBUG_NAMED(LOGNAME, "Maximum frquency for publishing a planning scene is now %lf Hz",
1296 }
1297 
1298 void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms)
1299 {
1300  const std::string& target = getRobotModel()->getModelFrame();
1301 
1302  std::vector<std::string> all_frame_names;
1303  tf_buffer_->_getFrameStrings(all_frame_names);
1304  for (std::size_t i = 0; i < all_frame_names.size(); ++i)
1305  {
1306  if (all_frame_names[i] == target || getRobotModel()->hasLinkModel(all_frame_names[i]))
1307  continue;
1308 
1309  geometry_msgs::TransformStamped f;
1310  try
1311  {
1312  f = tf_buffer_->lookupTransform(target, all_frame_names[i], ros::Time(0));
1313  }
1314  catch (tf2::TransformException& ex)
1315  {
1316  ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
1317  << all_frame_names[i] << "' to planning frame '" << target << "' ("
1318  << ex.what() << ")");
1319  continue;
1320  }
1321  f.header.frame_id = all_frame_names[i];
1322  f.child_frame_id = target;
1323  transforms.push_back(f);
1324  }
1325 }
1326 
1328 {
1329  if (!tf_buffer_)
1330  return;
1331 
1332  if (scene_)
1333  {
1334  std::vector<geometry_msgs::TransformStamped> transforms;
1335  getUpdatedFrameTransforms(transforms);
1336  {
1337  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1338  scene_->getTransformsNonConst().setTransforms(transforms);
1340  }
1342  }
1343 }
1344 
1346 {
1347  if (octomap_monitor_)
1348  octomap_monitor_->publishDebugInformation(flag);
1349 }
1350 
1351 void PlanningSceneMonitor::configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene)
1352 {
1353  if (!scene || robot_description_.empty())
1354  return;
1355  collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
1356 
1357  // read overriding values from the param server
1358 
1359  // first we do default collision operations
1360  if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
1361  ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
1362  else
1363  {
1364  ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
1365 
1366  XmlRpc::XmlRpcValue coll_ops;
1367  nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
1368 
1369  if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
1370  {
1371  ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
1372  return;
1373  }
1374 
1375  if (coll_ops.size() == 0)
1376  {
1377  ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
1378  return;
1379  }
1380 
1381  for (int i = 0; i < coll_ops.size(); ++i)
1382  {
1383  if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
1384  {
1385  ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
1386  continue;
1387  }
1388  acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
1389  std::string(coll_ops[i]["operation"]) == "disable");
1390  }
1391  }
1392 }
1393 
1395 {
1396  if (robot_description_.empty())
1397  {
1398  default_robot_padd_ = 0.0;
1399  default_robot_scale_ = 1.0;
1400  default_object_padd_ = 0.0;
1401  default_attached_padd_ = 0.0;
1402  return;
1403  }
1404 
1405  // Ensure no leading slash creates a bad param server address
1406  const std::string robot_description =
1407  (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
1408 
1409  nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
1410  nh_.param(robot_description + "_planning/default_robot_scale", default_robot_scale_, 1.0);
1411  nh_.param(robot_description + "_planning/default_object_padding", default_object_padd_, 0.0);
1412  nh_.param(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
1413  nh_.param(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_,
1414  std::map<std::string, double>());
1415  nh_.param(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_,
1416  std::map<std::string, double>());
1417 
1418  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
1419  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
1420 }
1421 } // 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 Thu Jul 2 2020 03:55:39