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>
46 
47 #include <memory>
48 
49 namespace planning_scene_monitor
50 {
51 using namespace moveit_ros_planning;
52 
53 static const std::string LOGNAME = "planning_scene_monitor";
54 
56 {
57 public:
59  : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle(decideNamespace(owner->getName())))
60  {
61  dynamic_reconfigure_server_.setCallback(
62  boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
63  }
64 
65 private:
66  // make sure we do not advertise the same service multiple times, in case we use multiple PlanningSceneMonitor
67  // instances in a process
68  static std::string decideNamespace(const std::string& name)
69  {
70  std::string ns = "~/" + name;
71  std::replace(ns.begin(), ns.end(), ' ', '_');
72  std::transform(ns.begin(), ns.end(), ns.begin(), ::tolower);
73  if (ros::service::exists(ns + "/set_parameters", false))
74  {
75  unsigned int c = 1;
76  while (ros::service::exists(ns + boost::lexical_cast<std::string>(c) + "/set_parameters", false))
77  c++;
78  ns += boost::lexical_cast<std::string>(c);
79  }
80  return ns;
81  }
82 
83  void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig& config, uint32_t level)
84  {
86  if (config.publish_geometry_updates)
88  if (config.publish_state_updates)
90  if (config.publish_transforms_updates)
92  if (config.publish_planning_scene)
93  {
94  owner_->setPlanningScenePublishingFrequency(config.publish_planning_scene_hz);
95  owner_->startPublishingPlanningScene(event);
96  }
97  else
98  owner_->stopPublishingPlanningScene();
99  }
100 
102  dynamic_reconfigure::Server<PlanningSceneMonitorDynamicReconfigureConfig> dynamic_reconfigure_server_;
103 };
104 
105 const std::string PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC = "joint_states";
106 const std::string PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC = "attached_collision_object";
107 const std::string PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC = "collision_object";
108 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC = "planning_scene_world";
109 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC = "planning_scene";
110 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE = "get_planning_scene";
111 const std::string PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC = "monitored_planning_scene";
112 
113 PlanningSceneMonitor::PlanningSceneMonitor(const std::string& robot_description,
114  const boost::shared_ptr<tf::Transformer>& tf, const std::string& name)
115  : PlanningSceneMonitor(planning_scene::PlanningScenePtr(), robot_description, tf, name)
116 {
117 }
118 
119 PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
120  const std::string& robot_description,
121  const boost::shared_ptr<tf::Transformer>& tf, const std::string& name)
122  : PlanningSceneMonitor(scene, std::make_shared<robot_model_loader::RobotModelLoader>(robot_description), tf, name)
123 {
124 }
125 
126 PlanningSceneMonitor::PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr& rm_loader,
127  const boost::shared_ptr<tf::Transformer>& tf, const std::string& name)
128  : PlanningSceneMonitor(planning_scene::PlanningScenePtr(), rm_loader, tf, name)
129 {
130 }
131 
132 PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
133  const robot_model_loader::RobotModelLoaderPtr& rm_loader,
134  const boost::shared_ptr<tf::Transformer>& tf, const std::string& name)
135  : monitor_name_(name), nh_("~"), tf_(tf), rm_loader_(rm_loader)
136 {
139  spinner_.reset(new ros::AsyncSpinner(1, &queue_));
140  spinner_->start();
141  initialize(scene);
142 }
143 
144 PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
145  const robot_model_loader::RobotModelLoaderPtr& rm_loader,
147  const std::string& name)
148  : monitor_name_(name), nh_("~"), root_nh_(nh), tf_(tf), rm_loader_(rm_loader)
149 {
150  // use same callback queue as root_nh_
152  initialize(scene);
153 }
154 
156 {
157  if (scene_)
158  {
159  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
160  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
161  }
166 
167  spinner_.reset();
168  delete reconfigure_impl_;
169  current_state_monitor_.reset();
170  scene_const_.reset();
171  scene_.reset();
172  parent_scene_.reset();
173  robot_model_.reset();
174  rm_loader_.reset();
175 }
176 
177 void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& scene)
178 {
180  moveit::tools::Profiler::ScopedBlock prof_block("PlanningSceneMonitor::initialize");
181 
182  if (monitor_name_.empty())
183  monitor_name_ = "planning_scene_monitor";
184  robot_description_ = rm_loader_->getRobotDescription();
185  if (rm_loader_->getModel())
186  {
187  robot_model_ = rm_loader_->getModel();
188  scene_ = scene;
191  if (!scene_)
192  {
193  try
194  {
195  scene_.reset(new planning_scene::PlanningScene(rm_loader_->getModel()));
200 
201  scene_->getCollisionRobotNonConst()->setPadding(default_robot_padd_);
202  scene_->getCollisionRobotNonConst()->setScale(default_robot_scale_);
203  for (std::map<std::string, double>::iterator it = default_robot_link_padd_.begin();
204  it != default_robot_link_padd_.end(); ++it)
205  {
206  scene_->getCollisionRobotNonConst()->setLinkPadding(it->first, it->second);
207  }
208  for (std::map<std::string, double>::iterator it = default_robot_link_scale_.begin();
209  it != default_robot_link_scale_.end(); ++it)
210  {
211  scene_->getCollisionRobotNonConst()->setLinkScale(it->first, it->second);
212  }
213  scene_->propogateRobotPadding();
214  }
215  catch (moveit::ConstructException& e)
216  {
217  ROS_ERROR_NAMED(LOGNAME, "Configuration of planning scene failed");
218  scene_.reset();
220  }
221  }
222  if (scene_)
223  {
224  scene_->setAttachedBodyUpdateCallback(
226  scene_->setCollisionObjectUpdateCallback(
227  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
228  }
229  }
230  else
231  {
232  ROS_ERROR_NAMED(LOGNAME, "Robot model not loaded");
233  }
234 
237 
241 
242  double temp_wait_time = 0.05;
243 
244  if (!robot_description_.empty())
245  nh_.param(robot_description_ + "_planning/shape_transform_cache_lookup_wait_time", temp_wait_time, temp_wait_time);
246 
248 
249  state_update_pending_ = false;
251  false, // not a oneshot timer
252  false); // do not start the timer yet
253 
255 }
256 
258 {
259  if (scene_)
260  {
261  if (flag)
262  {
263  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
264  if (scene_)
265  {
266  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
267  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
268  scene_->decoupleParent();
270  scene_ = parent_scene_->diff();
272  scene_->setAttachedBodyUpdateCallback(
274  scene_->setCollisionObjectUpdateCallback(
275  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
276  }
277  }
278  else
279  {
281  {
282  ROS_WARN_NAMED(LOGNAME, "Diff monitoring was stopped while publishing planning scene diffs. "
283  "Stopping planning scene diff publisher");
285  }
286  {
287  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
288  if (scene_)
289  {
290  scene_->decoupleParent();
291  parent_scene_.reset();
292  // remove the '+' added by .diff() at the end of the scene name
293  if (!scene_->getName().empty())
294  {
295  if (scene_->getName()[scene_->getName().length() - 1] == '+')
296  scene_->setName(scene_->getName().substr(0, scene_->getName().length() - 1));
297  }
298  }
299  }
300  }
301  }
302 }
303 
305 {
307  {
308  std::unique_ptr<boost::thread> copy;
309  copy.swap(publish_planning_scene_);
310  new_scene_update_condition_.notify_all();
311  copy->join();
312  monitorDiffs(false);
314  ROS_INFO_NAMED(LOGNAME, "Stopped publishing maintained planning scene.");
315  }
316 }
317 
319  const std::string& planning_scene_topic)
320 {
321  publish_update_types_ = update_type;
323  {
324  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>(planning_scene_topic, 100, false);
325  ROS_INFO_NAMED(LOGNAME, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
326  monitorDiffs(true);
327  publish_planning_scene_.reset(new boost::thread(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this)));
328  }
329 }
330 
332 {
333  ROS_DEBUG_NAMED(LOGNAME, "Started scene publishing thread ...");
334 
335  // publish the full planning scene once
336  {
337  moveit_msgs::PlanningScene msg;
338  {
340  if (octomap_monitor_)
341  lock = octomap_monitor_->getOcTreePtr()->reading();
342  scene_->getPlanningSceneMsg(msg);
343  }
345  ROS_DEBUG_NAMED(LOGNAME, "Published the full planning scene: '%s'", msg.name.c_str());
346  }
347 
348  do
349  {
350  moveit_msgs::PlanningScene msg;
351  bool publish_msg = false;
352  bool is_full = false;
354  {
355  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
357  new_scene_update_condition_.wait(ulock);
359  {
360  if ((publish_update_types_ & new_scene_update_) || new_scene_update_ == UPDATE_SCENE)
361  {
362  if (new_scene_update_ == UPDATE_SCENE)
363  is_full = true;
364  else
365  {
367  if (octomap_monitor_)
368  lock = octomap_monitor_->getOcTreePtr()->reading();
369  scene_->getPlanningSceneDiffMsg(msg);
370  }
371  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the
372  // transform cache to
373  // update while we are
374  // potentially changing
375  // attached bodies
376  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
377  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
378  scene_->pushDiffs(parent_scene_);
379  scene_->clearDiffs();
380  scene_->setAttachedBodyUpdateCallback(
382  scene_->setCollisionObjectUpdateCallback(
383  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
384  if (octomap_monitor_)
385  {
386  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
387  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
388  }
389  if (is_full)
390  {
392  if (octomap_monitor_)
393  lock = octomap_monitor_->getOcTreePtr()->reading();
394  scene_->getPlanningSceneMsg(msg);
395  }
396  // also publish timestamp of this robot_state
397  msg.robot_state.joint_state.header.stamp = last_robot_motion_time_;
398  publish_msg = true;
399  }
400  new_scene_update_ = UPDATE_NONE;
401  }
402  }
403  if (publish_msg)
404  {
405  rate.reset();
407  if (is_full)
408  ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str());
409  rate.sleep();
410  }
411  } while (publish_planning_scene_);
412 }
413 
414 void PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string>& topics) const
415 {
416  topics.clear();
418  {
419  const std::string& t = current_state_monitor_->getMonitoredTopic();
420  if (!t.empty())
421  topics.push_back(t);
422  }
424  topics.push_back(planning_scene_subscriber_.getTopic());
426  topics.push_back(collision_object_subscriber_->getTopic());
428  topics.push_back(planning_scene_world_subscriber_.getTopic());
429 }
430 
431 namespace
432 {
433 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr& scene,
434  const planning_scene::PlanningScene* possible_parent)
435 {
436  if (scene && scene.get() == possible_parent)
437  return true;
438  if (scene)
439  return sceneIsParentOf(scene->getParent(), possible_parent);
440  return false;
441 }
442 }
443 
444 bool PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr& scene) const
445 {
446  return sceneIsParentOf(scene_const_, scene.get());
447 }
448 
449 bool PlanningSceneMonitor::updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const
450 {
451  return sceneIsParentOf(scene_const_, scene.get());
452 }
453 
455 {
456  // do not modify update functions while we are calling them
457  boost::recursive_mutex::scoped_lock lock(update_lock_);
458 
459  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
460  update_callbacks_[i](update_type);
461  new_scene_update_ = (SceneUpdateType)((int)new_scene_update_ | (int)update_type);
462  new_scene_update_condition_.notify_all();
463 }
464 
465 bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_name)
466 {
467  // use global namespace for service
468  ros::ServiceClient client = ros::NodeHandle().serviceClient<moveit_msgs::GetPlanningScene>(service_name);
469  moveit_msgs::GetPlanningScene srv;
470  srv.request.components.components =
471  srv.request.components.SCENE_SETTINGS | srv.request.components.ROBOT_STATE |
472  srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS | srv.request.components.WORLD_OBJECT_NAMES |
473  srv.request.components.WORLD_OBJECT_GEOMETRY | srv.request.components.OCTOMAP |
474  srv.request.components.TRANSFORMS | srv.request.components.ALLOWED_COLLISION_MATRIX |
475  srv.request.components.LINK_PADDING_AND_SCALING | srv.request.components.OBJECT_COLORS;
476 
477  // Make sure client is connected to server
478  if (!client.exists())
479  {
480  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for service `" << service_name << "` to exist.");
481  client.waitForExistence(ros::Duration(5.0));
482  }
483 
484  if (client.call(srv))
485  {
486  newPlanningSceneMessage(srv.response.scene);
487  }
488  else
489  {
490  ROS_INFO_NAMED(LOGNAME, "Failed to call service %s, have you launched move_group? at %s:%d", service_name.c_str(),
491  __FILE__, __LINE__);
492  return false;
493  }
494  return true;
495 }
496 
497 void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene)
498 {
499  newPlanningSceneMessage(*scene);
500 }
501 
503 {
504  octomap_monitor_->getOcTreePtr()->lockWrite();
505  octomap_monitor_->getOcTreePtr()->clear();
506  octomap_monitor_->getOcTreePtr()->unlockWrite();
507 }
508 
509 bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
510 {
511  if (!scene_)
512  return false;
513 
514  bool result;
515 
517  std::string old_scene_name;
518  {
519  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
520  // we don't want the transform cache to update while we are potentially changing attached bodies
521  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
522 
524  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
525  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
526  "scene update " << fmod(last_update_time_.toSec(), 10.)
527  << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
528  old_scene_name = scene_->getName();
529  result = scene_->usePlanningSceneMsg(scene);
530  if (octomap_monitor_)
531  {
532  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
533  {
534  octomap_monitor_->getOcTreePtr()->lockWrite();
535  octomap_monitor_->getOcTreePtr()->clear();
536  octomap_monitor_->getOcTreePtr()->unlockWrite();
537  }
538  }
539  robot_model_ = scene_->getRobotModel();
540 
541  // if we just reset the scene completely but we were maintaining diffs, we need to fix that
542  if (!scene.is_diff && parent_scene_)
543  {
544  // the scene is now decoupled from the parent, since we just reset it
545  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
546  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
548  scene_ = parent_scene_->diff();
550  scene_->setAttachedBodyUpdateCallback(
552  scene_->setCollisionObjectUpdateCallback(
553  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
554  }
555  if (octomap_monitor_)
556  {
557  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
558  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
559  }
560  }
561 
562  // if we have a diff, try to more accuratelly determine the update type
563  if (scene.is_diff)
564  {
565  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
566  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
567  scene.link_scale.empty();
568  if (no_other_scene_upd)
569  {
570  upd = UPDATE_NONE;
571  if (!planning_scene::PlanningScene::isEmpty(scene.world))
572  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
573 
574  if (!scene.fixed_frame_transforms.empty())
575  upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
576 
577  if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
578  {
579  upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
580  if (!scene.robot_state.attached_collision_objects.empty() || scene.robot_state.is_diff == false)
581  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
582  }
583  }
584  }
586  return result;
587 }
588 
589 void PlanningSceneMonitor::newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world)
590 {
591  if (scene_)
592  {
594  {
595  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
597  scene_->getWorldNonConst()->clearObjects();
598  scene_->processPlanningSceneWorldMsg(*world);
599  if (octomap_monitor_)
600  {
601  if (world->octomap.octomap.data.empty())
602  {
603  octomap_monitor_->getOcTreePtr()->lockWrite();
604  octomap_monitor_->getOcTreePtr()->clear();
605  octomap_monitor_->getOcTreePtr()->unlockWrite();
606  }
607  }
608  }
610  }
611 }
612 
613 void PlanningSceneMonitor::collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr& obj,
615 {
616  // if we just want to remove objects, the frame does not matter
617  if (reason == tf::filter_failure_reasons::EmptyFrameID && obj->operation == moveit_msgs::CollisionObject::REMOVE)
619 }
620 
621 void PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj)
622 {
623  if (scene_)
624  {
626  {
627  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
629  scene_->processCollisionObjectMsg(*obj);
630  }
632  }
633 }
634 
635 void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
636 {
637  if (scene_)
638  {
640  {
641  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
643  scene_->processAttachedCollisionObjectMsg(*obj);
644  }
646  }
647 }
648 
650 {
651  if (!octomap_monitor_)
652  return;
653 
654  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
655 
657  const std::vector<const robot_model::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
659  bool warned = false;
660  for (std::size_t i = 0; i < links.size(); ++i)
661  {
662  std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes(); // copy shared ptrs on purpuse
663  for (std::size_t j = 0; j < shapes.size(); ++j)
664  {
665  // merge mesh vertices up to 0.1 mm apart
666  if (shapes[j]->type == shapes::MESH)
667  {
668  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
669  m->mergeVertices(1e-4);
670  shapes[j].reset(m);
671  }
672 
673  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
674  if (h)
675  link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
676  }
677  if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
678  {
679  ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
680  warned = true;
681  }
682  }
683 }
684 
686 {
687  if (!octomap_monitor_)
688  return;
689 
690  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
691 
692  for (LinkShapeHandles::iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
693  for (std::size_t i = 0; i < it->second.size(); ++i)
694  octomap_monitor_->forgetShape(it->second[i].first);
695  link_shape_handles_.clear();
696 }
697 
699 {
700  if (!octomap_monitor_)
701  return;
702 
703  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
704 
705  // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
706  for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin();
707  it != attached_body_shape_handles_.end(); ++it)
708  for (std::size_t k = 0; k < it->second.size(); ++k)
709  octomap_monitor_->forgetShape(it->second[k].first);
711 }
712 
714 {
715  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
716 
718  // add attached objects again
719  std::vector<const robot_state::AttachedBody*> ab;
720  scene_->getCurrentState().getAttachedBodies(ab);
721  for (std::size_t i = 0; i < ab.size(); ++i)
723 }
724 
726 {
727  if (!octomap_monitor_)
728  return;
729 
730  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
731 
732  // clear information about any attached object
733  for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin();
734  it != collision_body_shape_handles_.end(); ++it)
735  for (std::size_t k = 0; k < it->second.size(); ++k)
736  octomap_monitor_->forgetShape(it->second[k].first);
738 }
739 
741 {
742  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
743 
745  for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end();
746  ++it)
747  excludeWorldObjectFromOctree(it->second);
748 }
749 
750 void PlanningSceneMonitor::excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body)
751 {
752  if (!octomap_monitor_)
753  return;
754 
755  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
756  bool found = false;
757  for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
758  {
759  if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
760  continue;
761  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
762  if (h)
763  {
764  found = true;
765  attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
766  }
767  }
768  if (found)
769  ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
770 }
771 
772 void PlanningSceneMonitor::includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body)
773 {
774  if (!octomap_monitor_)
775  return;
776 
777  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
778 
779  AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
780  if (it != attached_body_shape_handles_.end())
781  {
782  for (std::size_t k = 0; k < it->second.size(); ++k)
783  octomap_monitor_->forgetShape(it->second[k].first);
784  ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
786  }
787 }
788 
789 void PlanningSceneMonitor::excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj)
790 {
791  if (!octomap_monitor_)
792  return;
793 
794  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
795 
796  bool found = false;
797  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
798  {
799  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
800  continue;
801  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
802  if (h)
803  {
804  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
805  found = true;
806  }
807  }
808  if (found)
809  ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
810 }
811 
812 void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj)
813 {
814  if (!octomap_monitor_)
815  return;
816 
817  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
818 
819  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
820  if (it != collision_body_shape_handles_.end())
821  {
822  for (std::size_t k = 0; k < it->second.size(); ++k)
823  octomap_monitor_->forgetShape(it->second[k].first);
824  ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
826  }
827 }
828 
829 void PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body,
830  bool just_attached)
831 {
832  if (!octomap_monitor_)
833  return;
834 
835  if (just_attached)
836  excludeAttachedBodyFromOctree(attached_body);
837  else
838  includeAttachedBodyInOctree(attached_body);
839 }
840 
841 void PlanningSceneMonitor::currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& obj,
843 {
844  if (!octomap_monitor_)
845  return;
847  return;
848 
851  else if (action & collision_detection::World::DESTROY)
853  else
854  {
857  }
858 }
859 
861 {
862  if (t.isZero())
863  return false;
865  ros::WallDuration timeout(wait_time);
866 
867  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
868 
870  {
871  // Wait for next robot update in state monitor.
872  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
873 
874  /* As robot updates are passed to the planning scene only in throttled fashion, there might
875  be still an update pending. If so, explicitly update the planning scene here.
876  If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
877  if (success)
878  {
879  boost::mutex::scoped_lock lock(state_pending_mutex_);
880  if (state_update_pending_) // enforce state update
881  {
882  state_update_pending_ = false;
884  lock.unlock();
886  }
887  return true;
888  }
889 
890  ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
891  return false;
892  }
893 
894  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
895  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
896  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
897  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
898  ros::Time prev_robot_motion_time = last_robot_motion_time_;
899  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
900  timeout > ros::WallDuration())
901  {
902  ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
903  new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
904  timeout -= ros::WallTime::now() - start; // compute remaining wait_time
905  }
906  bool success = last_robot_motion_time_ >= t;
907  // suppress warning if we received an update at all
908  if (!success && prev_robot_motion_time != last_robot_motion_time_)
909  ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
910  (t - last_robot_motion_time_).toSec());
911 
912  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
913  << " scene update: " << (t - last_update_time_).toSec());
914  return success;
915 }
916 
918 {
919  scene_update_mutex_.lock_shared();
920  if (octomap_monitor_)
921  octomap_monitor_->getOcTreePtr()->lockRead();
922 }
923 
925 {
926  if (octomap_monitor_)
927  octomap_monitor_->getOcTreePtr()->unlockRead();
928  scene_update_mutex_.unlock_shared();
929 }
930 
932 {
933  scene_update_mutex_.lock();
934  if (octomap_monitor_)
935  octomap_monitor_->getOcTreePtr()->lockWrite();
936 }
937 
939 {
940  if (octomap_monitor_)
941  octomap_monitor_->getOcTreePtr()->unlockWrite();
942  scene_update_mutex_.unlock();
943 }
944 
945 void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
946 {
948 
949  ROS_INFO_NAMED(LOGNAME, "Starting scene monitor");
950  // listen for planning scene updates; these messages include transforms, so no need for filters
951  if (!scene_topic.empty())
952  {
955  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
956  }
957 }
958 
960 {
962  {
963  ROS_INFO_NAMED(LOGNAME, "Stopping scene monitor");
965  }
966 }
967 
968 bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
970 {
971  if (!tf_)
972  return false;
973  try
974  {
975  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
976 
977  for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
978  {
980  tf_->waitForTransform(target_frame, it->first->getName(), target_time, shape_transform_cache_lookup_wait_time_);
981  tf_->lookupTransform(target_frame, it->first->getName(), target_time, tr);
982  Eigen::Affine3d ttr;
983  tf::transformTFToEigen(tr, ttr);
984  for (std::size_t j = 0; j < it->second.size(); ++j)
985  cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
986  }
987  for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin();
988  it != attached_body_shape_handles_.end(); ++it)
989  {
991  tf_->waitForTransform(target_frame, it->first->getAttachedLinkName(), target_time,
993  tf_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time, tr);
994  Eigen::Affine3d transform;
995  tf::transformTFToEigen(tr, transform);
996  for (std::size_t k = 0; k < it->second.size(); ++k)
997  cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
998  }
999  {
1001  tf_->waitForTransform(target_frame, scene_->getPlanningFrame(), target_time,
1003  tf_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time, tr);
1004  Eigen::Affine3d transform;
1005  tf::transformTFToEigen(tr, transform);
1006  for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin();
1007  it != collision_body_shape_handles_.end(); ++it)
1008  for (std::size_t k = 0; k < it->second.size(); ++k)
1009  cache[it->second[k].first] = transform * (*it->second[k].second);
1010  }
1011  }
1012  catch (tf::TransformException& ex)
1013  {
1014  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
1015  return false;
1016  }
1017  return true;
1018 }
1019 
1020 void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collision_objects_topic,
1021  const std::string& planning_scene_world_topic,
1022  const bool load_octomap_monitor)
1023 {
1025  ROS_INFO_NAMED(LOGNAME, "Starting world geometry monitor");
1026 
1027  // listen for world geometry updates using message filters
1028  if (!collision_objects_topic.empty())
1029  {
1031  new message_filters::Subscriber<moveit_msgs::CollisionObject>(root_nh_, collision_objects_topic, 1024));
1032  if (tf_)
1033  {
1035  *collision_object_subscriber_, *tf_, scene_->getPlanningFrame(), 1024));
1036  collision_object_filter_->registerCallback(boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
1037  collision_object_filter_->registerFailureCallback(
1038  boost::bind(&PlanningSceneMonitor::collisionObjectFailTFCallback, this, _1, _2));
1039  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message notifier with target frame '%s'",
1040  root_nh_.resolveName(collision_objects_topic).c_str(),
1041  collision_object_filter_->getTargetFramesString().c_str());
1042  }
1043  else
1044  {
1045  collision_object_subscriber_->registerCallback(
1046  boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
1047  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
1048  }
1049  }
1050 
1051  if (!planning_scene_world_topic.empty())
1052  {
1054  root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
1055  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
1056  root_nh_.resolveName(planning_scene_world_topic).c_str());
1057  }
1058 
1059  // Ocotomap monitor is optional
1060  if (load_octomap_monitor)
1061  {
1062  if (!octomap_monitor_)
1063  {
1064  octomap_monitor_.reset(new occupancy_map_monitor::OccupancyMapMonitor(tf_, scene_->getPlanningFrame()));
1068 
1069  octomap_monitor_->setTransformCacheCallback(
1070  boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
1071  octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
1072  }
1073  octomap_monitor_->startMonitor();
1074  }
1075 }
1076 
1078 {
1080  {
1081  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1082  collision_object_filter_.reset();
1085  }
1087  {
1088  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1090  }
1091  if (octomap_monitor_)
1092  octomap_monitor_->stopMonitor();
1093 }
1094 
1095 void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1096  const std::string& attached_objects_topic)
1097 {
1098  stopStateMonitor();
1099  if (scene_)
1100  {
1103  current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
1104  current_state_monitor_->startStateMonitor(joint_states_topic);
1105 
1106  {
1107  boost::mutex::scoped_lock lock(state_pending_mutex_);
1108  if (!dt_state_update_.isZero())
1110  }
1111 
1112  if (!attached_objects_topic.empty())
1113  {
1114  // using regular message filter as there's no header
1116  root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
1117  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
1118  root_nh_.resolveName(attached_objects_topic).c_str());
1119  }
1120  }
1121  else
1122  ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
1123 }
1124 
1126 {
1128  current_state_monitor_->stopStateMonitor();
1131 
1132  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1134  {
1135  boost::mutex::scoped_lock lock(state_pending_mutex_);
1136  state_update_pending_ = false;
1137  }
1138 }
1139 
1140 void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr& /* joint_state */)
1141 {
1142  const ros::WallTime& n = ros::WallTime::now();
1144 
1145  bool update = false;
1146  {
1147  boost::mutex::scoped_lock lock(state_pending_mutex_);
1148 
1149  if (dt < dt_state_update_)
1150  {
1151  state_update_pending_ = true;
1152  }
1153  else
1154  {
1155  state_update_pending_ = false;
1156  last_robot_state_update_wall_time_ = n;
1157  update = true;
1158  }
1159  }
1160  // run the state update with state_pending_mutex_ unlocked
1161  if (update)
1163 }
1164 
1166 {
1168  {
1169  bool update = false;
1170 
1171  const ros::WallTime& n = ros::WallTime::now();
1173 
1174  {
1175  // lock for access to dt_state_update_ and state_update_pending_
1176  boost::mutex::scoped_lock lock(state_pending_mutex_);
1178  {
1179  state_update_pending_ = false;
1180  last_robot_state_update_wall_time_ = ros::WallTime::now();
1181  update = true;
1182  ROS_DEBUG_STREAM_NAMED(LOGNAME,
1183  "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
1184  }
1185  }
1186 
1187  // run the state update with state_pending_mutex_ unlocked
1188  if (update)
1189  {
1191  ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
1192  }
1193  }
1194 }
1195 
1197 {
1198  if (!octomap_monitor_)
1199  return;
1200 
1202  {
1203  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1205  octomap_monitor_->getOcTreePtr()->lockRead();
1206  try
1207  {
1208  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Affine3d::Identity());
1209  octomap_monitor_->getOcTreePtr()->unlockRead();
1210  }
1211  catch (...)
1212  {
1213  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1214  throw;
1215  }
1216  }
1218 }
1219 
1221 {
1222  bool update = false;
1223  if (hz > std::numeric_limits<double>::epsilon())
1224  {
1225  boost::mutex::scoped_lock lock(state_pending_mutex_);
1226  dt_state_update_.fromSec(1.0 / hz);
1229  }
1230  else
1231  {
1232  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1234  boost::mutex::scoped_lock lock(state_pending_mutex_);
1237  update = true;
1238  }
1239  ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
1240 
1241  if (update)
1243 }
1244 
1246 {
1248  {
1249  std::vector<std::string> missing;
1250  if (!current_state_monitor_->haveCompleteState(missing) &&
1251  (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
1252  {
1253  std::string missing_str = boost::algorithm::join(missing, ", ");
1254  ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known. Missing %s",
1255  missing_str.c_str());
1256  }
1257 
1258  {
1259  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1261  ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
1262  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1263  scene_->getCurrentStateNonConst().update(); // compute all transforms
1264  }
1266  }
1267  else
1268  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
1269 }
1270 
1271 void PlanningSceneMonitor::addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn)
1272 {
1273  boost::recursive_mutex::scoped_lock lock(update_lock_);
1274  if (fn)
1275  update_callbacks_.push_back(fn);
1276 }
1277 
1279 {
1280  boost::recursive_mutex::scoped_lock lock(update_lock_);
1281  update_callbacks_.clear();
1282 }
1283 
1285 {
1287  ROS_DEBUG_NAMED(LOGNAME, "Maximum frquency for publishing a planning scene is now %lf Hz",
1289 }
1290 
1291 void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms)
1292 {
1293  const std::string& target = getRobotModel()->getModelFrame();
1294 
1295  std::vector<std::string> all_frame_names;
1296  tf_->getFrameStrings(all_frame_names);
1297  for (std::size_t i = 0; i < all_frame_names.size(); ++i)
1298  {
1299  const std::string& frame_no_slash = (!all_frame_names[i].empty() && all_frame_names[i][0] == '/') ?
1300  all_frame_names[i].substr(1) :
1301  all_frame_names[i];
1302  const std::string& frame_with_slash =
1303  (!all_frame_names[i].empty() && all_frame_names[i][0] != '/') ? '/' + all_frame_names[i] : all_frame_names[i];
1304 
1305  if (frame_with_slash == target || getRobotModel()->hasLinkModel(frame_no_slash))
1306  continue;
1307 
1308  ros::Time stamp(0);
1309  std::string err_string;
1310  if (tf_->getLatestCommonTime(target, all_frame_names[i], stamp, &err_string) != tf::NO_ERROR)
1311  {
1312  ROS_WARN_STREAM_NAMED(LOGNAME, "No transform available between frame '"
1313  << all_frame_names[i] << "' and planning frame '" << target << "' ("
1314  << err_string << ")");
1315  continue;
1316  }
1317 
1319  try
1320  {
1321  tf_->lookupTransform(target, all_frame_names[i], stamp, t);
1322  }
1323  catch (tf::TransformException& ex)
1324  {
1325  ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
1326  << all_frame_names[i] << "' to planning frame '" << target << "' ("
1327  << ex.what() << ")");
1328  continue;
1329  }
1330  geometry_msgs::TransformStamped f;
1331  f.header.frame_id = frame_with_slash;
1332  f.child_frame_id = target;
1333  f.transform.translation.x = t.getOrigin().x();
1334  f.transform.translation.y = t.getOrigin().y();
1335  f.transform.translation.z = t.getOrigin().z();
1336  const tf::Quaternion& q = t.getRotation();
1337  f.transform.rotation.x = q.x();
1338  f.transform.rotation.y = q.y();
1339  f.transform.rotation.z = q.z();
1340  f.transform.rotation.w = q.w();
1341  transforms.push_back(f);
1342  }
1343 }
1344 
1346 {
1347  if (!tf_)
1348  return;
1349 
1350  if (scene_)
1351  {
1352  std::vector<geometry_msgs::TransformStamped> transforms;
1353  getUpdatedFrameTransforms(transforms);
1354  {
1355  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1356  scene_->getTransformsNonConst().setTransforms(transforms);
1358  }
1360  }
1361 }
1362 
1364 {
1365  if (octomap_monitor_)
1366  octomap_monitor_->publishDebugInformation(flag);
1367 }
1368 
1369 void PlanningSceneMonitor::configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene)
1370 {
1371  if (!scene || robot_description_.empty())
1372  return;
1373  collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
1374 
1375  // read overriding values from the param server
1376 
1377  // first we do default collision operations
1378  if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
1379  ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
1380  else
1381  {
1382  ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
1383 
1384  XmlRpc::XmlRpcValue coll_ops;
1385  nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
1386 
1387  if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
1388  {
1389  ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
1390  return;
1391  }
1392 
1393  if (coll_ops.size() == 0)
1394  {
1395  ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
1396  return;
1397  }
1398 
1399  for (int i = 0; i < coll_ops.size(); ++i)
1400  {
1401  if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
1402  {
1403  ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
1404  continue;
1405  }
1406  acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
1407  std::string(coll_ops[i]["operation"]) == "disable");
1408  }
1409  }
1410 }
1411 
1413 {
1414  if (robot_description_.empty())
1415  {
1416  default_robot_padd_ = 0.0;
1417  default_robot_scale_ = 1.0;
1418  default_object_padd_ = 0.0;
1419  default_attached_padd_ = 0.0;
1420  return;
1421  }
1422 
1423  // Ensure no leading slash creates a bad param server address
1424  static const std::string robot_description =
1425  (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
1426 
1427  nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
1428  nh_.param(robot_description + "_planning/default_robot_scale", default_robot_scale_, 1.0);
1429  nh_.param(robot_description + "_planning/default_object_padding", default_object_padd_, 0.0);
1430  nh_.param(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
1431  nh_.param(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_,
1432  std::map<std::string, double>());
1433  nh_.param(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_,
1434  std::map<std::string, double>());
1435 
1436  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
1437  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
1438 }
1439 }
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor.
boost::recursive_mutex update_lock_
lock access to update_callbacks_
dynamic_reconfigure::Server< PlanningSceneMonitorDynamicReconfigureConfig > dynamic_reconfigure_server_
std::map< ShapeHandle, Eigen::Affine3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Affine3d > > > ShapeTransformCache
void clearUpdateCallbacks()
Clear the functions to be called when an update to the scene is received.
#define ROS_INFO_NAMED(name,...)
#define ROS_WARN_THROTTLE_NAMED(rate, name,...)
void mergeVertices(double threshold)
void getUpdatedFrameTransforms(std::vector< geometry_msgs::TransformStamped > &transforms)
#define ROS_DEBUG_STREAM_NAMED(name, args)
void publish(const boost::shared_ptr< M > &message) const
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
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. ...
int size() const
void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state)
void setPeriod(const WallDuration &period, bool reset=true)
std::string resolveName(const std::string &name, bool remap=true) const
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)
const robot_model::RobotModelConstPtr & getRobotModel() const
ros::Duration shape_transform_cache_lookup_wait_time_
the amount of time to wait when looking up transforms
std::string getName(void *handle)
void octomapUpdateCallback()
Callback for octomap updates.
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) ...
std::map< std::string, ObjectPtr >::const_iterator const_iterator
Type const & getType() const
static const std::string OCTOMAP_NS
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...
PlanningSceneMonitor(const std::string &robot_description, const boost::shared_ptr< tf::Transformer > &tf=boost::shared_ptr< tf::Transformer >(), const std::string &name="")
Constructor.
The state in the monitored scene was updated.
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
std::unique_ptr< message_filters::Subscriber< moveit_msgs::CollisionObject > > collision_object_subscriber_
void setStateUpdateFrequency(double hz)
Update the scene using the monitored state at a specified frequency, in Hz. This function has an effe...
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...
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.
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
TFSIMD_FORCE_INLINE const tfScalar & x() const
void excludeAttachedBodyFromOctree(const robot_state::AttachedBody *attached_body)
#define ROS_DEBUG_NAMED(name,...)
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
std::unique_ptr< boost::thread > publish_planning_scene_
void getMonitoredTopics(std::vector< std::string > &topics) const
Get the topic names that the monitor is listening to.
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_.
std::unique_ptr< tf::MessageFilter< moveit_msgs::CollisionObject > > collision_object_filter_
TFSIMD_FORCE_INLINE const tfScalar & z() const
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 listening for objects in the world, the collision map and attached collision objects...
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
WallDuration & fromSec(double t)
std::unique_ptr< occupancy_map_monitor::OccupancyMapMonitor > octomap_monitor_
PlanningSceneMonitor Subscribes to the topic planning_scene.
std::map< std::string, double > default_robot_link_scale_
default robot link scale
bool param(const std::string &param_name, T &param_val, const T &default_val) const
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.
std::string getTopic() const
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...
TFSIMD_FORCE_INLINE const tfScalar & y() const
void updateSceneWithCurrentState()
Update the scene using the monitored state. This function is automatically called when an update to t...
ros::WallTimer state_update_timer_
timer for state updates.
#define ROS_ERROR_THROTTLE_NAMED(rate, name,...)
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
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.
void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene)
Configure the collision matrix for a particular scene.
boost::condition_variable_any new_scene_update_condition_
boost::shared_ptr< tf::Transformer > tf_
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...
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
bool sleep()
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.
bool hasMember(const std::string &name) const
std::vector< boost::function< void(SceneUpdateType)> > update_callbacks_
void collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr &obj, tf::filter_failure_reasons::FilterFailureReason reason)
Callback for a new collision object msg that failed to pass the TF filter.
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
static WallTime now()
Quaternion getRotation() const
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_
bool getParam(const std::string &key, std::string &s) const
bool getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const
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 planning scene state using a service call.
#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.
void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e)
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
bool newPlanningSceneMessage(const moveit_msgs::PlanningScene &scene)
CallbackQueueInterface * getCallbackQueue() const
bool hasParam(const std::string &key) const
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_
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_
NO_ERROR
#define ROS_WARN_STREAM_NAMED(name, args)


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:17:33