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 
108  "attached_collision_object";
111  "world";
115  "scene";
116 
119  const std::string& name)
120  : monitor_name_(name), nh_("~"), tf_(tf)
121 {
122  rm_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
123  initialize(planning_scene::PlanningScenePtr());
124 }
125 
126 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
127  const std::string& robot_description,
129  const std::string& name)
130  : monitor_name_(name), nh_("~"), tf_(tf)
131 {
132  rm_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
133  initialize(scene);
134 }
135 
137  const robot_model_loader::RobotModelLoaderPtr& rm_loader, const boost::shared_ptr<tf::Transformer>& tf,
138  const std::string& name)
139  : monitor_name_(name), nh_("~"), tf_(tf), rm_loader_(rm_loader)
140 {
141  initialize(planning_scene::PlanningScenePtr());
142 }
143 
145  const planning_scene::PlanningScenePtr& scene, const robot_model_loader::RobotModelLoaderPtr& rm_loader,
146  const boost::shared_ptr<tf::Transformer>& tf, const std::string& name)
147  : monitor_name_(name), nh_("~"), tf_(tf), rm_loader_(rm_loader)
148 {
149  initialize(scene);
150 }
151 
153 {
154  if (scene_)
155  {
156  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
157  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
158  }
163 
164  delete reconfigure_impl_;
165  current_state_monitor_.reset();
166  scene_const_.reset();
167  scene_.reset();
168  parent_scene_.reset();
169  robot_model_.reset();
170  rm_loader_.reset();
171 }
172 
173 void planning_scene_monitor::PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& scene)
174 {
176  moveit::tools::Profiler::ScopedBlock prof_block("PlanningSceneMonitor::initialize");
178 
179  if (monitor_name_.empty())
180  monitor_name_ = "planning_scene_monitor";
181  robot_description_ = rm_loader_->getRobotDescription();
182  if (rm_loader_->getModel())
183  {
184  robot_model_ = rm_loader_->getModel();
185  scene_ = scene;
188  if (!scene_)
189  {
190  try
191  {
192  scene_.reset(new planning_scene::PlanningScene(rm_loader_->getModel()));
197 
198  scene_->getCollisionRobotNonConst()->setPadding(default_robot_padd_);
199  scene_->getCollisionRobotNonConst()->setScale(default_robot_scale_);
200  for (std::map<std::string, double>::iterator it = default_robot_link_padd_.begin();
201  it != default_robot_link_padd_.end(); ++it)
202  {
203  scene_->getCollisionRobotNonConst()->setLinkPadding(it->first, it->second);
204  }
205  for (std::map<std::string, double>::iterator it = default_robot_link_scale_.begin();
206  it != default_robot_link_scale_.end(); ++it)
207  {
208  scene_->getCollisionRobotNonConst()->setLinkScale(it->first, it->second);
209  }
210  scene_->propogateRobotPadding();
211  }
212  catch (moveit::ConstructException& e)
213  {
214  ROS_ERROR_NAMED(LOGNAME, "Configuration of planning scene failed");
215  scene_.reset();
217  }
218  }
219  if (scene_)
220  {
221  scene_->setAttachedBodyUpdateCallback(
223  scene_->setCollisionObjectUpdateCallback(
224  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
225  }
226  }
227  else
228  {
229  ROS_ERROR_NAMED(LOGNAME, "Robot model not loaded");
230  }
231 
234 
238 
239  double temp_wait_time = 0.05;
240 
241  if (!robot_description_.empty())
242  nh_.param(robot_description_ + "_planning/shape_transform_cache_lookup_wait_time", temp_wait_time, temp_wait_time);
243 
245 
246  state_update_pending_ = false;
248  false, // not a oneshot timer
249  false); // do not start the timer yet
250 
252 }
253 
255 {
256  if (scene_)
257  {
258  if (flag)
259  {
260  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
261  if (scene_)
262  {
263  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
264  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
265  scene_->decoupleParent();
267  scene_ = parent_scene_->diff();
269  scene_->setAttachedBodyUpdateCallback(
271  scene_->setCollisionObjectUpdateCallback(
272  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
273  }
274  }
275  else
276  {
278  {
279  ROS_WARN_NAMED(LOGNAME, "Diff monitoring was stopped while publishing planning scene diffs. "
280  "Stopping planning scene diff publisher");
282  }
283  {
284  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
285  if (scene_)
286  {
287  scene_->decoupleParent();
288  parent_scene_.reset();
289  // remove the '+' added by .diff() at the end of the scene name
290  if (!scene_->getName().empty())
291  {
292  if (scene_->getName()[scene_->getName().length() - 1] == '+')
293  scene_->setName(scene_->getName().substr(0, scene_->getName().length() - 1));
294  }
295  }
296  }
297  }
298  }
299 }
300 
302 {
304  {
305  std::unique_ptr<boost::thread> copy;
306  copy.swap(publish_planning_scene_);
308  copy->join();
309  monitorDiffs(false);
311  ROS_INFO_NAMED(LOGNAME, "Stopped publishing maintained planning scene.");
312  }
313 }
314 
316  const std::string& planning_scene_topic)
317 {
318  publish_update_types_ = update_type;
320  {
321  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>(planning_scene_topic, 100, false);
322  ROS_INFO_NAMED(LOGNAME, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
323  monitorDiffs(true);
324  publish_planning_scene_.reset(new boost::thread(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this)));
325  }
326 }
327 
329 {
330  ROS_DEBUG_NAMED(LOGNAME, "Started scene publishing thread ...");
331 
332  // publish the full planning scene
333  moveit_msgs::PlanningScene msg;
334  {
336  if (octomap_monitor_)
337  lock = octomap_monitor_->getOcTreePtr()->reading();
338  scene_->getPlanningSceneMsg(msg);
339  }
341  ROS_DEBUG_NAMED(LOGNAME, "Published the full planning scene: '%s'", msg.name.c_str());
342 
343  do
344  {
345  bool publish_msg = false;
346  bool is_full = false;
348  {
349  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
353  {
354  if ((publish_update_types_ & new_scene_update_) || new_scene_update_ == UPDATE_SCENE)
355  {
356  if (new_scene_update_ == UPDATE_SCENE)
357  is_full = true;
358  else
359  {
361  if (octomap_monitor_)
362  lock = octomap_monitor_->getOcTreePtr()->reading();
363  scene_->getPlanningSceneDiffMsg(msg);
364  }
365  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the
366  // transform cache to
367  // update while we are
368  // potentially changing
369  // attached bodies
370  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
371  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
372  scene_->pushDiffs(parent_scene_);
373  scene_->clearDiffs();
374  scene_->setAttachedBodyUpdateCallback(
376  scene_->setCollisionObjectUpdateCallback(
377  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
378  if (octomap_monitor_)
379  {
380  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
381  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
382  }
383  if (is_full)
384  {
386  if (octomap_monitor_)
387  lock = octomap_monitor_->getOcTreePtr()->reading();
388  scene_->getPlanningSceneMsg(msg);
389  }
390  // also publish timestamp of this robot_state
391  msg.robot_state.joint_state.header.stamp = last_robot_motion_time_;
392  publish_msg = true;
393  }
394  new_scene_update_ = UPDATE_NONE;
395  }
396  }
397  if (publish_msg)
398  {
399  rate.reset();
401  if (is_full)
402  ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str());
403  rate.sleep();
404  }
405  } while (publish_planning_scene_);
406 }
407 
408 void planning_scene_monitor::PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string>& topics) const
409 {
410  topics.clear();
412  {
413  const std::string& t = current_state_monitor_->getMonitoredTopic();
414  if (!t.empty())
415  topics.push_back(t);
416  }
418  topics.push_back(planning_scene_subscriber_.getTopic());
420  topics.push_back(collision_object_subscriber_->getTopic());
422  topics.push_back(planning_scene_world_subscriber_.getTopic());
423 }
424 
425 namespace
426 {
427 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr& scene,
428  const planning_scene::PlanningScene* possible_parent)
429 {
430  if (scene && scene.get() == possible_parent)
431  return true;
432  if (scene)
433  return sceneIsParentOf(scene->getParent(), possible_parent);
434  return false;
435 }
436 }
437 
438 bool planning_scene_monitor::PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr& scene) const
439 {
440  return sceneIsParentOf(scene_const_, scene.get());
441 }
442 
444  const planning_scene::PlanningSceneConstPtr& scene) const
445 {
446  return sceneIsParentOf(scene_const_, scene.get());
447 }
448 
450 {
451  // do not modify update functions while we are calling them
452  boost::recursive_mutex::scoped_lock lock(update_lock_);
453 
454  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
455  update_callbacks_[i](update_type);
456  new_scene_update_ = (SceneUpdateType)((int)new_scene_update_ | (int)update_type);
458 }
459 
461 {
462  // use global namespace for service
463  ros::ServiceClient client = ros::NodeHandle().serviceClient<moveit_msgs::GetPlanningScene>(service_name);
464  moveit_msgs::GetPlanningScene srv;
465  srv.request.components.components =
466  srv.request.components.SCENE_SETTINGS | srv.request.components.ROBOT_STATE |
467  srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS | srv.request.components.WORLD_OBJECT_NAMES |
468  srv.request.components.WORLD_OBJECT_GEOMETRY | srv.request.components.OCTOMAP |
469  srv.request.components.TRANSFORMS | srv.request.components.ALLOWED_COLLISION_MATRIX |
470  srv.request.components.LINK_PADDING_AND_SCALING | srv.request.components.OBJECT_COLORS;
471 
472  // Make sure client is connected to server
473  if (!client.exists())
474  {
475  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for service `" << service_name << "` to exist.");
476  client.waitForExistence(ros::Duration(5.0));
477  }
478 
479  if (client.call(srv))
480  {
481  newPlanningSceneMessage(srv.response.scene);
482  }
483  else
484  {
485  ROS_INFO_NAMED(LOGNAME, "Failed to call service %s, have you launched move_group? at %s:%d", service_name.c_str(),
486  __FILE__, __LINE__);
487  return false;
488  }
489  return true;
490 }
491 
493  const moveit_msgs::PlanningSceneConstPtr& scene)
494 {
495  newPlanningSceneMessage(*scene);
496 }
497 
499 {
500  octomap_monitor_->getOcTreePtr()->lockWrite();
501  octomap_monitor_->getOcTreePtr()->clear();
502  octomap_monitor_->getOcTreePtr()->unlockWrite();
503 }
504 
506 {
507  if (!scene_)
508  return false;
509 
510  bool result;
511 
513  std::string old_scene_name;
514  {
515  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
516  // we don't want the transform cache to update while we are potentially changing attached bodies
517  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
518 
520  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
521  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
522  "scene update " << fmod(last_update_time_.toSec(), 10.)
523  << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
524  old_scene_name = scene_->getName();
525  result = scene_->usePlanningSceneMsg(scene);
526  if (octomap_monitor_)
527  {
528  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
529  {
530  octomap_monitor_->getOcTreePtr()->lockWrite();
531  octomap_monitor_->getOcTreePtr()->clear();
532  octomap_monitor_->getOcTreePtr()->unlockWrite();
533  }
534  }
535  robot_model_ = scene_->getRobotModel();
536 
537  // if we just reset the scene completely but we were maintaining diffs, we need to fix that
538  if (!scene.is_diff && parent_scene_)
539  {
540  // the scene is now decoupled from the parent, since we just reset it
541  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
542  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
544  scene_ = parent_scene_->diff();
546  scene_->setAttachedBodyUpdateCallback(
548  scene_->setCollisionObjectUpdateCallback(
549  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
550  }
551  if (octomap_monitor_)
552  {
553  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
554  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
555  }
556  }
557 
558  // if we have a diff, try to more accuratelly determine the update type
559  if (scene.is_diff)
560  {
561  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
562  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
563  scene.link_scale.empty();
564  if (no_other_scene_upd)
565  {
566  upd = UPDATE_NONE;
567  if (!planning_scene::PlanningScene::isEmpty(scene.world))
568  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
569 
570  if (!scene.fixed_frame_transforms.empty())
571  upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
572 
573  if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
574  {
575  upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
576  if (!scene.robot_state.attached_collision_objects.empty() || scene.robot_state.is_diff == false)
577  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
578  }
579  }
580  }
582  return result;
583 }
584 
586  const moveit_msgs::PlanningSceneWorldConstPtr& world)
587 {
588  if (scene_)
589  {
591  {
592  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
594  scene_->getWorldNonConst()->clearObjects();
595  scene_->processPlanningSceneWorldMsg(*world);
596  if (octomap_monitor_)
597  {
598  if (world->octomap.octomap.data.empty())
599  {
600  octomap_monitor_->getOcTreePtr()->lockWrite();
601  octomap_monitor_->getOcTreePtr()->clear();
602  octomap_monitor_->getOcTreePtr()->unlockWrite();
603  }
604  }
605  }
607  }
608 }
609 
611  const moveit_msgs::CollisionObjectConstPtr& obj, tf::filter_failure_reasons::FilterFailureReason reason)
612 {
613  // if we just want to remove objects, the frame does not matter
614  if (reason == tf::filter_failure_reasons::EmptyFrameID && obj->operation == moveit_msgs::CollisionObject::REMOVE)
616 }
617 
619  const moveit_msgs::CollisionObjectConstPtr& obj)
620 {
621  if (scene_)
622  {
624  {
625  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
627  scene_->processCollisionObjectMsg(*obj);
628  }
630  }
631 }
632 
634  const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
635 {
636  if (scene_)
637  {
639  {
640  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
642  scene_->processAttachedCollisionObjectMsg(*obj);
643  }
645  }
646 }
647 
649 {
650  if (!octomap_monitor_)
651  return;
652 
653  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
654 
656  const std::vector<const robot_model::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
658  bool warned = false;
659  for (std::size_t i = 0; i < links.size(); ++i)
660  {
661  std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes(); // copy shared ptrs on purpuse
662  for (std::size_t j = 0; j < shapes.size(); ++j)
663  {
664  // merge mesh vertices up to 0.1 mm apart
665  if (shapes[j]->type == shapes::MESH)
666  {
667  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
668  m->mergeVertices(1e-4);
669  shapes[j].reset(m);
670  }
671 
672  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
673  if (h)
674  link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
675  }
676  if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
677  {
678  ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
679  warned = true;
680  }
681  }
682 }
683 
685 {
686  if (!octomap_monitor_)
687  return;
688 
689  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
690 
691  for (LinkShapeHandles::iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
692  for (std::size_t i = 0; i < it->second.size(); ++i)
693  octomap_monitor_->forgetShape(it->second[i].first);
694  link_shape_handles_.clear();
695 }
696 
698 {
699  if (!octomap_monitor_)
700  return;
701 
702  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
703 
704  // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
705  for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin();
706  it != attached_body_shape_handles_.end(); ++it)
707  for (std::size_t k = 0; k < it->second.size(); ++k)
708  octomap_monitor_->forgetShape(it->second[k].first);
710 }
711 
713 {
714  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
715 
717  // add attached objects again
718  std::vector<const robot_state::AttachedBody*> ab;
719  scene_->getCurrentState().getAttachedBodies(ab);
720  for (std::size_t i = 0; i < ab.size(); ++i)
722 }
723 
725 {
726  if (!octomap_monitor_)
727  return;
728 
729  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
730 
731  // clear information about any attached object
732  for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin();
733  it != collision_body_shape_handles_.end(); ++it)
734  for (std::size_t k = 0; k < it->second.size(); ++k)
735  octomap_monitor_->forgetShape(it->second[k].first);
737 }
738 
740 {
741  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
742 
744  for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end();
745  ++it)
746  excludeWorldObjectFromOctree(it->second);
747 }
748 
750  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 
773  const robot_state::AttachedBody* attached_body)
774 {
775  if (!octomap_monitor_)
776  return;
777 
778  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
779 
780  AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
781  if (it != attached_body_shape_handles_.end())
782  {
783  for (std::size_t k = 0; k < it->second.size(); ++k)
784  octomap_monitor_->forgetShape(it->second[k].first);
785  ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
787  }
788 }
789 
791  const collision_detection::World::ObjectConstPtr& obj)
792 {
793  if (!octomap_monitor_)
794  return;
795 
796  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
797 
798  bool found = false;
799  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
800  {
801  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
802  continue;
803  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
804  if (h)
805  {
806  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
807  found = true;
808  }
809  }
810  if (found)
811  ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
812 }
813 
815  const collision_detection::World::ObjectConstPtr& obj)
816 {
817  if (!octomap_monitor_)
818  return;
819 
820  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
821 
822  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
823  if (it != collision_body_shape_handles_.end())
824  {
825  for (std::size_t k = 0; k < it->second.size(); ++k)
826  octomap_monitor_->forgetShape(it->second[k].first);
827  ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
829  }
830 }
831 
833  robot_state::AttachedBody* attached_body, bool just_attached)
834 {
835  if (!octomap_monitor_)
836  return;
837 
838  if (just_attached)
839  excludeAttachedBodyFromOctree(attached_body);
840  else
841  includeAttachedBodyInOctree(attached_body);
842 }
843 
845  const collision_detection::World::ObjectConstPtr& obj, collision_detection::World::Action action)
846 {
847  if (!octomap_monitor_)
848  return;
850  return;
851 
854  else if (action & collision_detection::World::DESTROY)
856  else
857  {
860  }
861 }
862 
864 {
865  if (t.isZero())
866  return false;
868  ros::WallDuration timeout(wait_time);
869 
870  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
871 
873  {
874  // Wait for next robot update in state monitor. Those updates are only passed to PSM when robot actually moved!
875  enforce_next_state_update_ = true; // enforce potential updates to be directly applied
876  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
878  false; // back to normal throttling behavior, not applying incoming updates immediately
879 
880  /* If the robot doesn't move, we will never receive an update from CSM in planning scene.
881  As we ensured that an update, if it is triggered by CSM, is directly passed to the scene,
882  we can early return true here (if we successfully received a CSM update). Otherwise return false. */
883  if (success)
884  return true;
885 
886  ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
887  return false;
888  }
889 
890  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
891  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
892  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
893  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
894  ros::Time prev_robot_motion_time = last_robot_motion_time_;
895  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
896  timeout > ros::WallDuration())
897  {
898  ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
899  new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
900  timeout -= ros::WallTime::now() - start; // compute remaining wait_time
901  }
902  bool success = last_robot_motion_time_ >= t;
903  // suppress warning if we received an update at all
904  if (!success && prev_robot_motion_time != last_robot_motion_time_)
905  ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
906  (t - last_robot_motion_time_).toSec());
907 
908  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
909  << " scene update: " << (t - last_update_time_).toSec());
910  return success;
911 }
912 
914 {
915  scene_update_mutex_.lock_shared();
916  if (octomap_monitor_)
917  octomap_monitor_->getOcTreePtr()->lockRead();
918 }
919 
921 {
922  if (octomap_monitor_)
923  octomap_monitor_->getOcTreePtr()->unlockRead();
924  scene_update_mutex_.unlock_shared();
925 }
926 
928 {
929  scene_update_mutex_.lock();
930  if (octomap_monitor_)
931  octomap_monitor_->getOcTreePtr()->lockWrite();
932 }
933 
935 {
936  if (octomap_monitor_)
937  octomap_monitor_->getOcTreePtr()->unlockWrite();
938  scene_update_mutex_.unlock();
939 }
940 
942 {
944 
945  ROS_INFO_NAMED(LOGNAME, "Starting scene monitor");
946  // listen for planning scene updates; these messages include transforms, so no need for filters
947  if (!scene_topic.empty())
948  {
951  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
952  }
953 }
954 
956 {
958  {
959  ROS_INFO_NAMED(LOGNAME, "Stopping scene monitor");
961  }
962 }
963 
965  const std::string& target_frame, const ros::Time& target_time,
967 {
968  if (!tf_)
969  return false;
970  try
971  {
972  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
973 
974  for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
975  {
977  tf_->waitForTransform(target_frame, it->first->getName(), target_time, shape_transform_cache_lookup_wait_time_);
978  tf_->lookupTransform(target_frame, it->first->getName(), target_time, tr);
979  Eigen::Affine3d ttr;
980  tf::transformTFToEigen(tr, ttr);
981  for (std::size_t j = 0; j < it->second.size(); ++j)
982  cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
983  }
984  for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin();
985  it != attached_body_shape_handles_.end(); ++it)
986  {
988  tf_->waitForTransform(target_frame, it->first->getAttachedLinkName(), target_time,
990  tf_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time, tr);
991  Eigen::Affine3d transform;
992  tf::transformTFToEigen(tr, transform);
993  for (std::size_t k = 0; k < it->second.size(); ++k)
994  cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
995  }
996  {
998  tf_->waitForTransform(target_frame, scene_->getPlanningFrame(), target_time,
1000  tf_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time, tr);
1001  Eigen::Affine3d transform;
1002  tf::transformTFToEigen(tr, transform);
1003  for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin();
1004  it != collision_body_shape_handles_.end(); ++it)
1005  for (std::size_t k = 0; k < it->second.size(); ++k)
1006  cache[it->second[k].first] = transform * (*it->second[k].second);
1007  }
1008  }
1009  catch (tf::TransformException& ex)
1010  {
1011  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
1012  return false;
1013  }
1014  return true;
1015 }
1016 
1018  const std::string& collision_objects_topic, const std::string& planning_scene_world_topic,
1019  const bool load_octomap_monitor)
1020 {
1022  ROS_INFO_NAMED(LOGNAME, "Starting world geometry monitor");
1023 
1024  // listen for world geometry updates using message filters
1025  if (!collision_objects_topic.empty())
1026  {
1028  new message_filters::Subscriber<moveit_msgs::CollisionObject>(root_nh_, collision_objects_topic, 1024));
1029  if (tf_)
1030  {
1032  *collision_object_subscriber_, *tf_, scene_->getPlanningFrame(), 1024));
1033  collision_object_filter_->registerCallback(boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
1034  collision_object_filter_->registerFailureCallback(
1035  boost::bind(&PlanningSceneMonitor::collisionObjectFailTFCallback, this, _1, _2));
1036  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message notifier with target frame '%s'",
1037  root_nh_.resolveName(collision_objects_topic).c_str(),
1038  collision_object_filter_->getTargetFramesString().c_str());
1039  }
1040  else
1041  {
1042  collision_object_subscriber_->registerCallback(
1043  boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
1044  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
1045  }
1046  }
1047 
1048  if (!planning_scene_world_topic.empty())
1049  {
1051  root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
1052  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
1053  root_nh_.resolveName(planning_scene_world_topic).c_str());
1054  }
1055 
1056  // Ocotomap monitor is optional
1057  if (load_octomap_monitor)
1058  {
1059  if (!octomap_monitor_)
1060  {
1061  octomap_monitor_.reset(new occupancy_map_monitor::OccupancyMapMonitor(tf_, scene_->getPlanningFrame()));
1065 
1066  octomap_monitor_->setTransformCacheCallback(
1067  boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
1068  octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
1069  }
1070  octomap_monitor_->startMonitor();
1071  }
1072 }
1073 
1075 {
1077  {
1078  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1079  collision_object_filter_.reset();
1082  }
1084  {
1085  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1087  }
1088  if (octomap_monitor_)
1089  octomap_monitor_->stopMonitor();
1090 }
1091 
1092 void planning_scene_monitor::PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1093  const std::string& attached_objects_topic)
1094 {
1095  stopStateMonitor();
1096  if (scene_)
1097  {
1100  current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
1101  current_state_monitor_->startStateMonitor(joint_states_topic);
1102 
1103  {
1104  boost::mutex::scoped_lock lock(state_pending_mutex_);
1105  if (!dt_state_update_.isZero())
1107  }
1108 
1109  if (!attached_objects_topic.empty())
1110  {
1111  // using regular message filter as there's no header
1113  root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
1114  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
1115  root_nh_.resolveName(attached_objects_topic).c_str());
1116  }
1117  }
1118  else
1119  ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
1120 }
1121 
1123 {
1125  current_state_monitor_->stopStateMonitor();
1128 
1129  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1131  {
1132  boost::mutex::scoped_lock lock(state_pending_mutex_);
1133  state_update_pending_ = false;
1134  }
1135 }
1136 
1138  const sensor_msgs::JointStateConstPtr& /* joint_state */)
1139 {
1140  const ros::WallTime& n = ros::WallTime::now();
1142 
1144  {
1145  boost::mutex::scoped_lock lock(state_pending_mutex_);
1146 
1147  if (dt < dt_state_update_ && !update)
1148  {
1149  state_update_pending_ = true;
1150  }
1151  else
1152  {
1153  state_update_pending_ = false;
1154  last_robot_state_update_wall_time_ = n;
1155  update = true;
1156  }
1157  }
1158  // run the state update with state_pending_mutex_ unlocked
1159  if (update)
1161 }
1162 
1164 {
1166  {
1167  bool update = false;
1168 
1169  const ros::WallTime& n = ros::WallTime::now();
1171 
1172  {
1173  // lock for access to dt_state_update_ and state_update_pending_
1174  boost::mutex::scoped_lock lock(state_pending_mutex_);
1176  {
1177  state_update_pending_ = false;
1178  last_robot_state_update_wall_time_ = ros::WallTime::now();
1179  update = true;
1181  "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
1182  }
1183  }
1184 
1185  // run the state update with state_pending_mutex_ unlocked
1186  if (update)
1187  {
1189  ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
1190  }
1191  }
1192 }
1193 
1195 {
1196  if (!octomap_monitor_)
1197  return;
1198 
1200  {
1201  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1203  octomap_monitor_->getOcTreePtr()->lockRead();
1204  try
1205  {
1206  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Affine3d::Identity());
1207  octomap_monitor_->getOcTreePtr()->unlockRead();
1208  }
1209  catch (...)
1210  {
1211  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1212  throw;
1213  }
1214  }
1216 }
1217 
1219 {
1220  bool update = false;
1221  if (hz > std::numeric_limits<double>::epsilon())
1222  {
1223  boost::mutex::scoped_lock lock(state_pending_mutex_);
1224  dt_state_update_.fromSec(1.0 / hz);
1227  }
1228  else
1229  {
1230  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1232  boost::mutex::scoped_lock lock(state_pending_mutex_);
1235  update = true;
1236  }
1237  ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
1238 
1239  if (update)
1241 }
1242 
1244 {
1246  {
1247  std::vector<std::string> missing;
1248  if (!current_state_monitor_->haveCompleteState(missing) &&
1249  (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
1250  {
1251  std::string missing_str = boost::algorithm::join(missing, ", ");
1252  ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known. Missing %s",
1253  missing_str.c_str());
1254  }
1255 
1256  {
1257  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1259  ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
1260  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1261  scene_->getCurrentStateNonConst().update(); // compute all transforms
1262  }
1264  }
1265  else
1266  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
1267 }
1268 
1270 {
1271  boost::recursive_mutex::scoped_lock lock(update_lock_);
1272  if (fn)
1273  update_callbacks_.push_back(fn);
1274 }
1275 
1277 {
1278  boost::recursive_mutex::scoped_lock lock(update_lock_);
1279  update_callbacks_.clear();
1280 }
1281 
1283 {
1285  ROS_DEBUG_NAMED(LOGNAME, "Maximum frquency for publishing a planning scene is now %lf Hz",
1287 }
1288 
1290  std::vector<geometry_msgs::TransformStamped>& transforms)
1291 {
1292  const std::string& target = getRobotModel()->getModelFrame();
1293 
1294  std::vector<std::string> all_frame_names;
1295  tf_->getFrameStrings(all_frame_names);
1296  for (std::size_t i = 0; i < all_frame_names.size(); ++i)
1297  {
1298  const std::string& frame_no_slash = (!all_frame_names[i].empty() && all_frame_names[i][0] == '/') ?
1299  all_frame_names[i].substr(1) :
1300  all_frame_names[i];
1301  const std::string& frame_with_slash =
1302  (!all_frame_names[i].empty() && all_frame_names[i][0] != '/') ? '/' + all_frame_names[i] : all_frame_names[i];
1303 
1304  if (frame_with_slash == target || getRobotModel()->hasLinkModel(frame_no_slash))
1305  continue;
1306 
1307  ros::Time stamp(0);
1308  std::string err_string;
1309  if (tf_->getLatestCommonTime(target, all_frame_names[i], stamp, &err_string) != tf::NO_ERROR)
1310  {
1311  ROS_WARN_STREAM_NAMED(LOGNAME, "No transform available between frame '"
1312  << all_frame_names[i] << "' and planning frame '" << target << "' ("
1313  << err_string << ")");
1314  continue;
1315  }
1316 
1318  try
1319  {
1320  tf_->lookupTransform(target, all_frame_names[i], stamp, t);
1321  }
1322  catch (tf::TransformException& ex)
1323  {
1324  ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
1325  << all_frame_names[i] << "' to planning frame '" << target << "' ("
1326  << ex.what() << ")");
1327  continue;
1328  }
1329  geometry_msgs::TransformStamped f;
1330  f.header.frame_id = frame_with_slash;
1331  f.child_frame_id = target;
1332  f.transform.translation.x = t.getOrigin().x();
1333  f.transform.translation.y = t.getOrigin().y();
1334  f.transform.translation.z = t.getOrigin().z();
1335  const tf::Quaternion& q = t.getRotation();
1336  f.transform.rotation.x = q.x();
1337  f.transform.rotation.y = q.y();
1338  f.transform.rotation.z = q.z();
1339  f.transform.rotation.w = q.w();
1340  transforms.push_back(f);
1341  }
1342 }
1343 
1345 {
1346  if (!tf_)
1347  return;
1348 
1349  if (scene_)
1350  {
1351  std::vector<geometry_msgs::TransformStamped> transforms;
1352  getUpdatedFrameTransforms(transforms);
1353  {
1354  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1355  scene_->getTransformsNonConst().setTransforms(transforms);
1357  }
1359  }
1360 }
1361 
1363 {
1364  if (octomap_monitor_)
1365  octomap_monitor_->publishDebugInformation(flag);
1366 }
1367 
1369  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 }
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.
void notify_all() BOOST_NOEXCEPT
bool enforce_next_state_update_
Last time the robot has moved.
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
bool waitForCurrentRobotState(const ros::Time &t, double wait_time=1.)
Wait for robot state to become more recent than time t.
ros::NodeHandle nh_
flag to enforce immediate state update in onStateUpdate()
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)
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 Mon Jan 15 2018 03:51:18