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  }
375  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the
376  // transform cache to
377  // update while we are
378  // potentially changing
379  // attached bodies
380  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
381  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
382  scene_->pushDiffs(parent_scene_);
383  scene_->clearDiffs();
384  scene_->setAttachedBodyUpdateCallback(
386  scene_->setCollisionObjectUpdateCallback(
387  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
388  if (octomap_monitor_)
389  {
390  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
391  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
392  }
393  if (is_full)
394  {
396  if (octomap_monitor_)
397  lock = octomap_monitor_->getOcTreePtr()->reading();
398  scene_->getPlanningSceneMsg(msg);
399  }
400  // also publish timestamp of this robot_state
401  msg.robot_state.joint_state.header.stamp = last_robot_motion_time_;
402  publish_msg = true;
403  }
404  new_scene_update_ = UPDATE_NONE;
405  }
406  }
407  if (publish_msg)
408  {
409  rate.reset();
411  if (is_full)
412  ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str());
413  rate.sleep();
414  }
415  } while (publish_planning_scene_);
416 }
417 
418 void PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string>& topics) const
419 {
420  topics.clear();
422  {
423  const std::string& t = current_state_monitor_->getMonitoredTopic();
424  if (!t.empty())
425  topics.push_back(t);
426  }
428  topics.push_back(planning_scene_subscriber_.getTopic());
430  topics.push_back(collision_object_subscriber_.getTopic());
432  topics.push_back(planning_scene_world_subscriber_.getTopic());
433 }
434 
435 namespace
436 {
437 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr& scene,
438  const planning_scene::PlanningScene* possible_parent)
439 {
440  if (scene && scene.get() == possible_parent)
441  return true;
442  if (scene)
443  return sceneIsParentOf(scene->getParent(), possible_parent);
444  return false;
445 }
446 } // namespace
447 
448 bool PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr& scene) const
449 {
450  return sceneIsParentOf(scene_const_, scene.get());
451 }
452 
453 bool PlanningSceneMonitor::updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const
454 {
455  return sceneIsParentOf(scene_const_, scene.get());
456 }
457 
459 {
460  // do not modify update functions while we are calling them
461  boost::recursive_mutex::scoped_lock lock(update_lock_);
462 
463  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
464  update_callbacks_[i](update_type);
465  new_scene_update_ = (SceneUpdateType)((int)new_scene_update_ | (int)update_type);
466  new_scene_update_condition_.notify_all();
467 }
468 
469 bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_name)
470 {
471  if (get_scene_service_.getService() == service_name)
472  {
473  ROS_FATAL_STREAM_NAMED(LOGNAME, "requestPlanningSceneState() to self-provided service '" << service_name << "'");
474  throw std::runtime_error("requestPlanningSceneState() to self-provided service: " + service_name);
475  }
476  // use global namespace for service
477  ros::ServiceClient client = ros::NodeHandle().serviceClient<moveit_msgs::GetPlanningScene>(service_name);
478  moveit_msgs::GetPlanningScene srv;
479  srv.request.components.components =
480  srv.request.components.SCENE_SETTINGS | srv.request.components.ROBOT_STATE |
481  srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS | srv.request.components.WORLD_OBJECT_NAMES |
482  srv.request.components.WORLD_OBJECT_GEOMETRY | srv.request.components.OCTOMAP |
483  srv.request.components.TRANSFORMS | srv.request.components.ALLOWED_COLLISION_MATRIX |
484  srv.request.components.LINK_PADDING_AND_SCALING | srv.request.components.OBJECT_COLORS;
485 
486  // Make sure client is connected to server
487  if (!client.exists())
488  {
489  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for service `" << service_name << "` to exist.");
490  client.waitForExistence(ros::Duration(5.0));
491  }
492 
493  if (client.call(srv))
494  {
495  newPlanningSceneMessage(srv.response.scene);
496  }
497  else
498  {
500  LOGNAME, "Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?",
501  service_name.c_str());
502  return false;
503  }
504  return true;
505 }
506 
507 void PlanningSceneMonitor::providePlanningSceneService(const std::string& service_name)
508 {
509  // Load the service
512 }
513 
514 bool PlanningSceneMonitor::getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req,
515  moveit_msgs::GetPlanningScene::Response& res)
516 {
517  if (req.components.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
519 
520  moveit_msgs::PlanningSceneComponents all_components;
521  all_components.components = UINT_MAX; // Return all scene components if nothing is specified.
522 
523  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
524  scene_->getPlanningSceneMsg(res.scene, req.components.components ? req.components : all_components);
525 
526  return true;
527 }
528 
529 void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene)
530 {
531  newPlanningSceneMessage(*scene);
532 }
533 
535 {
536  octomap_monitor_->getOcTreePtr()->lockWrite();
537  octomap_monitor_->getOcTreePtr()->clear();
538  octomap_monitor_->getOcTreePtr()->unlockWrite();
539 }
540 
541 bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
542 {
543  if (!scene_)
544  return false;
545 
546  bool result;
547 
549  std::string old_scene_name;
550  {
551  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
552  // we don't want the transform cache to update while we are potentially changing attached bodies
553  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
554 
556  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
557  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
558  "scene update " << fmod(last_update_time_.toSec(), 10.)
559  << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
560  old_scene_name = scene_->getName();
561  result = scene_->usePlanningSceneMsg(scene);
562  if (octomap_monitor_)
563  {
564  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
565  {
566  octomap_monitor_->getOcTreePtr()->lockWrite();
567  octomap_monitor_->getOcTreePtr()->clear();
568  octomap_monitor_->getOcTreePtr()->unlockWrite();
569  }
570  }
571  robot_model_ = scene_->getRobotModel();
572 
573  // if we just reset the scene completely but we were maintaining diffs, we need to fix that
574  if (!scene.is_diff && parent_scene_)
575  {
576  // the scene is now decoupled from the parent, since we just reset it
577  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
578  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
580  scene_ = parent_scene_->diff();
582  scene_->setAttachedBodyUpdateCallback(
584  scene_->setCollisionObjectUpdateCallback(
585  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
586  }
587  if (octomap_monitor_)
588  {
589  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
590  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
591  }
592  }
593 
594  // if we have a diff, try to more accuratelly determine the update type
595  if (scene.is_diff)
596  {
597  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
598  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
599  scene.link_scale.empty();
600  if (no_other_scene_upd)
601  {
602  upd = UPDATE_NONE;
603  if (!planning_scene::PlanningScene::isEmpty(scene.world))
604  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
605 
606  if (!scene.fixed_frame_transforms.empty())
607  upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
608 
609  if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
610  {
611  upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
612  if (!scene.robot_state.attached_collision_objects.empty() || !static_cast<bool>(scene.robot_state.is_diff))
613  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
614  }
615  }
616  }
618  return result;
619 }
620 
621 void PlanningSceneMonitor::newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world)
622 {
623  if (scene_)
624  {
626  {
627  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
629  scene_->getWorldNonConst()->clearObjects();
630  scene_->processPlanningSceneWorldMsg(*world);
631  if (octomap_monitor_)
632  {
633  if (world->octomap.octomap.data.empty())
634  {
635  octomap_monitor_->getOcTreePtr()->lockWrite();
636  octomap_monitor_->getOcTreePtr()->clear();
637  octomap_monitor_->getOcTreePtr()->unlockWrite();
638  }
639  }
640  }
642  }
643 }
644 
645 void PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj)
646 {
647  if (!scene_)
648  return;
649 
651  {
652  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
654  if (!scene_->processCollisionObjectMsg(*obj))
655  return;
656  }
658 }
659 
660 void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
661 {
662  if (scene_)
663  {
665  {
666  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
668  scene_->processAttachedCollisionObjectMsg(*obj);
669  }
671  }
672 }
673 
675 {
676  if (!octomap_monitor_)
677  return;
678 
679  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
680 
682  const std::vector<const robot_model::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
684  bool warned = false;
685  for (std::size_t i = 0; i < links.size(); ++i)
686  {
687  std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes(); // copy shared ptrs on purpuse
688  for (std::size_t j = 0; j < shapes.size(); ++j)
689  {
690  // merge mesh vertices up to 0.1 mm apart
691  if (shapes[j]->type == shapes::MESH)
692  {
693  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
694  m->mergeVertices(1e-4);
695  shapes[j].reset(m);
696  }
697 
698  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
699  if (h)
700  link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
701  }
702  if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
703  {
704  ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
705  warned = true;
706  }
707  }
708 }
709 
711 {
712  if (!octomap_monitor_)
713  return;
714 
715  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
716 
717  for (LinkShapeHandles::iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
718  for (std::size_t i = 0; i < it->second.size(); ++i)
719  octomap_monitor_->forgetShape(it->second[i].first);
720  link_shape_handles_.clear();
721 }
722 
724 {
725  if (!octomap_monitor_)
726  return;
727 
728  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
729 
730  // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
731  for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin();
732  it != attached_body_shape_handles_.end(); ++it)
733  for (std::size_t k = 0; k < it->second.size(); ++k)
734  octomap_monitor_->forgetShape(it->second[k].first);
736 }
737 
739 {
740  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
741 
743  // add attached objects again
744  std::vector<const robot_state::AttachedBody*> ab;
745  scene_->getCurrentState().getAttachedBodies(ab);
746  for (std::size_t i = 0; i < ab.size(); ++i)
748 }
749 
751 {
752  if (!octomap_monitor_)
753  return;
754 
755  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
756 
757  // clear information about any attached object
758  for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin();
759  it != collision_body_shape_handles_.end(); ++it)
760  for (std::size_t k = 0; k < it->second.size(); ++k)
761  octomap_monitor_->forgetShape(it->second[k].first);
763 }
764 
766 {
767  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
768 
770  for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end();
771  ++it)
772  excludeWorldObjectFromOctree(it->second);
773 }
774 
775 void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body)
776 {
777  if (!octomap_monitor_)
778  return;
779 
780  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
781  bool found = false;
782  for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
783  {
784  if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
785  continue;
786  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
787  if (h)
788  {
789  found = true;
790  attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
791  }
792  }
793  if (found)
794  ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
795 }
796 
797 void PlanningSceneMonitor::includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body)
798 {
799  if (!octomap_monitor_)
800  return;
801 
802  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
803 
804  AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
805  if (it != attached_body_shape_handles_.end())
806  {
807  for (std::size_t k = 0; k < it->second.size(); ++k)
808  octomap_monitor_->forgetShape(it->second[k].first);
809  ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
811  }
812 }
813 
814 void PlanningSceneMonitor::excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj)
815 {
816  if (!octomap_monitor_)
817  return;
818 
819  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
820 
821  bool found = false;
822  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
823  {
824  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
825  continue;
826  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
827  if (h)
828  {
829  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
830  found = true;
831  }
832  }
833  if (found)
834  ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
835 }
836 
837 void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj)
838 {
839  if (!octomap_monitor_)
840  return;
841 
842  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
843 
844  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
845  if (it != collision_body_shape_handles_.end())
846  {
847  for (std::size_t k = 0; k < it->second.size(); ++k)
848  octomap_monitor_->forgetShape(it->second[k].first);
849  ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
851  }
852 }
853 
854 void PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body,
855  bool just_attached)
856 {
857  if (!octomap_monitor_)
858  return;
859 
860  if (just_attached)
861  excludeAttachedBodyFromOctree(attached_body);
862  else
863  includeAttachedBodyInOctree(attached_body);
864 }
865 
866 void PlanningSceneMonitor::currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& obj,
868 {
869  if (!octomap_monitor_)
870  return;
872  return;
873 
876  else if (action & collision_detection::World::DESTROY)
878  else
879  {
882  }
883 }
884 
886 {
887  if (t.isZero())
888  return false;
890  ros::WallDuration timeout(wait_time);
891 
892  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
893 
895  {
896  // Wait for next robot update in state monitor.
897  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
898 
899  /* As robot updates are passed to the planning scene only in throttled fashion, there might
900  be still an update pending. If so, explicitly update the planning scene here.
901  If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
902  if (success)
903  {
904  boost::mutex::scoped_lock lock(state_pending_mutex_);
905  if (state_update_pending_) // enforce state update
906  {
907  state_update_pending_ = false;
909  lock.unlock();
911  }
912  return true;
913  }
914 
915  ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
916  return false;
917  }
918 
919  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
920  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
921  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
922  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
923  ros::Time prev_robot_motion_time = last_robot_motion_time_;
924  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
925  timeout > ros::WallDuration())
926  {
927  ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
928  new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
929  timeout -= ros::WallTime::now() - start; // compute remaining wait_time
930  }
931  bool success = last_robot_motion_time_ >= t;
932  // suppress warning if we received an update at all
933  if (!success && prev_robot_motion_time != last_robot_motion_time_)
934  ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
935  (t - last_robot_motion_time_).toSec());
936 
937  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
938  << " scene update: " << (t - last_update_time_).toSec());
939  return success;
940 }
941 
943 {
944  scene_update_mutex_.lock_shared();
945  if (octomap_monitor_)
946  octomap_monitor_->getOcTreePtr()->lockRead();
947 }
948 
950 {
951  if (octomap_monitor_)
952  octomap_monitor_->getOcTreePtr()->unlockRead();
953  scene_update_mutex_.unlock_shared();
954 }
955 
957 {
958  scene_update_mutex_.lock();
959  if (octomap_monitor_)
960  octomap_monitor_->getOcTreePtr()->lockWrite();
961 }
962 
964 {
965  if (octomap_monitor_)
966  octomap_monitor_->getOcTreePtr()->unlockWrite();
967  scene_update_mutex_.unlock();
968 }
969 
970 void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
971 {
973 
974  ROS_INFO_NAMED(LOGNAME, "Starting planning scene monitor");
975  // listen for planning scene updates; these messages include transforms, so no need for filters
976  if (!scene_topic.empty())
977  {
980  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
981  }
982 }
983 
985 {
987  {
988  ROS_INFO_NAMED(LOGNAME, "Stopping planning scene monitor");
990  }
991 }
992 
993 bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
995 {
996  if (!tf_buffer_)
997  return false;
998  try
999  {
1000  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
1001 
1002  for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
1003  {
1004  tf_buffer_->canTransform(target_frame, it->first->getName(), target_time,
1006  Eigen::Isometry3d ttr =
1007  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, it->first->getName(), target_time));
1008  for (std::size_t j = 0; j < it->second.size(); ++j)
1009  cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
1010  }
1011  for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin();
1012  it != attached_body_shape_handles_.end(); ++it)
1013  {
1014  tf_buffer_->canTransform(target_frame, it->first->getAttachedLinkName(), target_time,
1016  Eigen::Isometry3d transform = tf2::transformToEigen(
1017  tf_buffer_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time));
1018  for (std::size_t k = 0; k < it->second.size(); ++k)
1019  cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
1020  }
1021  {
1022  tf_buffer_->canTransform(target_frame, scene_->getPlanningFrame(), target_time,
1024  Eigen::Isometry3d transform =
1025  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time));
1026  for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin();
1027  it != collision_body_shape_handles_.end(); ++it)
1028  for (std::size_t k = 0; k < it->second.size(); ++k)
1029  cache[it->second[k].first] = transform * (*it->second[k].second);
1030  }
1031  }
1032  catch (tf2::TransformException& ex)
1033  {
1034  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
1035  return false;
1036  }
1037  return true;
1038 }
1039 
1040 void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collision_objects_topic,
1041  const std::string& planning_scene_world_topic,
1042  const bool load_octomap_monitor)
1043 {
1045  ROS_INFO_NAMED(LOGNAME, "Starting world geometry update monitor for collision objects, attached objects, octomap "
1046  "updates.");
1047 
1048  // Listen to the /collision_objects topic to detect requests to add/remove/update collision objects to/from the world
1049  if (!collision_objects_topic.empty())
1050  {
1052  root_nh_.subscribe(collision_objects_topic, 1024, &PlanningSceneMonitor::collisionObjectCallback, this);
1053  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
1054  }
1055 
1056  if (!planning_scene_world_topic.empty())
1057  {
1059  root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
1060  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
1061  root_nh_.resolveName(planning_scene_world_topic).c_str());
1062  }
1063 
1064  // Ocotomap monitor is optional
1065  if (load_octomap_monitor)
1066  {
1067  if (!octomap_monitor_)
1068  {
1073 
1074  octomap_monitor_->setTransformCacheCallback(
1075  boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
1076  octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
1077  }
1078  octomap_monitor_->startMonitor();
1079  }
1080 }
1081 
1083 {
1085  {
1086  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1088  }
1090  {
1091  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1093  }
1094  if (octomap_monitor_)
1095  octomap_monitor_->stopMonitor();
1096 }
1097 
1098 void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1099  const std::string& attached_objects_topic)
1100 {
1101  stopStateMonitor();
1102  if (scene_)
1103  {
1106  current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
1107  current_state_monitor_->startStateMonitor(joint_states_topic);
1108 
1109  {
1110  boost::mutex::scoped_lock lock(state_pending_mutex_);
1111  if (!dt_state_update_.isZero())
1113  }
1114 
1115  if (!attached_objects_topic.empty())
1116  {
1117  // using regular message filter as there's no header
1119  root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
1120  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
1121  root_nh_.resolveName(attached_objects_topic).c_str());
1122  }
1123  }
1124  else
1125  ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
1126 }
1127 
1129 {
1131  current_state_monitor_->stopStateMonitor();
1134 
1135  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1137  {
1138  boost::mutex::scoped_lock lock(state_pending_mutex_);
1139  state_update_pending_ = false;
1140  }
1141 }
1142 
1143 void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr& /* joint_state */)
1144 {
1145  const ros::WallTime& n = ros::WallTime::now();
1147 
1148  bool update = false;
1149  {
1150  boost::mutex::scoped_lock lock(state_pending_mutex_);
1151 
1152  if (dt < dt_state_update_)
1153  {
1154  state_update_pending_ = true;
1155  }
1156  else
1157  {
1158  state_update_pending_ = false;
1159  last_robot_state_update_wall_time_ = n;
1160  update = true;
1161  }
1162  }
1163  // run the state update with state_pending_mutex_ unlocked
1164  if (update)
1166 }
1167 
1169 {
1171  {
1172  bool update = false;
1173 
1174  const ros::WallTime& n = ros::WallTime::now();
1176 
1177  {
1178  // lock for access to dt_state_update_ and state_update_pending_
1179  boost::mutex::scoped_lock lock(state_pending_mutex_);
1181  {
1182  state_update_pending_ = false;
1183  last_robot_state_update_wall_time_ = ros::WallTime::now();
1184  update = true;
1185  ROS_DEBUG_STREAM_NAMED(LOGNAME,
1186  "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
1187  }
1188  }
1189 
1190  // run the state update with state_pending_mutex_ unlocked
1191  if (update)
1192  {
1194  ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
1195  }
1196  }
1197 }
1198 
1200 {
1201  if (!octomap_monitor_)
1202  return;
1203 
1205  {
1206  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1208  octomap_monitor_->getOcTreePtr()->lockRead();
1209  try
1210  {
1211  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Isometry3d::Identity());
1212  octomap_monitor_->getOcTreePtr()->unlockRead();
1213  }
1214  catch (...)
1215  {
1216  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1217  throw;
1218  }
1219  }
1221 }
1222 
1224 {
1225  bool update = false;
1226  if (hz > std::numeric_limits<double>::epsilon())
1227  {
1228  boost::mutex::scoped_lock lock(state_pending_mutex_);
1229  dt_state_update_.fromSec(1.0 / hz);
1232  }
1233  else
1234  {
1235  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1237  boost::mutex::scoped_lock lock(state_pending_mutex_);
1240  update = true;
1241  }
1242  ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
1243 
1244  if (update)
1246 }
1247 
1249 {
1251  {
1252  std::vector<std::string> missing;
1253  if (!current_state_monitor_->haveCompleteState(missing) &&
1254  (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
1255  {
1256  std::string missing_str = boost::algorithm::join(missing, ", ");
1257  ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known. Missing %s",
1258  missing_str.c_str());
1259  }
1260 
1261  {
1262  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1264  ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
1265  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1266  scene_->getCurrentStateNonConst().update(); // compute all transforms
1267  }
1269  }
1270  else
1271  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
1272 }
1273 
1274 void PlanningSceneMonitor::addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn)
1275 {
1276  boost::recursive_mutex::scoped_lock lock(update_lock_);
1277  if (fn)
1278  update_callbacks_.push_back(fn);
1279 }
1280 
1282 {
1283  boost::recursive_mutex::scoped_lock lock(update_lock_);
1284  update_callbacks_.clear();
1285 }
1286 
1288 {
1290  ROS_DEBUG_NAMED(LOGNAME, "Maximum frquency for publishing a planning scene is now %lf Hz",
1292 }
1293 
1294 void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms)
1295 {
1296  const std::string& target = getRobotModel()->getModelFrame();
1297 
1298  std::vector<std::string> all_frame_names;
1299  tf_buffer_->_getFrameStrings(all_frame_names);
1300  for (std::size_t i = 0; i < all_frame_names.size(); ++i)
1301  {
1302  if (all_frame_names[i] == target || getRobotModel()->hasLinkModel(all_frame_names[i]))
1303  continue;
1304 
1305  geometry_msgs::TransformStamped f;
1306  try
1307  {
1308  f = tf_buffer_->lookupTransform(target, all_frame_names[i], ros::Time(0));
1309  }
1310  catch (tf2::TransformException& ex)
1311  {
1312  ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
1313  << all_frame_names[i] << "' to planning frame '" << target << "' ("
1314  << ex.what() << ")");
1315  continue;
1316  }
1317  f.header.frame_id = all_frame_names[i];
1318  f.child_frame_id = target;
1319  transforms.push_back(f);
1320  }
1321 }
1322 
1324 {
1325  if (!tf_buffer_)
1326  return;
1327 
1328  if (scene_)
1329  {
1330  std::vector<geometry_msgs::TransformStamped> transforms;
1331  getUpdatedFrameTransforms(transforms);
1332  {
1333  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1334  scene_->getTransformsNonConst().setTransforms(transforms);
1336  }
1338  }
1339 }
1340 
1342 {
1343  if (octomap_monitor_)
1344  octomap_monitor_->publishDebugInformation(flag);
1345 }
1346 
1347 void PlanningSceneMonitor::configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene)
1348 {
1349  if (!scene || robot_description_.empty())
1350  return;
1351  collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
1352 
1353  // read overriding values from the param server
1354 
1355  // first we do default collision operations
1356  if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
1357  ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
1358  else
1359  {
1360  ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
1361 
1362  XmlRpc::XmlRpcValue coll_ops;
1363  nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
1364 
1365  if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
1366  {
1367  ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
1368  return;
1369  }
1370 
1371  if (coll_ops.size() == 0)
1372  {
1373  ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
1374  return;
1375  }
1376 
1377  for (int i = 0; i < coll_ops.size(); ++i)
1378  {
1379  if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
1380  {
1381  ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
1382  continue;
1383  }
1384  acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
1385  std::string(coll_ops[i]["operation"]) == "disable");
1386  }
1387  }
1388 }
1389 
1391 {
1392  if (robot_description_.empty())
1393  {
1394  default_robot_padd_ = 0.0;
1395  default_robot_scale_ = 1.0;
1396  default_object_padd_ = 0.0;
1397  default_attached_padd_ = 0.0;
1398  return;
1399  }
1400 
1401  // Ensure no leading slash creates a bad param server address
1402  const std::string robot_description =
1403  (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
1404 
1405  nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
1406  nh_.param(robot_description + "_planning/default_robot_scale", default_robot_scale_, 1.0);
1407  nh_.param(robot_description + "_planning/default_object_padding", default_object_padd_, 0.0);
1408  nh_.param(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
1409  nh_.param(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_,
1410  std::map<std::string, double>());
1411  nh_.param(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_,
1412  std::map<std::string, double>());
1413 
1414  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
1415  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
1416 }
1417 } // 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 reset()
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 Apr 2 2020 03:50:53