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");
177 
178  if (monitor_name_.empty())
179  monitor_name_ = "planning_scene_monitor";
180  robot_description_ = rm_loader_->getRobotDescription();
181  if (rm_loader_->getModel())
182  {
183  robot_model_ = rm_loader_->getModel();
184  scene_ = scene;
187  if (!scene_)
188  {
189  try
190  {
191  scene_.reset(new planning_scene::PlanningScene(rm_loader_->getModel()));
196 
197  scene_->getCollisionRobotNonConst()->setPadding(default_robot_padd_);
198  scene_->getCollisionRobotNonConst()->setScale(default_robot_scale_);
199  for (std::map<std::string, double>::iterator it = default_robot_link_padd_.begin();
200  it != default_robot_link_padd_.end(); ++it)
201  {
202  scene_->getCollisionRobotNonConst()->setLinkPadding(it->first, it->second);
203  }
204  for (std::map<std::string, double>::iterator it = default_robot_link_scale_.begin();
205  it != default_robot_link_scale_.end(); ++it)
206  {
207  scene_->getCollisionRobotNonConst()->setLinkScale(it->first, it->second);
208  }
209  scene_->propogateRobotPadding();
210  }
211  catch (moveit::ConstructException& e)
212  {
213  ROS_ERROR_NAMED(LOGNAME, "Configuration of planning scene failed");
214  scene_.reset();
216  }
217  }
218  if (scene_)
219  {
220  scene_->setAttachedBodyUpdateCallback(
222  scene_->setCollisionObjectUpdateCallback(
223  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
224  }
225  }
226  else
227  {
228  ROS_ERROR_NAMED(LOGNAME, "Robot model not loaded");
229  }
230 
233 
237 
238  double temp_wait_time = 0.05;
239 
240  if (!robot_description_.empty())
241  nh_.param(robot_description_ + "_planning/shape_transform_cache_lookup_wait_time", temp_wait_time, temp_wait_time);
242 
244 
245  state_update_pending_ = false;
247  false, // not a oneshot timer
248  false); // do not start the timer yet
249 
251 }
252 
254 {
255  if (scene_)
256  {
257  if (flag)
258  {
259  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
260  if (scene_)
261  {
262  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
263  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
264  scene_->decoupleParent();
266  scene_ = parent_scene_->diff();
268  scene_->setAttachedBodyUpdateCallback(
270  scene_->setCollisionObjectUpdateCallback(
271  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
272  }
273  }
274  else
275  {
277  {
278  ROS_WARN_NAMED(LOGNAME, "Diff monitoring was stopped while publishing planning scene diffs. "
279  "Stopping planning scene diff publisher");
281  }
282  {
283  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
284  if (scene_)
285  {
286  scene_->decoupleParent();
287  parent_scene_.reset();
288  // remove the '+' added by .diff() at the end of the scene name
289  if (!scene_->getName().empty())
290  {
291  if (scene_->getName()[scene_->getName().length() - 1] == '+')
292  scene_->setName(scene_->getName().substr(0, scene_->getName().length() - 1));
293  }
294  }
295  }
296  }
297  }
298 }
299 
301 {
303  {
304  std::unique_ptr<boost::thread> copy;
305  copy.swap(publish_planning_scene_);
307  copy->join();
308  monitorDiffs(false);
310  ROS_INFO_NAMED(LOGNAME, "Stopped publishing maintained planning scene.");
311  }
312 }
313 
315  const std::string& planning_scene_topic)
316 {
317  publish_update_types_ = update_type;
319  {
320  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>(planning_scene_topic, 100, false);
321  ROS_INFO_NAMED(LOGNAME, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
322  monitorDiffs(true);
323  publish_planning_scene_.reset(new boost::thread(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this)));
324  }
325 }
326 
328 {
329  ROS_DEBUG_NAMED(LOGNAME, "Started scene publishing thread ...");
330 
331  // publish the full planning scene
332  moveit_msgs::PlanningScene msg;
333  {
335  if (octomap_monitor_)
336  lock = octomap_monitor_->getOcTreePtr()->reading();
337  scene_->getPlanningSceneMsg(msg);
338  }
340  ROS_DEBUG_NAMED(LOGNAME, "Published the full planning scene: '%s'", msg.name.c_str());
341 
342  do
343  {
344  bool publish_msg = false;
345  bool is_full = false;
347  {
348  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
352  {
353  if ((publish_update_types_ & new_scene_update_) || new_scene_update_ == UPDATE_SCENE)
354  {
355  if (new_scene_update_ == UPDATE_SCENE)
356  is_full = true;
357  else
358  {
360  if (octomap_monitor_)
361  lock = octomap_monitor_->getOcTreePtr()->reading();
362  scene_->getPlanningSceneDiffMsg(msg);
363  }
364  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the
365  // transform cache to
366  // update while we are
367  // potentially changing
368  // attached bodies
369  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
370  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
371  scene_->pushDiffs(parent_scene_);
372  scene_->clearDiffs();
373  scene_->setAttachedBodyUpdateCallback(
375  scene_->setCollisionObjectUpdateCallback(
376  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
377  if (octomap_monitor_)
378  {
379  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
380  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
381  }
382  if (is_full)
383  {
385  if (octomap_monitor_)
386  lock = octomap_monitor_->getOcTreePtr()->reading();
387  scene_->getPlanningSceneMsg(msg);
388  }
389  // also publish timestamp of this robot_state
390  msg.robot_state.joint_state.header.stamp = last_robot_motion_time_;
391  publish_msg = true;
392  }
393  new_scene_update_ = UPDATE_NONE;
394  }
395  }
396  if (publish_msg)
397  {
398  rate.reset();
400  if (is_full)
401  ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str());
402  rate.sleep();
403  }
404  } while (publish_planning_scene_);
405 }
406 
407 void planning_scene_monitor::PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string>& topics) const
408 {
409  topics.clear();
411  {
412  const std::string& t = current_state_monitor_->getMonitoredTopic();
413  if (!t.empty())
414  topics.push_back(t);
415  }
417  topics.push_back(planning_scene_subscriber_.getTopic());
419  topics.push_back(collision_object_subscriber_->getTopic());
421  topics.push_back(planning_scene_world_subscriber_.getTopic());
422 }
423 
424 namespace
425 {
426 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr& scene,
427  const planning_scene::PlanningScene* possible_parent)
428 {
429  if (scene && scene.get() == possible_parent)
430  return true;
431  if (scene)
432  return sceneIsParentOf(scene->getParent(), possible_parent);
433  return false;
434 }
435 }
436 
437 bool planning_scene_monitor::PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr& scene) const
438 {
439  return sceneIsParentOf(scene_const_, scene.get());
440 }
441 
443  const planning_scene::PlanningSceneConstPtr& scene) const
444 {
445  return sceneIsParentOf(scene_const_, scene.get());
446 }
447 
449 {
450  // do not modify update functions while we are calling them
451  boost::recursive_mutex::scoped_lock lock(update_lock_);
452 
453  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
454  update_callbacks_[i](update_type);
455  new_scene_update_ = (SceneUpdateType)((int)new_scene_update_ | (int)update_type);
457 }
458 
460 {
461  // use global namespace for service
462  ros::ServiceClient client = ros::NodeHandle().serviceClient<moveit_msgs::GetPlanningScene>(service_name);
463  moveit_msgs::GetPlanningScene srv;
464  srv.request.components.components =
465  srv.request.components.SCENE_SETTINGS | srv.request.components.ROBOT_STATE |
466  srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS | srv.request.components.WORLD_OBJECT_NAMES |
467  srv.request.components.WORLD_OBJECT_GEOMETRY | srv.request.components.OCTOMAP |
468  srv.request.components.TRANSFORMS | srv.request.components.ALLOWED_COLLISION_MATRIX |
469  srv.request.components.LINK_PADDING_AND_SCALING | srv.request.components.OBJECT_COLORS;
470 
471  // Make sure client is connected to server
472  if (!client.exists())
473  {
474  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for service `" << service_name << "` to exist.");
475  client.waitForExistence(ros::Duration(5.0));
476  }
477 
478  if (client.call(srv))
479  {
480  newPlanningSceneMessage(srv.response.scene);
481  }
482  else
483  {
484  ROS_INFO_NAMED(LOGNAME, "Failed to call service %s, have you launched move_group? at %s:%d", service_name.c_str(),
485  __FILE__, __LINE__);
486  return false;
487  }
488  return true;
489 }
490 
492  const moveit_msgs::PlanningSceneConstPtr& scene)
493 {
494  newPlanningSceneMessage(*scene);
495 }
496 
498 {
499  octomap_monitor_->getOcTreePtr()->lockWrite();
500  octomap_monitor_->getOcTreePtr()->clear();
501  octomap_monitor_->getOcTreePtr()->unlockWrite();
502 }
503 
505 {
506  if (!scene_)
507  return false;
508 
509  bool result;
510 
512  std::string old_scene_name;
513  {
514  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
515  // we don't want the transform cache to update while we are potentially changing attached bodies
516  boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
517 
519  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
520  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor",
521  "scene update " << fmod(last_update_time_.toSec(), 10.)
522  << " robot stamp: " << fmod(last_robot_motion_time_.toSec(), 10.));
523  old_scene_name = scene_->getName();
524  result = scene_->usePlanningSceneMsg(scene);
525  if (octomap_monitor_)
526  {
527  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
528  {
529  octomap_monitor_->getOcTreePtr()->lockWrite();
530  octomap_monitor_->getOcTreePtr()->clear();
531  octomap_monitor_->getOcTreePtr()->unlockWrite();
532  }
533  }
534  robot_model_ = scene_->getRobotModel();
535 
536  // if we just reset the scene completely but we were maintaining diffs, we need to fix that
537  if (!scene.is_diff && parent_scene_)
538  {
539  // the scene is now decoupled from the parent, since we just reset it
540  scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
541  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
543  scene_ = parent_scene_->diff();
545  scene_->setAttachedBodyUpdateCallback(
547  scene_->setCollisionObjectUpdateCallback(
548  boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
549  }
550  if (octomap_monitor_)
551  {
552  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
553  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
554  }
555  }
556 
557  // if we have a diff, try to more accuratelly determine the update type
558  if (scene.is_diff)
559  {
560  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
561  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
562  scene.link_scale.empty();
563  if (no_other_scene_upd)
564  {
565  upd = UPDATE_NONE;
566  if (!planning_scene::PlanningScene::isEmpty(scene.world))
567  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
568 
569  if (!scene.fixed_frame_transforms.empty())
570  upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
571 
572  if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
573  {
574  upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
575  if (!scene.robot_state.attached_collision_objects.empty() || scene.robot_state.is_diff == false)
576  upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
577  }
578  }
579  }
581  return result;
582 }
583 
585  const moveit_msgs::PlanningSceneWorldConstPtr& world)
586 {
587  if (scene_)
588  {
590  {
591  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
593  scene_->getWorldNonConst()->clearObjects();
594  scene_->processPlanningSceneWorldMsg(*world);
595  if (octomap_monitor_)
596  {
597  if (world->octomap.octomap.data.empty())
598  {
599  octomap_monitor_->getOcTreePtr()->lockWrite();
600  octomap_monitor_->getOcTreePtr()->clear();
601  octomap_monitor_->getOcTreePtr()->unlockWrite();
602  }
603  }
604  }
606  }
607 }
608 
610  const moveit_msgs::CollisionObjectConstPtr& obj, tf::filter_failure_reasons::FilterFailureReason reason)
611 {
612  // if we just want to remove objects, the frame does not matter
613  if (reason == tf::filter_failure_reasons::EmptyFrameID && obj->operation == moveit_msgs::CollisionObject::REMOVE)
615 }
616 
618  const moveit_msgs::CollisionObjectConstPtr& obj)
619 {
620  if (scene_)
621  {
623  {
624  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
626  scene_->processCollisionObjectMsg(*obj);
627  }
629  }
630 }
631 
633  const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
634 {
635  if (scene_)
636  {
638  {
639  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
641  scene_->processAttachedCollisionObjectMsg(*obj);
642  }
644  }
645 }
646 
648 {
649  if (!octomap_monitor_)
650  return;
651 
652  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
653 
655  const std::vector<const robot_model::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
657  bool warned = false;
658  for (std::size_t i = 0; i < links.size(); ++i)
659  {
660  std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes(); // copy shared ptrs on purpuse
661  for (std::size_t j = 0; j < shapes.size(); ++j)
662  {
663  // merge mesh vertices up to 0.1 mm apart
664  if (shapes[j]->type == shapes::MESH)
665  {
666  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
667  m->mergeVertices(1e-4);
668  shapes[j].reset(m);
669  }
670 
671  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
672  if (h)
673  link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
674  }
675  if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
676  {
677  ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
678  warned = true;
679  }
680  }
681 }
682 
684 {
685  if (!octomap_monitor_)
686  return;
687 
688  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
689 
690  for (LinkShapeHandles::iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
691  for (std::size_t i = 0; i < it->second.size(); ++i)
692  octomap_monitor_->forgetShape(it->second[i].first);
693  link_shape_handles_.clear();
694 }
695 
697 {
698  if (!octomap_monitor_)
699  return;
700 
701  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
702 
703  // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
704  for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin();
705  it != attached_body_shape_handles_.end(); ++it)
706  for (std::size_t k = 0; k < it->second.size(); ++k)
707  octomap_monitor_->forgetShape(it->second[k].first);
709 }
710 
712 {
713  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
714 
716  // add attached objects again
717  std::vector<const robot_state::AttachedBody*> ab;
718  scene_->getCurrentState().getAttachedBodies(ab);
719  for (std::size_t i = 0; i < ab.size(); ++i)
721 }
722 
724 {
725  if (!octomap_monitor_)
726  return;
727 
728  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
729 
730  // clear information about any attached object
731  for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin();
732  it != collision_body_shape_handles_.end(); ++it)
733  for (std::size_t k = 0; k < it->second.size(); ++k)
734  octomap_monitor_->forgetShape(it->second[k].first);
736 }
737 
739 {
740  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
741 
743  for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end();
744  ++it)
745  excludeWorldObjectFromOctree(it->second);
746 }
747 
749  const robot_state::AttachedBody* attached_body)
750 {
751  if (!octomap_monitor_)
752  return;
753 
754  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
755  bool found = false;
756  for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
757  {
758  if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
759  continue;
760  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
761  if (h)
762  {
763  found = true;
764  attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
765  }
766  }
767  if (found)
768  ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
769 }
770 
772  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 
790  const collision_detection::World::ObjectConstPtr& obj)
791 {
792  if (!octomap_monitor_)
793  return;
794 
795  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
796 
797  bool found = false;
798  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
799  {
800  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
801  continue;
802  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
803  if (h)
804  {
805  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
806  found = true;
807  }
808  }
809  if (found)
810  ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
811 }
812 
814  const collision_detection::World::ObjectConstPtr& obj)
815 {
816  if (!octomap_monitor_)
817  return;
818 
819  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
820 
821  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
822  if (it != collision_body_shape_handles_.end())
823  {
824  for (std::size_t k = 0; k < it->second.size(); ++k)
825  octomap_monitor_->forgetShape(it->second[k].first);
826  ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
828  }
829 }
830 
832  robot_state::AttachedBody* attached_body, bool just_attached)
833 {
834  if (!octomap_monitor_)
835  return;
836 
837  if (just_attached)
838  excludeAttachedBodyFromOctree(attached_body);
839  else
840  includeAttachedBodyInOctree(attached_body);
841 }
842 
844  const collision_detection::World::ObjectConstPtr& obj, collision_detection::World::Action action)
845 {
846  if (!octomap_monitor_)
847  return;
849  return;
850 
853  else if (action & collision_detection::World::DESTROY)
855  else
856  {
859  }
860 }
861 
863 {
864  if (t.isZero())
865  return false;
867  ros::WallDuration timeout(wait_time);
868 
869  ROS_DEBUG_NAMED(LOGNAME, "sync robot state to: %.3fs", fmod(t.toSec(), 10.));
870 
872  {
873  // Wait for next robot update in state monitor.
874  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
875 
876  /* As robot updates are passed to the planning scene only in throttled fashion, there might
877  be still an update pending. If so, explicitly update the planning scene here.
878  If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
879  if (success)
880  {
881  boost::mutex::scoped_lock lock(state_pending_mutex_);
882  if (state_update_pending_) // enforce state update
883  {
884  state_update_pending_ = false;
886  lock.unlock();
888  }
889  return true;
890  }
891 
892  ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
893  return false;
894  }
895 
896  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
897  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
898  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
899  boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
900  ros::Time prev_robot_motion_time = last_robot_motion_time_;
901  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
902  timeout > ros::WallDuration())
903  {
904  ROS_DEBUG_STREAM_NAMED(LOGNAME, "last robot motion: " << (t - last_robot_motion_time_).toSec() << " ago");
905  new_scene_update_condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
906  timeout -= ros::WallTime::now() - start; // compute remaining wait_time
907  }
908  bool success = last_robot_motion_time_ >= t;
909  // suppress warning if we received an update at all
910  if (!success && prev_robot_motion_time != last_robot_motion_time_)
911  ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs",
912  (t - last_robot_motion_time_).toSec());
913 
914  ROS_DEBUG_STREAM_NAMED(LOGNAME, "sync done: robot motion: " << (t - last_robot_motion_time_).toSec()
915  << " scene update: " << (t - last_update_time_).toSec());
916  return success;
917 }
918 
920 {
921  scene_update_mutex_.lock_shared();
922  if (octomap_monitor_)
923  octomap_monitor_->getOcTreePtr()->lockRead();
924 }
925 
927 {
928  if (octomap_monitor_)
929  octomap_monitor_->getOcTreePtr()->unlockRead();
930  scene_update_mutex_.unlock_shared();
931 }
932 
934 {
935  scene_update_mutex_.lock();
936  if (octomap_monitor_)
937  octomap_monitor_->getOcTreePtr()->lockWrite();
938 }
939 
941 {
942  if (octomap_monitor_)
943  octomap_monitor_->getOcTreePtr()->unlockWrite();
944  scene_update_mutex_.unlock();
945 }
946 
948 {
950 
951  ROS_INFO_NAMED(LOGNAME, "Starting scene monitor");
952  // listen for planning scene updates; these messages include transforms, so no need for filters
953  if (!scene_topic.empty())
954  {
957  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
958  }
959 }
960 
962 {
964  {
965  ROS_INFO_NAMED(LOGNAME, "Stopping scene monitor");
967  }
968 }
969 
971  const std::string& target_frame, const ros::Time& target_time,
973 {
974  if (!tf_)
975  return false;
976  try
977  {
978  boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
979 
980  for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
981  {
983  tf_->waitForTransform(target_frame, it->first->getName(), target_time, shape_transform_cache_lookup_wait_time_);
984  tf_->lookupTransform(target_frame, it->first->getName(), target_time, tr);
985  Eigen::Affine3d ttr;
986  tf::transformTFToEigen(tr, ttr);
987  for (std::size_t j = 0; j < it->second.size(); ++j)
988  cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
989  }
990  for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin();
991  it != attached_body_shape_handles_.end(); ++it)
992  {
994  tf_->waitForTransform(target_frame, it->first->getAttachedLinkName(), target_time,
996  tf_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time, tr);
997  Eigen::Affine3d transform;
998  tf::transformTFToEigen(tr, transform);
999  for (std::size_t k = 0; k < it->second.size(); ++k)
1000  cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
1001  }
1002  {
1004  tf_->waitForTransform(target_frame, scene_->getPlanningFrame(), target_time,
1006  tf_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time, tr);
1007  Eigen::Affine3d transform;
1008  tf::transformTFToEigen(tr, transform);
1009  for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin();
1010  it != collision_body_shape_handles_.end(); ++it)
1011  for (std::size_t k = 0; k < it->second.size(); ++k)
1012  cache[it->second[k].first] = transform * (*it->second[k].second);
1013  }
1014  }
1015  catch (tf::TransformException& ex)
1016  {
1017  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
1018  return false;
1019  }
1020  return true;
1021 }
1022 
1024  const std::string& collision_objects_topic, const std::string& planning_scene_world_topic,
1025  const bool load_octomap_monitor)
1026 {
1028  ROS_INFO_NAMED(LOGNAME, "Starting world geometry monitor");
1029 
1030  // listen for world geometry updates using message filters
1031  if (!collision_objects_topic.empty())
1032  {
1034  new message_filters::Subscriber<moveit_msgs::CollisionObject>(root_nh_, collision_objects_topic, 1024));
1035  if (tf_)
1036  {
1038  *collision_object_subscriber_, *tf_, scene_->getPlanningFrame(), 1024));
1039  collision_object_filter_->registerCallback(boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
1040  collision_object_filter_->registerFailureCallback(
1041  boost::bind(&PlanningSceneMonitor::collisionObjectFailTFCallback, this, _1, _2));
1042  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message notifier with target frame '%s'",
1043  root_nh_.resolveName(collision_objects_topic).c_str(),
1044  collision_object_filter_->getTargetFramesString().c_str());
1045  }
1046  else
1047  {
1048  collision_object_subscriber_->registerCallback(
1049  boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
1050  ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
1051  }
1052  }
1053 
1054  if (!planning_scene_world_topic.empty())
1055  {
1057  root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
1058  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
1059  root_nh_.resolveName(planning_scene_world_topic).c_str());
1060  }
1061 
1062  // Ocotomap monitor is optional
1063  if (load_octomap_monitor)
1064  {
1065  if (!octomap_monitor_)
1066  {
1067  octomap_monitor_.reset(new occupancy_map_monitor::OccupancyMapMonitor(tf_, scene_->getPlanningFrame()));
1071 
1072  octomap_monitor_->setTransformCacheCallback(
1073  boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
1074  octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
1075  }
1076  octomap_monitor_->startMonitor();
1077  }
1078 }
1079 
1081 {
1083  {
1084  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1085  collision_object_filter_.reset();
1088  }
1090  {
1091  ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
1093  }
1094  if (octomap_monitor_)
1095  octomap_monitor_->stopMonitor();
1096 }
1097 
1098 void planning_scene_monitor::PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1099  const std::string& attached_objects_topic)
1100 {
1101  stopStateMonitor();
1102  if (scene_)
1103  {
1106  current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
1107  current_state_monitor_->startStateMonitor(joint_states_topic);
1108 
1109  {
1110  boost::mutex::scoped_lock lock(state_pending_mutex_);
1111  if (!dt_state_update_.isZero())
1113  }
1114 
1115  if (!attached_objects_topic.empty())
1116  {
1117  // using regular message filter as there's no header
1119  root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
1120  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
1121  root_nh_.resolveName(attached_objects_topic).c_str());
1122  }
1123  }
1124  else
1125  ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
1126 }
1127 
1129 {
1131  current_state_monitor_->stopStateMonitor();
1134 
1135  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1137  {
1138  boost::mutex::scoped_lock lock(state_pending_mutex_);
1139  state_update_pending_ = false;
1140  }
1141 }
1142 
1144  const sensor_msgs::JointStateConstPtr& /* joint_state */)
1145 {
1146  const ros::WallTime& n = ros::WallTime::now();
1148 
1149  bool update = false;
1150  {
1151  boost::mutex::scoped_lock lock(state_pending_mutex_);
1152 
1153  if (dt < dt_state_update_)
1154  {
1155  state_update_pending_ = true;
1156  }
1157  else
1158  {
1159  state_update_pending_ = false;
1160  last_robot_state_update_wall_time_ = n;
1161  update = true;
1162  }
1163  }
1164  // run the state update with state_pending_mutex_ unlocked
1165  if (update)
1167 }
1168 
1170 {
1172  {
1173  bool update = false;
1174 
1175  const ros::WallTime& n = ros::WallTime::now();
1177 
1178  {
1179  // lock for access to dt_state_update_ and state_update_pending_
1180  boost::mutex::scoped_lock lock(state_pending_mutex_);
1182  {
1183  state_update_pending_ = false;
1184  last_robot_state_update_wall_time_ = ros::WallTime::now();
1185  update = true;
1187  "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
1188  }
1189  }
1190 
1191  // run the state update with state_pending_mutex_ unlocked
1192  if (update)
1193  {
1195  ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
1196  }
1197  }
1198 }
1199 
1201 {
1202  if (!octomap_monitor_)
1203  return;
1204 
1206  {
1207  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1209  octomap_monitor_->getOcTreePtr()->lockRead();
1210  try
1211  {
1212  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Affine3d::Identity());
1213  octomap_monitor_->getOcTreePtr()->unlockRead();
1214  }
1215  catch (...)
1216  {
1217  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1218  throw;
1219  }
1220  }
1222 }
1223 
1225 {
1226  bool update = false;
1227  if (hz > std::numeric_limits<double>::epsilon())
1228  {
1229  boost::mutex::scoped_lock lock(state_pending_mutex_);
1230  dt_state_update_.fromSec(1.0 / hz);
1233  }
1234  else
1235  {
1236  // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1238  boost::mutex::scoped_lock lock(state_pending_mutex_);
1241  update = true;
1242  }
1243  ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
1244 
1245  if (update)
1247 }
1248 
1250 {
1252  {
1253  std::vector<std::string> missing;
1254  if (!current_state_monitor_->haveCompleteState(missing) &&
1255  (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
1256  {
1257  std::string missing_str = boost::algorithm::join(missing, ", ");
1258  ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known. Missing %s",
1259  missing_str.c_str());
1260  }
1261 
1262  {
1263  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1265  ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
1266  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1267  scene_->getCurrentStateNonConst().update(); // compute all transforms
1268  }
1270  }
1271  else
1272  ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
1273 }
1274 
1276 {
1277  boost::recursive_mutex::scoped_lock lock(update_lock_);
1278  if (fn)
1279  update_callbacks_.push_back(fn);
1280 }
1281 
1283 {
1284  boost::recursive_mutex::scoped_lock lock(update_lock_);
1285  update_callbacks_.clear();
1286 }
1287 
1289 {
1291  ROS_DEBUG_NAMED(LOGNAME, "Maximum frquency for publishing a planning scene is now %lf Hz",
1293 }
1294 
1296  std::vector<geometry_msgs::TransformStamped>& transforms)
1297 {
1298  const std::string& target = getRobotModel()->getModelFrame();
1299 
1300  std::vector<std::string> all_frame_names;
1301  tf_->getFrameStrings(all_frame_names);
1302  for (std::size_t i = 0; i < all_frame_names.size(); ++i)
1303  {
1304  const std::string& frame_no_slash = (!all_frame_names[i].empty() && all_frame_names[i][0] == '/') ?
1305  all_frame_names[i].substr(1) :
1306  all_frame_names[i];
1307  const std::string& frame_with_slash =
1308  (!all_frame_names[i].empty() && all_frame_names[i][0] != '/') ? '/' + all_frame_names[i] : all_frame_names[i];
1309 
1310  if (frame_with_slash == target || getRobotModel()->hasLinkModel(frame_no_slash))
1311  continue;
1312 
1313  ros::Time stamp(0);
1314  std::string err_string;
1315  if (tf_->getLatestCommonTime(target, all_frame_names[i], stamp, &err_string) != tf::NO_ERROR)
1316  {
1317  ROS_WARN_STREAM_NAMED(LOGNAME, "No transform available between frame '"
1318  << all_frame_names[i] << "' and planning frame '" << target << "' ("
1319  << err_string << ")");
1320  continue;
1321  }
1322 
1324  try
1325  {
1326  tf_->lookupTransform(target, all_frame_names[i], stamp, t);
1327  }
1328  catch (tf::TransformException& ex)
1329  {
1330  ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
1331  << all_frame_names[i] << "' to planning frame '" << target << "' ("
1332  << ex.what() << ")");
1333  continue;
1334  }
1335  geometry_msgs::TransformStamped f;
1336  f.header.frame_id = frame_with_slash;
1337  f.child_frame_id = target;
1338  f.transform.translation.x = t.getOrigin().x();
1339  f.transform.translation.y = t.getOrigin().y();
1340  f.transform.translation.z = t.getOrigin().z();
1341  const tf::Quaternion& q = t.getRotation();
1342  f.transform.rotation.x = q.x();
1343  f.transform.rotation.y = q.y();
1344  f.transform.rotation.z = q.z();
1345  f.transform.rotation.w = q.w();
1346  transforms.push_back(f);
1347  }
1348 }
1349 
1351 {
1352  if (!tf_)
1353  return;
1354 
1355  if (scene_)
1356  {
1357  std::vector<geometry_msgs::TransformStamped> transforms;
1358  getUpdatedFrameTransforms(transforms);
1359  {
1360  boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
1361  scene_->getTransformsNonConst().setTransforms(transforms);
1363  }
1365  }
1366 }
1367 
1369 {
1370  if (octomap_monitor_)
1371  octomap_monitor_->publishDebugInformation(flag);
1372 }
1373 
1375  const planning_scene::PlanningScenePtr& scene)
1376 {
1377  if (!scene || robot_description_.empty())
1378  return;
1379  collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
1380 
1381  // read overriding values from the param server
1382 
1383  // first we do default collision operations
1384  if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
1385  ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
1386  else
1387  {
1388  ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
1389 
1390  XmlRpc::XmlRpcValue coll_ops;
1391  nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
1392 
1393  if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
1394  {
1395  ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
1396  return;
1397  }
1398 
1399  if (coll_ops.size() == 0)
1400  {
1401  ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
1402  return;
1403  }
1404 
1405  for (int i = 0; i < coll_ops.size(); ++i)
1406  {
1407  if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
1408  {
1409  ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
1410  continue;
1411  }
1412  acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
1413  std::string(coll_ops[i]["operation"]) == "disable");
1414  }
1415  }
1416 }
1417 
1419 {
1420  if (robot_description_.empty())
1421  {
1422  default_robot_padd_ = 0.0;
1423  default_robot_scale_ = 1.0;
1424  default_object_padd_ = 0.0;
1425  default_attached_padd_ = 0.0;
1426  return;
1427  }
1428 
1429  // Ensure no leading slash creates a bad param server address
1430  static const std::string robot_description =
1431  (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
1432 
1433  nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
1434  nh_.param(robot_description + "_planning/default_robot_scale", default_robot_scale_, 1.0);
1435  nh_.param(robot_description + "_planning/default_object_padding", default_object_padd_, 0.0);
1436  nh_.param(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
1437  nh_.param(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_,
1438  std::map<std::string, double>());
1439  nh_.param(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_,
1440  std::map<std::string, double>());
1441 
1442  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
1443  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
1444 }
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
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_
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)
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 Sat Apr 21 2018 02:55:20