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 (octomap_monitor_)
541  {
542  octomap_monitor_->getOcTreePtr()->lockWrite();
543  octomap_monitor_->getOcTreePtr()->clear();
544  octomap_monitor_->getOcTreePtr()->unlockWrite();
545  }
546  else
547  {
548  ROS_WARN_NAMED(LOGNAME, "Unable to clear octomap since no octomap monitor has been initialized");
549  }
550 }
551 
552 bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
553 {
554  if (!scene_)
555  return false;
556 
557  bool result;
558 
560  std::string old_scene_name;
561  {
562  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
563  // we don't want the transform cache to update while we are potentially changing attached bodies
564  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
565 
567  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
568  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
569  "scene update " << fmod(last_update_time_.toSec(), 10.)
570  << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
571  old_scene_name = scene_->getName();
572  result = scene_->usePlanningSceneMsg(scene);
573  if (octomap_monitor_)
574  {
575  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
576  {
577  octomap_monitor_->getOcTreePtr()->lockWrite();
578  octomap_monitor_->getOcTreePtr()->clear();
579  octomap_monitor_->getOcTreePtr()->unlockWrite();
580  }
581  }
582  robot_model_ = scene_->getRobotModel();
583 
584  // if we just reset the scene completely but we were maintaining diffs, we need to fix that
585  if (!scene.is_diff && parent_scene_)
586  {
587  // the scene is now decoupled from the parent, since we just reset it
588  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
589  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
591  scene_ = parent_scene_->diff();
593  scene_->setAttachedBodyUpdateCallback(
595  scene_->setCollisionObjectUpdateCallback(
596  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
597  }
598  if (octomap_monitor_)
599  {
600  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
601  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
602  }
603  }
604 
605  // if we have a diff, try to more accuratelly determine the update type
606  if (scene.is_diff)
607  {
608  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
609  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
610  scene.link_scale.empty();
611  if (no_other_scene_upd)
612  {
613  upd = UPDATE_NONE;
614  if (!planning_scene::PlanningScene::isEmpty(scene.world))
615  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
616 
617  if (!scene.fixed_frame_transforms.empty())
618  upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
619 
620  if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
621  {
622  upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
623  if (!scene.robot_state.attached_collision_objects.empty() || !static_cast<bool>(scene.robot_state.is_diff))
624  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
625  }
626  }
627  }
629  return result;
630 }
631 
632 void PlanningSceneMonitor::newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world)
633 {
634  if (scene_)
635  {
637  {
638  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
640  scene_->getWorldNonConst()->clearObjects();
641  scene_->processPlanningSceneWorldMsg(*world);
642  if (octomap_monitor_)
643  {
644  if (world->octomap.octomap.data.empty())
645  {
646  octomap_monitor_->getOcTreePtr()->lockWrite();
647  octomap_monitor_->getOcTreePtr()->clear();
648  octomap_monitor_->getOcTreePtr()->unlockWrite();
649  }
650  }
651  }
653  }
654 }
655 
656 void PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj)
657 {
658  if (!scene_)
659  return;
660 
662  {
663  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
665  if (!scene_->processCollisionObjectMsg(*obj))
666  return;
667  }
669 }
670 
671 void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
672 {
673  if (scene_)
674  {
676  {
677  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
679  scene_->processAttachedCollisionObjectMsg(*obj);
680  }
682  }
683 }
684 
686 {
687  if (!octomap_monitor_)
688  return;
689 
690  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
691 
693  const std::vector<const robot_model::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
695  bool warned = false;
696  for (std::size_t i = 0; i < links.size(); ++i)
697  {
698  std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes(); // copy shared ptrs on purpuse
699  for (std::size_t j = 0; j < shapes.size(); ++j)
700  {
701  // merge mesh vertices up to 0.1 mm apart
702  if (shapes[j]->type == shapes::MESH)
703  {
704  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
705  m->mergeVertices(1e-4);
706  shapes[j].reset(m);
707  }
708 
709  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
710  if (h)
711  link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
712  }
713  if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
714  {
715  ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
716  warned = true;
717  }
718  }
719 }
720 
722 {
723  if (!octomap_monitor_)
724  return;
725 
726  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
727 
728  for (LinkShapeHandles::iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
729  for (std::size_t i = 0; i < it->second.size(); ++i)
730  octomap_monitor_->forgetShape(it->second[i].first);
731  link_shape_handles_.clear();
732 }
733 
735 {
736  if (!octomap_monitor_)
737  return;
738 
739  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
740 
741  // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
742  for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin();
743  it != attached_body_shape_handles_.end(); ++it)
744  for (std::size_t k = 0; k < it->second.size(); ++k)
745  octomap_monitor_->forgetShape(it->second[k].first);
747 }
748 
750 {
751  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
752 
754  // add attached objects again
755  std::vector<const robot_state::AttachedBody*> ab;
756  scene_->getCurrentState().getAttachedBodies(ab);
757  for (std::size_t i = 0; i < ab.size(); ++i)
759 }
760 
762 {
763  if (!octomap_monitor_)
764  return;
765 
766  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
767 
768  // clear information about any attached object
769  for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin();
770  it != collision_body_shape_handles_.end(); ++it)
771  for (std::size_t k = 0; k < it->second.size(); ++k)
772  octomap_monitor_->forgetShape(it->second[k].first);
774 }
775 
777 {
778  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
779 
781  for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end();
782  ++it)
783  excludeWorldObjectFromOctree(it->second);
784 }
785 
786 void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body)
787 {
788  if (!octomap_monitor_)
789  return;
790 
791  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
792  bool found = false;
793  for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
794  {
795  if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
796  continue;
797  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
798  if (h)
799  {
800  found = true;
801  attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
802  }
803  }
804  if (found)
805  ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
806 }
807 
808 void PlanningSceneMonitor::includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body)
809 {
810  if (!octomap_monitor_)
811  return;
812 
813  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
814 
815  AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
816  if (it != attached_body_shape_handles_.end())
817  {
818  for (std::size_t k = 0; k < it->second.size(); ++k)
819  octomap_monitor_->forgetShape(it->second[k].first);
820  ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
822  }
823 }
824 
825 void PlanningSceneMonitor::excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj)
826 {
827  if (!octomap_monitor_)
828  return;
829 
830  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
831 
832  bool found = false;
833  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
834  {
835  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
836  continue;
837  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
838  if (h)
839  {
840  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
841  found = true;
842  }
843  }
844  if (found)
845  ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
846 }
847 
848 void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj)
849 {
850  if (!octomap_monitor_)
851  return;
852 
853  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
854 
855  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
856  if (it != collision_body_shape_handles_.end())
857  {
858  for (std::size_t k = 0; k < it->second.size(); ++k)
859  octomap_monitor_->forgetShape(it->second[k].first);
860  ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
862  }
863 }
864 
865 void PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body,
866  bool just_attached)
867 {
868  if (!octomap_monitor_)
869  return;
870 
871  if (just_attached)
872  excludeAttachedBodyFromOctree(attached_body);
873  else
874  includeAttachedBodyInOctree(attached_body);
875 }
876 
877 void PlanningSceneMonitor::currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& obj,
879 {
880  if (!octomap_monitor_)
881  return;
883  return;
884 
887  else if (action & collision_detection::World::DESTROY)
889  else
890  {
893  }
894 }
895 
897 {
898  if (t.isZero())
899  return false;
901  ros::WallDuration timeout(wait_time);
902 
903  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
904 
906  {
907  // Wait for next robot update in state monitor.
908  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
909 
910  /* As robot updates are passed to the planning scene only in throttled fashion, there might
911  be still an update pending. If so, explicitly update the planning scene here.
912  If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
913  if (success)
914  {
915  boost::mutex::scoped_lock lock(state_pending_mutex_);
916  if (state_update_pending_) // enforce state update
917  {
918  state_update_pending_ = false;
920  lock.unlock();
922  }
923  return true;
924  }
925 
926  ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
927  return false;
928  }
929 
930  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
931  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
932  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
933  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
934  ros::Time prev_robot_motion_time = last_robot_motion_time_;
935  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
936  timeout > ros::WallDuration())
937  {
938  ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
939  new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
940  timeout -= ros::WallTime::now() - start; // compute remaining wait_time
941  }
942  bool success = last_robot_motion_time_ >= t;
943  // suppress warning if we received an update at all
944  if (!success && prev_robot_motion_time != last_robot_motion_time_)
945  ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
946  (t - last_robot_motion_time_).toSec());
947 
948  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
949  << " scene update: " << (t - last_update_time_).toSec());
950  return success;
951 }
952 
954 {
955  scene_update_mutex_.lock_shared();
956  if (octomap_monitor_)
957  octomap_monitor_->getOcTreePtr()->lockRead();
958 }
959 
961 {
962  if (octomap_monitor_)
963  octomap_monitor_->getOcTreePtr()->unlockRead();
964  scene_update_mutex_.unlock_shared();
965 }
966 
968 {
969  scene_update_mutex_.lock();
970  if (octomap_monitor_)
971  octomap_monitor_->getOcTreePtr()->lockWrite();
972 }
973 
975 {
976  if (octomap_monitor_)
977  octomap_monitor_->getOcTreePtr()->unlockWrite();
978  scene_update_mutex_.unlock();
979 }
980 
981 void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
982 {
984 
985  ROS_INFO_NAMED(LOGNAME, "Starting planning scene monitor");
986  // listen for planning scene updates; these messages include transforms, so no need for filters
987  if (!scene_topic.empty())
988  {
991  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
992  }
993 }
994 
996 {
998  {
999  ROS_INFO_NAMED(LOGNAME, "Stopping planning scene monitor");
1001  }
1002 }
1003 
1004 bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
1006 {
1007  if (!tf_buffer_)
1008  return false;
1009  try
1010  {
1011  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
1012 
1013  for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
1014  {
1015  tf_buffer_->canTransform(target_frame, it->first->getName(), target_time, shape_transform_cache_lookup_wait_time_);
1016  Eigen::Isometry3d ttr =
1017  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, it->first->getName(), target_time));
1018  for (std::size_t j = 0; j < it->second.size(); ++j)
1019  cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
1020  }
1021  for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin();
1022  it != attached_body_shape_handles_.end(); ++it)
1023  {
1024  tf_buffer_->canTransform(target_frame, it->first->getAttachedLinkName(), target_time,
1026  Eigen::Isometry3d transform = tf2::transformToEigen(
1027  tf_buffer_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time));
1028  for (std::size_t k = 0; k < it->second.size(); ++k)
1029  cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
1030  }
1031  {
1032  tf_buffer_->canTransform(target_frame, scene_->getPlanningFrame(), target_time,
1034  Eigen::Isometry3d transform =
1035  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time));
1036  for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin();
1037  it != collision_body_shape_handles_.end(); ++it)
1038  for (std::size_t k = 0; k < it->second.size(); ++k)
1039  cache[it->second[k].first] = transform * (*it->second[k].second);
1040  }
1041  }
1042  catch (tf2::TransformException& ex)
1043  {
1044  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
1045  return false;
1046  }
1047  return true;
1048 }
1049 
1050 void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collision_objects_topic,
1051  const std::string& planning_scene_world_topic,
1052  const bool load_octomap_monitor)
1053 {
1055  ROS_INFO_NAMED(LOGNAME, "Starting world geometry update monitor for collision objects, attached objects, octomap "
1056  "updates.");
1057 
1058  // Listen to the /collision_objects topic to detect requests to add/remove/update collision objects to/from the world
1059  if (!collision_objects_topic.empty())
1060  {
1062  root_nh_.subscribe(collision_objects_topic, 1024, &PlanningSceneMonitor::collisionObjectCallback, this);
1063  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
1064  }
1065 
1066  if (!planning_scene_world_topic.empty())
1067  {
1069  root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
1070  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
1071  root_nh_.resolveName(planning_scene_world_topic).c_str());
1072  }
1073 
1074  // Ocotomap monitor is optional
1075  if (load_octomap_monitor)
1076  {
1077  if (!octomap_monitor_)
1078  {
1083 
1084  octomap_monitor_->setTransformCacheCallback(
1085  boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
1086  octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
1087  }
1088  octomap_monitor_->startMonitor();
1089  }
1090 }
1091 
1093 {
1095  {
1096  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1098  }
1100  {
1101  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1103  }
1104  if (octomap_monitor_)
1105  octomap_monitor_->stopMonitor();
1106 }
1107 
1108 void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1109  const std::string& attached_objects_topic)
1110 {
1111  stopStateMonitor();
1112  if (scene_)
1113  {
1116  current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
1117  current_state_monitor_->startStateMonitor(joint_states_topic);
1118 
1119  {
1120  boost::mutex::scoped_lock lock(state_pending_mutex_);
1121  if (!dt_state_update_.isZero())
1123  }
1124 
1125  if (!attached_objects_topic.empty())
1126  {
1127  // using regular message filter as there's no header
1129  root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
1130  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
1131  root_nh_.resolveName(attached_objects_topic).c_str());
1132  }
1133  }
1134  else
1135  ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
1136 }
1137 
1139 {
1141  current_state_monitor_->stopStateMonitor();
1144 
1145  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1147  {
1148  boost::mutex::scoped_lock lock(state_pending_mutex_);
1149  state_update_pending_ = false;
1150  }
1151 }
1152 
1153 void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr& /* joint_state */)
1154 {
1155  const ros::WallTime& n = ros::WallTime::now();
1157 
1158  bool update = false;
1159  {
1160  boost::mutex::scoped_lock lock(state_pending_mutex_);
1161 
1162  if (dt < dt_state_update_)
1163  {
1164  state_update_pending_ = true;
1165  }
1166  else
1167  {
1168  state_update_pending_ = false;
1169  last_robot_state_update_wall_time_ = n;
1170  update = true;
1171  }
1172  }
1173  // run the state update with state_pending_mutex_ unlocked
1174  if (update)
1176 }
1177 
1179 {
1181  {
1182  bool update = false;
1183 
1184  const ros::WallTime& n = ros::WallTime::now();
1186 
1187  {
1188  // lock for access to dt_state_update_ and state_update_pending_
1189  boost::mutex::scoped_lock lock(state_pending_mutex_);
1191  {
1192  state_update_pending_ = false;
1193  last_robot_state_update_wall_time_ = ros::WallTime::now();
1194  update = true;
1195  ROS_DEBUG_STREAM_NAMED(LOGNAME,
1196  "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
1197  }
1198  }
1199 
1200  // run the state update with state_pending_mutex_ unlocked
1201  if (update)
1202  {
1204  ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
1205  }
1206  }
1207 }
1208 
1210 {
1211  if (!octomap_monitor_)
1212  return;
1213 
1215  {
1216  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1218  octomap_monitor_->getOcTreePtr()->lockRead();
1219  try
1220  {
1221  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Isometry3d::Identity());
1222  octomap_monitor_->getOcTreePtr()->unlockRead();
1223  }
1224  catch (...)
1225  {
1226  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1227  throw;
1228  }
1229  }
1231 }
1232 
1234 {
1235  bool update = false;
1236  if (hz > std::numeric_limits<double>::epsilon())
1237  {
1238  boost::mutex::scoped_lock lock(state_pending_mutex_);
1239  dt_state_update_.fromSec(1.0 / hz);
1242  }
1243  else
1244  {
1245  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1247  boost::mutex::scoped_lock lock(state_pending_mutex_);
1250  update = true;
1251  }
1252  ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
1253 
1254  if (update)
1256 }
1257 
1259 {
1261  {
1262  std::vector<std::string> missing;
1263  if (!current_state_monitor_->haveCompleteState(missing) &&
1264  (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
1265  {
1266  std::string missing_str = boost::algorithm::join(missing, ", ");
1267  ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known. Missing %s",
1268  missing_str.c_str());
1269  }
1270 
1271  {
1272  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1274  ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
1275  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1276  scene_->getCurrentStateNonConst().update(); // compute all transforms
1277  }
1279  }
1280  else
1281  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
1282 }
1283 
1284 void PlanningSceneMonitor::addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn)
1285 {
1286  boost::recursive_mutex::scoped_lock lock(update_lock_);
1287  if (fn)
1288  update_callbacks_.push_back(fn);
1289 }
1290 
1292 {
1293  boost::recursive_mutex::scoped_lock lock(update_lock_);
1294  update_callbacks_.clear();
1295 }
1296 
1298 {
1300  ROS_DEBUG_NAMED(LOGNAME, "Maximum frquency for publishing a planning scene is now %lf Hz",
1302 }
1303 
1304 void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms)
1305 {
1306  const std::string& target = getRobotModel()->getModelFrame();
1307 
1308  std::vector<std::string> all_frame_names;
1309  tf_buffer_->_getFrameStrings(all_frame_names);
1310  for (std::size_t i = 0; i < all_frame_names.size(); ++i)
1311  {
1312  if (all_frame_names[i] == target || getRobotModel()->hasLinkModel(all_frame_names[i]))
1313  continue;
1314 
1315  geometry_msgs::TransformStamped f;
1316  try
1317  {
1318  f = tf_buffer_->lookupTransform(target, all_frame_names[i], ros::Time(0));
1319  }
1320  catch (tf2::TransformException& ex)
1321  {
1322  ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
1323  << all_frame_names[i] << "' to planning frame '" << target << "' ("
1324  << ex.what() << ")");
1325  continue;
1326  }
1327  f.header.frame_id = all_frame_names[i];
1328  f.child_frame_id = target;
1329  transforms.push_back(f);
1330  }
1331 }
1332 
1334 {
1335  if (!tf_buffer_)
1336  return;
1337 
1338  if (scene_)
1339  {
1340  std::vector<geometry_msgs::TransformStamped> transforms;
1341  getUpdatedFrameTransforms(transforms);
1342  {
1343  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1344  scene_->getTransformsNonConst().setTransforms(transforms);
1346  }
1348  }
1349 }
1350 
1352 {
1353  if (octomap_monitor_)
1354  octomap_monitor_->publishDebugInformation(flag);
1355 }
1356 
1357 void PlanningSceneMonitor::configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene)
1358 {
1359  if (!scene || robot_description_.empty())
1360  return;
1361  collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
1362 
1363  // read overriding values from the param server
1364 
1365  // first we do default collision operations
1366  if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
1367  ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
1368  else
1369  {
1370  ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
1371 
1372  XmlRpc::XmlRpcValue coll_ops;
1373  nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
1374 
1375  if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
1376  {
1377  ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
1378  return;
1379  }
1380 
1381  if (coll_ops.size() == 0)
1382  {
1383  ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
1384  return;
1385  }
1386 
1387  for (int i = 0; i < coll_ops.size(); ++i)
1388  {
1389  if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
1390  {
1391  ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
1392  continue;
1393  }
1394  acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
1395  std::string(coll_ops[i]["operation"]) == "disable");
1396  }
1397  }
1398 }
1399 
1401 {
1402  if (robot_description_.empty())
1403  {
1404  default_robot_padd_ = 0.0;
1405  default_robot_scale_ = 1.0;
1406  default_object_padd_ = 0.0;
1407  default_attached_padd_ = 0.0;
1408  return;
1409  }
1410 
1411  // Ensure no leading slash creates a bad param server address
1412  const std::string robot_description =
1413  (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
1414 
1415  nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
1416  nh_.param(robot_description + "_planning/default_robot_scale", default_robot_scale_, 1.0);
1417  nh_.param(robot_description + "_planning/default_object_padding", default_object_padd_, 0.0);
1418  nh_.param(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
1419  nh_.param(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_,
1420  std::map<std::string, double>());
1421  nh_.param(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_,
1422  std::map<std::string, double>());
1423 
1424  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
1425  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
1426 }
1427 } // namespace planning_scene_monitor
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor (ROS topic-based)
boost::recursive_mutex update_lock_
lock access to update_callbacks_
dynamic_reconfigure::Server< PlanningSceneMonitorDynamicReconfigureConfig > dynamic_reconfigure_server_
void clearUpdateCallbacks()
Clear the functions to be called when an update to the scene is received.
std::string getTopic() const
#define ROS_INFO_NAMED(name,...)
void mergeVertices(double threshold)
void getUpdatedFrameTransforms(std::vector< geometry_msgs::TransformStamped > &transforms)
#define ROS_DEBUG_STREAM_NAMED(name, args)
ROSCPP_DECL void start()
#define ROS_WARN_THROTTLE_NAMED(period, name,...)
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
The geometry of the scene was updated. This includes receiving new octomaps, collision objects...
#define ROS_WARN_NAMED(name,...)
static const std::string LOGNAME
void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig &config, uint32_t level)
robot_model_loader::RobotModelLoaderPtr rm_loader_
f
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
void lockSceneWrite()
Lock the scene for writing (only one thread can lock for writing and no other thread can lock for rea...
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
static const std::string DEFAULT_PLANNING_SCENE_SERVICE
The name of the service used by default for requesting full planning scene state. ...
void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state)
void setPeriod(const WallDuration &period, bool reset=true)
ros::Time last_robot_motion_time_
Last time the state was updated.
void addUpdateCallback(const boost::function< void(SceneUpdateType)> &fn)
Add a function to be called when an update to the scene is received.
boost::shared_lock< boost::shared_mutex > ReadLock
Monitors the joint_states topic and tf to maintain the current state of the robot.
bool call(MReq &req, MRes &res)
PlanningSceneMonitor(const std::string &robot_description, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >(), const std::string &name="")
Constructor.
ros::Duration shape_transform_cache_lookup_wait_time_
the amount of time to wait when looking up transforms
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
void octomapUpdateCallback()
Callback for octomap updates.
const robot_model::RobotModelConstPtr & getRobotModel() const
void configureDefaultPadding()
Configure the default padding.
std::shared_ptr< ros::AsyncSpinner > spinner_
void triggerSceneUpdateEvent(SceneUpdateType update_type)
This function is called every time there is a change to the planning scene.
void stopPublishingPlanningScene()
Stop publishing the maintained planning scene.
void lockSceneRead()
Lock the scene for reading (multiple threads can lock for reading at the same time) ...
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::map< std::string, ObjectPtr >::const_iterator const_iterator
static const std::string OCTOMAP_NS
geometry_msgs::TransformStamped t
bool updatesScene(const planning_scene::PlanningSceneConstPtr &scene) const
Return true if the scene scene can be updated directly or indirectly by this monitor. This function will return true if the pointer of the scene is the same as the one maintained, or if a parent of the scene is the one maintained.
std::map< ShapeHandle, Eigen::Isometry3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Isometry3d > > > ShapeTransformCache
void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr &obj)
Callback for a new collision object msg.
void unlockSceneWrite()
Lock the scene from writing (only one thread can lock for writing and no other thread can lock for re...
The state in the monitored scene was updated.
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
void setStateUpdateFrequency(double hz)
Update the scene using the monitored state at a specified frequency, in Hz. This function has an effe...
bool getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request &req, moveit_msgs::GetPlanningScene::Response &res)
void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody *attached_body, bool just_attached)
Callback for a change for an attached object of the current state of the planning scene...
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
void excludeAttachedBodyFromOctree(const robot_state::AttachedBody *attached_body)
#define ROS_DEBUG_NAMED(name,...)
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
void setupScene(ros::NodeHandle &nh, const planning_scene::PlanningScenePtr &scene)
This can be called on a new planning scene to setup the collision detector.
unsigned int ShapeHandle
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Type const & getType() const
void publish(const boost::shared_ptr< M > &message) const
std::unique_ptr< boost::thread > publish_planning_scene_
void updateFrameTransforms()
Update the transforms for the frames that are not part of the kinematic model using tf...
ros::WallTime last_robot_state_update_wall_time_
Last time the state was updated from current_state_monitor_.
#define ROS_ERROR_THROTTLE_NAMED(period, name,...)
void startWorldGeometryMonitor(const std::string &collision_objects_topic=DEFAULT_COLLISION_OBJECT_TOPIC, const std::string &planning_scene_world_topic=DEFAULT_PLANNING_SCENE_WORLD_TOPIC, const bool load_octomap_monitor=true)
Start the OccupancyMapMonitor and listening for:
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
WallDuration & fromSec(double t)
#define ROS_FATAL_STREAM_NAMED(name, args)
std::unique_ptr< occupancy_map_monitor::OccupancyMapMonitor > octomap_monitor_
bool getParam(const std::string &key, std::string &s) const
PlanningSceneMonitor Subscribes to the topic planning_scene.
std::map< std::string, double > default_robot_link_scale_
default robot link scale
void setCallbackQueue(CallbackQueueInterface *queue)
bool waitForCurrentRobotState(const ros::Time &t, double wait_time=1.)
Wait for robot state to become more recent than time t.
ros::NodeHandle nh_
Last time the robot has moved.
void initialize(const planning_scene::PlanningScenePtr &scene)
Initialize the planning scene monitor.
static const std::string DEFAULT_PLANNING_SCENE_TOPIC
The name of the topic used by default for receiving full planning scenes or planning scene diffs...
void updateSceneWithCurrentState()
Update the scene using the monitored state. This function is automatically called when an update to t...
void getMonitoredTopics(std::vector< std::string > &topics) const
Get the topic names that the monitor is listening to.
ros::WallTimer state_update_timer_
timer for state updates.
std::string resolveName(const std::string &name, bool remap=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
CallbackQueueInterface * getCallbackQueue() const
void startStateMonitor(const std::string &joint_states_topic=DEFAULT_JOINT_STATES_TOPIC, const std::string &attached_objects_topic=DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC)
Start the current state monitor.
std::string getService() const
void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene)
Configure the collision matrix for a particular scene.
boost::condition_variable_any new_scene_update_condition_
void startPublishingPlanningScene(SceneUpdateType event, const std::string &planning_scene_topic=MONITORED_PLANNING_SCENE_TOPIC)
Start publishing the maintained planning scene. The first message set out is a complete planning scen...
bool hasParam(const std::string &key) const
bool sleep()
void providePlanningSceneService(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
Create an optional service for getting the complete planning scene This is useful for satisfying the ...
void unlockSceneRead()
Unlock the scene from reading (multiple threads can lock for reading at the same time) ...
void stateUpdateTimerCallback(const ros::WallTimerEvent &event)
ros::Time last_update_time_
mutex for stored scene
static const std::string DEFAULT_COLLISION_OBJECT_TOPIC
The name of the topic used by default for receiving collision objects in the world.
std::vector< boost::function< void(SceneUpdateType)> > update_callbacks_
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
static WallTime now()
boost::shared_mutex scene_update_mutex_
if diffs are monitored, this is the pointer to the parent scene
void monitorDiffs(bool flag)
By default, the maintained planning scene does not reason about diffs. When the flag passed in is tru...
planning_scene::PlanningScenePtr parent_scene_
void stopWorldGeometryMonitor()
Stop the world geometry monitor.
void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj)
bool requestPlanningSceneState(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
Request a full planning scene state using a service call Be careful not to use this in conjunction wi...
#define ROS_ERROR_NAMED(name,...)
void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr &obj)
The maintained set of fixed transforms in the monitored scene was updated.
static Time now()
double default_attached_padd_
default attached padding
std::string monitor_name_
The name of this scene monitor.
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
bool newPlanningSceneMessage(const moveit_msgs::PlanningScene &scene)
void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr &object, collision_detection::World::Action action)
Callback for a change in the world maintained by the planning scene.
void setPlanningScenePublishingFrequency(double hz)
Set the maximum frequency at which planning scenes are being published.
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
The name of the topic used by default for attached collision objects.
std::map< std::string, double > default_robot_link_padd_
default robot link padding
volatile bool state_update_pending_
True when we need to update the RobotState from current_state_monitor_.
void includeAttachedBodyInOctree(const robot_state::AttachedBody *attached_body)
void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene)
void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr &obj)
Callback for a new attached object msg.
planning_scene::PlanningSceneConstPtr scene_const_
bool hasMember(const std::string &name) const
void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world)
Callback for a new planning scene world.
static const std::string DEFAULT_JOINT_STATES_TOPIC
The name of the topic used by default for receiving joint states.
ros::WallDuration dt_state_update_
the amount of time to wait in between updates to the robot state
collision_detection::CollisionPluginLoader collision_loader_
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
bool getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const
#define ROS_WARN_STREAM_NAMED(name, args)


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Fri Oct 23 2020 03:58:16