moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 
41 #include <moveit_msgs/srv/get_planning_scene.hpp>
42 
43 #include <rclcpp/qos.hpp>
44 
45 // TODO: Remove conditional includes when released to all active distros.
46 #if __has_include(<tf2/exceptions.hpp>)
47 #include <tf2/exceptions.hpp>
48 #else
49 #include <tf2/exceptions.h>
50 #endif
51 #if __has_include(<tf2/LinearMath/Transform.hpp>)
52 #include <tf2/LinearMath/Transform.hpp>
53 #else
54 #include <tf2/LinearMath/Transform.h>
55 #endif
56 #include <tf2_eigen/tf2_eigen.hpp>
57 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
58 
59 #include <boost/algorithm/string/join.hpp>
60 #include <memory>
61 
62 #include <std_msgs/msg/string.hpp>
63 
64 #include <chrono>
65 using namespace std::chrono_literals;
66 
67 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.planning_scene_monitor.planning_scene_monitor");
68 
69 namespace planning_scene_monitor
70 {
71 const std::string PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC = "joint_states";
72 const std::string PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC = "attached_collision_object";
73 const std::string PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC = "collision_object";
74 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC = "planning_scene_world";
75 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC = "planning_scene";
76 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE = "get_planning_scene";
77 const std::string PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC = "monitored_planning_scene";
78 
79 PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const std::string& robot_description,
80  const std::string& name)
81  : PlanningSceneMonitor(node, planning_scene::PlanningScenePtr(), robot_description, name)
82 {
83 }
84 
85 PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node,
86  const planning_scene::PlanningScenePtr& scene,
87  const std::string& robot_description, const std::string& name)
88  : PlanningSceneMonitor(node, scene, std::make_shared<robot_model_loader::RobotModelLoader>(node, robot_description),
89  name)
90 {
91 }
92 
93 PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node,
94  const robot_model_loader::RobotModelLoaderPtr& rm_loader,
95  const std::string& name)
96  : PlanningSceneMonitor(node, planning_scene::PlanningScenePtr(), rm_loader, name)
97 {
98 }
99 
100 PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node,
101  const planning_scene::PlanningScenePtr& scene,
102  const robot_model_loader::RobotModelLoaderPtr& rm_loader,
103  const std::string& name)
104  : monitor_name_(name)
105  , node_(node)
106  , private_executor_(std::make_shared<rclcpp::executors::SingleThreadedExecutor>())
107  , tf_buffer_(std::make_shared<tf2_ros::Buffer>(node->get_clock()))
108  , dt_state_update_(0.0)
109  , shape_transform_cache_lookup_wait_time_(0, 0)
110  , rm_loader_(rm_loader)
111 {
112  std::vector<std::string> new_args = rclcpp::NodeOptions().arguments();
113  new_args.push_back("--ros-args");
114  new_args.push_back("-r");
115  // Add random ID to prevent warnings about multiple publishers within the same node
116  new_args.push_back(std::string("__node:=") + node_->get_name() + "_private_" +
117  std::to_string(reinterpret_cast<std::size_t>(this)));
118  new_args.push_back("--");
119  pnode_ = std::make_shared<rclcpp::Node>("_", node_->get_namespace(), rclcpp::NodeOptions().arguments(new_args));
120  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
121  tf_buffer_->setUsingDedicatedThread(true);
122  // use same callback queue as root_nh_
123  // root_nh_.setCallbackQueue(&queue_);
124  // nh_.setCallbackQueue(&queue_);
125  // spinner_.reset(new ros::AsyncSpinner(1, &queue_));
126  // spinner_->start();
127  initialize(scene);
128 
129  use_sim_time_ = node->get_parameter("use_sim_time").as_bool();
130 }
131 
133 {
134  if (scene_)
135  {
136  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
137  scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback());
138  }
143 
144  private_executor_->cancel();
145  if (private_executor_thread_.joinable())
147  private_executor_.reset();
148 
149  current_state_monitor_.reset();
150  scene_const_.reset();
151  scene_.reset();
152  parent_scene_.reset();
153  robot_model_.reset();
154  rm_loader_.reset();
155 }
156 
157 planning_scene::PlanningScenePtr PlanningSceneMonitor::copyPlanningScene(const moveit_msgs::msg::PlanningScene& diff)
158 {
159  // We cannot use LockedPlanningSceneRO for RAII because it requires a PSMPtr
160  // Instead assume clone will not throw
161  lockSceneRead();
163  unlockSceneRead();
164 
165  if (!moveit::core::isEmpty(diff))
166  scene->setPlanningSceneDiffMsg(diff);
167  return scene;
168 }
169 
170 void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& scene)
171 {
172  if (monitor_name_.empty())
173  monitor_name_ = "planning_scene_monitor";
174  robot_description_ = rm_loader_->getRobotDescription();
175  if (rm_loader_->getModel())
176  {
177  robot_model_ = rm_loader_->getModel();
178  scene_ = scene;
179  if (!scene_)
180  {
181  try
182  {
183  scene_ = std::make_shared<planning_scene::PlanningScene>(rm_loader_->getModel());
186 
187  scene_->getCollisionEnvNonConst()->setPadding(default_robot_padd_);
188  scene_->getCollisionEnvNonConst()->setScale(default_robot_scale_);
189  for (const std::pair<const std::string, double>& it : default_robot_link_padd_)
190  {
191  scene_->getCollisionEnvNonConst()->setLinkPadding(it.first, it.second);
192  }
193  for (const std::pair<const std::string, double>& it : default_robot_link_scale_)
194  {
195  scene_->getCollisionEnvNonConst()->setLinkScale(it.first, it.second);
196  }
197  }
198  catch (moveit::ConstructException& e)
199  {
200  RCLCPP_ERROR(LOGGER, "Configuration of planning scene failed");
201  scene_.reset();
202  }
203  }
204  // scene_const_ is set regardless if scene_ is null or not
206  if (scene_)
207  {
208  // The scene_ is loaded on the collision loader only if it was correctly instantiated
209  collision_loader_.setupScene(node_, scene_);
210  scene_->setAttachedBodyUpdateCallback([this](moveit::core::AttachedBody* body, bool attached) {
212  });
213  scene_->setCollisionObjectUpdateCallback(
214  [this](const collision_detection::World::ObjectConstPtr& object, collision_detection::World::Action action) {
216  });
217  }
218  }
219  else
220  {
221  RCLCPP_ERROR(LOGGER, "Robot model not loaded");
222  }
223 
226 
227  last_update_time_ = last_robot_motion_time_ = rclcpp::Clock().now();
228  last_robot_state_update_wall_time_ = std::chrono::system_clock::now();
229  dt_state_update_ = std::chrono::duration<double>(0.03);
230 
231  double temp_wait_time = 0.05;
232 
233  if (!robot_description_.empty())
234  node_->get_parameter_or(robot_description_ + "_planning.shape_transform_cache_lookup_wait_time", temp_wait_time,
235  temp_wait_time);
236 
237  shape_transform_cache_lookup_wait_time_ = rclcpp::Duration::from_seconds(temp_wait_time);
238 
239  state_update_pending_.store(false);
240  // Period for 0.1 sec
241  using std::chrono::nanoseconds;
242  state_update_timer_ = pnode_->create_wall_timer(dt_state_update_, [this]() { return stateUpdateTimerCallback(); });
243  private_executor_->add_node(pnode_);
244 
245  // start executor on a different thread now
246  private_executor_thread_ = std::thread([this]() { private_executor_->spin(); });
247 
248  auto declare_parameter = [this](const std::string& param_name, auto default_val,
249  const std::string& description) -> auto
250  {
251  rcl_interfaces::msg::ParameterDescriptor desc;
252  desc.set__description(description);
253  return pnode_->declare_parameter(param_name, default_val, desc);
254  };
255 
256  try
257  {
258  // Set up publishing parameters
259  bool publish_planning_scene =
260  declare_parameter("publish_planning_scene", false, "Set to True to publish Planning Scenes");
261  bool publish_geometry_updates = declare_parameter("publish_geometry_updates", false,
262  "Set to True to publish geometry updates of the planning scene");
263  bool publish_state_updates =
264  declare_parameter("publish_state_updates", false, "Set to True to publish state updates of the planning scene");
265  bool publish_transform_updates = declare_parameter(
266  "publish_transforms_updates", false, "Set to True to publish transform updates of the planning scene");
267  double publish_planning_scene_hz = declare_parameter(
268  "publish_planning_scene_hz", 4.0, "Set the maximum frequency at which planning scene updates are published");
269  updatePublishSettings(publish_geometry_updates, publish_state_updates, publish_transform_updates,
270  publish_planning_scene, publish_planning_scene_hz);
271  }
272  catch (const rclcpp::exceptions::InvalidParameterTypeException& e)
273  {
274  RCLCPP_ERROR_STREAM(LOGGER, "Invalid parameter type in PlanningSceneMonitor: " << e.what());
275  RCLCPP_ERROR(LOGGER, "Dynamic publishing parameters won't be available");
276  return;
277  }
278 
279  auto psm_parameter_set_callback = [this](const std::vector<rclcpp::Parameter>& parameters) -> auto
280  {
281  auto result = rcl_interfaces::msg::SetParametersResult();
282  result.successful = true;
283 
284  bool publish_planning_scene = false, publish_geometry_updates = false, publish_state_updates = false,
285  publish_transform_updates = false;
286  double publish_planning_scene_hz = 4.0;
287  bool declared_params_valid = pnode_->get_parameter("publish_planning_scene", publish_planning_scene) &&
288  pnode_->get_parameter("publish_geometry_updates", publish_geometry_updates) &&
289  pnode_->get_parameter("publish_state_updates", publish_state_updates) &&
290  pnode_->get_parameter("publish_transforms_updates", publish_transform_updates) &&
291  pnode_->get_parameter("publish_planning_scene_hz", publish_planning_scene_hz);
292 
293  if (!declared_params_valid)
294  {
295  RCLCPP_ERROR(LOGGER, "Initially declared parameters are invalid - failed to process update callback");
296  result.successful = false;
297  return result;
298  }
299 
300  for (const auto& parameter : parameters)
301  {
302  const auto& name = parameter.get_name();
303  const auto& type = parameter.get_type();
304 
305  // Only allow already declared parameters with same value type
306  if (!pnode_->has_parameter(name) || pnode_->get_parameter(name).get_type() != type)
307  {
308  RCLCPP_ERROR(LOGGER, "Invalid parameter in PlanningSceneMonitor parameter callback");
309  result.successful = false;
310  return result;
311  }
312 
313  // Update parameter values
314  if (name == "planning_scene_monitor.publish_planning_scene")
315  publish_planning_scene = parameter.as_bool();
316  else if (name == "planning_scene_monitor.publish_geometry_updates")
317  publish_geometry_updates = parameter.as_bool();
318  else if (name == "planning_scene_monitor.publish_state_updates")
319  publish_state_updates = parameter.as_bool();
320  else if (name == "planning_scene_monitor.publish_transforms_updates")
321  publish_transform_updates = parameter.as_bool();
322  else if (name == "planning_scene_monitor.publish_planning_scene_hz")
323  publish_planning_scene_hz = parameter.as_double();
324  }
325 
326  if (result.successful)
327  updatePublishSettings(publish_geometry_updates, publish_state_updates, publish_transform_updates,
328  publish_planning_scene, publish_planning_scene_hz);
329  return result;
330  };
331 
332  callback_handler_ = pnode_->add_on_set_parameters_callback(psm_parameter_set_callback);
333 }
334 
336 {
337  if (scene_)
338  {
339  if (flag)
340  {
341  std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
342  if (scene_)
343  {
344  scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback());
345  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
346  scene_->decoupleParent();
348  scene_ = parent_scene_->diff();
350  scene_->setAttachedBodyUpdateCallback([this](moveit::core::AttachedBody* body, bool attached) {
352  });
353  scene_->setCollisionObjectUpdateCallback(
354  [this](const collision_detection::World::ObjectConstPtr& object,
356  }
357  }
358  else
359  {
361  {
362  RCLCPP_WARN(LOGGER, "Diff monitoring was stopped while publishing planning scene diffs. "
363  "Stopping planning scene diff publisher");
365  }
366  {
367  std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
368  if (scene_)
369  {
370  scene_->decoupleParent();
371  parent_scene_.reset();
372  // remove the '+' added by .diff() at the end of the scene name
373  if (!scene_->getName().empty())
374  {
375  if (scene_->getName()[scene_->getName().length() - 1] == '+')
376  scene_->setName(scene_->getName().substr(0, scene_->getName().length() - 1));
377  }
378  }
379  }
380  }
381  }
382 }
383 
385 {
387  {
388  std::unique_ptr<std::thread> copy;
389  copy.swap(publish_planning_scene_);
390  new_scene_update_condition_.notify_all();
391  copy->join();
392  monitorDiffs(false);
394  RCLCPP_INFO(LOGGER, "Stopped publishing maintained planning scene.");
395  }
396 }
397 
399  const std::string& planning_scene_topic)
400 {
401  publish_update_types_ = update_type;
403  {
404  planning_scene_publisher_ = pnode_->create_publisher<moveit_msgs::msg::PlanningScene>(planning_scene_topic, 100);
405  RCLCPP_INFO(LOGGER, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
406  monitorDiffs(true);
407  publish_planning_scene_ = std::make_unique<std::thread>([this] { scenePublishingThread(); });
408  }
409 }
410 
411 void PlanningSceneMonitor::scenePublishingThread()
412 {
413  RCLCPP_DEBUG(LOGGER, "Started scene publishing thread ...");
414 
415  // publish the full planning scene once
416  {
417  moveit_msgs::msg::PlanningScene msg;
418  {
420  if (octomap_monitor_)
421  lock = octomap_monitor_->getOcTreePtr()->reading();
422  scene_->getPlanningSceneMsg(msg);
423  }
424  planning_scene_publisher_->publish(msg);
425  RCLCPP_DEBUG(LOGGER, "Published the full planning scene: '%s'", msg.name.c_str());
426  }
427 
428  do
429  {
430  moveit_msgs::msg::PlanningScene msg;
431  bool publish_msg = false;
432  bool is_full = false;
433  rclcpp::Rate rate(publish_planning_scene_frequency_);
434  {
435  std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
437  new_scene_update_condition_.wait(ulock);
439  {
441  {
443  is_full = true;
444  else
445  {
447  if (octomap_monitor_)
448  lock = octomap_monitor_->getOcTreePtr()->reading();
449  scene_->getPlanningSceneDiffMsg(msg);
451  {
452  msg.robot_state.attached_collision_objects.clear();
453  msg.robot_state.is_diff = true;
454  }
455  }
456  std::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the
457  // transform cache to
458  // update while we are
459  // potentially changing
460  // attached bodies
461  scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback());
462  scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
463  scene_->pushDiffs(parent_scene_);
464  scene_->clearDiffs();
465  scene_->setAttachedBodyUpdateCallback([this](moveit::core::AttachedBody* body, bool attached) {
467  });
468  scene_->setCollisionObjectUpdateCallback(
469  [this](const collision_detection::World::ObjectConstPtr& object,
471  if (octomap_monitor_)
472  {
473  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
474  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
475  }
476  if (is_full)
477  {
479  if (octomap_monitor_)
480  lock = octomap_monitor_->getOcTreePtr()->reading();
481  scene_->getPlanningSceneMsg(msg);
482  }
483  // also publish timestamp of this robot_state
484  msg.robot_state.joint_state.header.stamp = last_robot_motion_time_;
485  publish_msg = true;
486  }
488  }
489  }
490  if (publish_msg)
491  {
492  planning_scene_publisher_->publish(msg);
493  if (is_full)
494  RCLCPP_DEBUG(LOGGER, "Published full planning scene: '%s'", msg.name.c_str());
495  // finish thread on rclcpp shutdown (otherwise rate.sleep() will crash)
496  if (!rclcpp::ok())
497  break;
498  rate.sleep();
499  }
500  } while (publish_planning_scene_);
501 }
502 
503 void PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string>& topics) const
504 {
505  // TODO(anasarrak): Do we need this for ROS2?
506  topics.clear();
508  {
509  const std::string& t = current_state_monitor_->getMonitoredTopic();
510  if (!t.empty())
511  topics.push_back(t);
512  }
514  topics.push_back(planning_scene_subscriber_->get_topic_name());
516  // TODO (anasarrak) This has been changed to subscriber on Moveit, look at
517  // https://github.com/ros-planning/moveit/pull/1406/files/cb9488312c00e9c8949d89b363766f092330954d#diff-fb6e26ecc9a73d59dbdae3f3e08145e6
518  topics.push_back(collision_object_subscriber_->get_topic_name());
520  topics.push_back(planning_scene_world_subscriber_->get_topic_name());
521 }
522 
523 namespace
524 {
525 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr& scene,
526  const planning_scene::PlanningScene* possible_parent)
527 {
528  if (scene && scene.get() == possible_parent)
529  return true;
530  if (scene)
531  return sceneIsParentOf(scene->getParent(), possible_parent);
532  return false;
533 }
534 } // namespace
535 
536 bool PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr& scene) const
537 {
538  return sceneIsParentOf(scene_const_, scene.get());
539 }
540 
541 bool PlanningSceneMonitor::updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const
542 {
543  return sceneIsParentOf(scene_const_, scene.get());
544 }
545 
547 {
548  // do not modify update functions while we are calling them
549  std::scoped_lock lock(update_lock_);
550 
551  for (std::function<void(SceneUpdateType)>& update_callback : update_callbacks_)
552  update_callback(update_type);
553  new_scene_update_ = (SceneUpdateType)(static_cast<int>(new_scene_update_) | static_cast<int>(update_type));
554  new_scene_update_condition_.notify_all();
555 }
556 
557 bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_name)
558 {
559  if (get_scene_service_ && get_scene_service_->get_service_name() == service_name)
560  {
561  RCLCPP_FATAL_STREAM(LOGGER, "requestPlanningSceneState() to self-provided service '" << service_name << "'");
562  throw std::runtime_error("requestPlanningSceneState() to self-provided service: " + service_name);
563  }
564  // use global namespace for service
565  auto client = pnode_->create_client<moveit_msgs::srv::GetPlanningScene>(service_name);
566  auto srv = std::make_shared<moveit_msgs::srv::GetPlanningScene::Request>();
567  srv->components.components = srv->components.SCENE_SETTINGS | srv->components.ROBOT_STATE |
568  srv->components.ROBOT_STATE_ATTACHED_OBJECTS | srv->components.WORLD_OBJECT_NAMES |
569  srv->components.WORLD_OBJECT_GEOMETRY | srv->components.OCTOMAP |
570  srv->components.TRANSFORMS | srv->components.ALLOWED_COLLISION_MATRIX |
571  srv->components.LINK_PADDING_AND_SCALING | srv->components.OBJECT_COLORS;
572 
573  // Make sure client is connected to server
574  RCLCPP_DEBUG(LOGGER, "Waiting for GetPlanningScene service `%s` to exist.", service_name.c_str());
575  if (client->wait_for_service(std::chrono::seconds(5)))
576  {
577  RCLCPP_DEBUG(LOGGER, "Sending service request to `%s`.", service_name.c_str());
578  auto service_result = client->async_send_request(srv);
579  const auto service_result_status = service_result.wait_for(std::chrono::seconds(5));
580  if (service_result_status == std::future_status::ready) // Success
581  {
582  RCLCPP_DEBUG(LOGGER, "Service request succeeded, applying new planning scene");
583  newPlanningSceneMessage(service_result.get()->scene);
584  return true;
585  }
586  if (service_result_status == std::future_status::timeout) // Timeout
587  {
588  RCLCPP_INFO(LOGGER, "GetPlanningScene service call to %s timed out. at %s:%d", service_name.c_str(), __FILE__,
589  __LINE__);
590  return false;
591  }
592  }
593 
594  // If we are here, service is not available or call failed
595  RCLCPP_INFO(LOGGER,
596  "Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?",
597  service_name.c_str());
598 
599  return false;
600 }
601 
602 void PlanningSceneMonitor::providePlanningSceneService(const std::string& service_name)
603 {
604  // Load the service
605  get_scene_service_ = pnode_->create_service<moveit_msgs::srv::GetPlanningScene>(
606  service_name, [this](const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req,
607  const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res) {
608  return getPlanningSceneServiceCallback(req, res);
609  });
610 }
611 
612 void PlanningSceneMonitor::getPlanningSceneServiceCallback(
613  const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req,
614  const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res)
615 {
616  if (req->components.components & moveit_msgs::msg::PlanningSceneComponents::TRANSFORMS)
618 
619  moveit_msgs::msg::PlanningSceneComponents all_components;
620  all_components.components = UINT_MAX; // Return all scene components if nothing is specified.
621 
622  std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
623  scene_->getPlanningSceneMsg(res->scene, req->components.components ? req->components : all_components);
624 }
625 
626 void PlanningSceneMonitor::updatePublishSettings(bool publish_geom_updates, bool publish_state_updates,
627  bool publish_transform_updates, bool publish_planning_scene,
628  double publish_planning_scene_hz)
629 {
631  if (publish_geom_updates)
632  event = (PlanningSceneMonitor::SceneUpdateType)(static_cast<int>(event) |
633  static_cast<int>(PlanningSceneMonitor::UPDATE_GEOMETRY));
634  if (publish_state_updates)
635  event = (PlanningSceneMonitor::SceneUpdateType)(static_cast<int>(event) |
636  static_cast<int>(PlanningSceneMonitor::UPDATE_STATE));
637  if (publish_transform_updates)
638  event = (PlanningSceneMonitor::SceneUpdateType)(static_cast<int>(event) |
639  static_cast<int>(PlanningSceneMonitor::UPDATE_TRANSFORMS));
640  if (publish_planning_scene)
641  {
642  setPlanningScenePublishingFrequency(publish_planning_scene_hz);
644  }
645  else
647 }
648 
649 void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene)
650 {
652 }
653 
655 {
656  bool removed = false;
657  {
658  std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
659  removed = scene_->getWorldNonConst()->removeObject(scene_->OCTOMAP_NS);
660 
661  if (octomap_monitor_)
662  {
663  octomap_monitor_->getOcTreePtr()->lockWrite();
664  octomap_monitor_->getOcTreePtr()->clear();
665  octomap_monitor_->getOcTreePtr()->unlockWrite();
666  }
667  else
668  {
669  RCLCPP_WARN(LOGGER, "Unable to clear octomap since no octomap monitor has been initialized");
670  } // Lift the scoped lock before calling triggerSceneUpdateEvent to avoid deadlock
671  }
672 
673  if (removed)
675 }
676 
677 bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene& scene)
678 {
679  if (!scene_)
680  return false;
681 
682  bool result;
683 
685  std::string old_scene_name;
686  {
687  std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
688  // we don't want the transform cache to update while we are potentially changing attached bodies
689  std::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
690 
691  last_update_time_ = rclcpp::Clock().now();
692  last_robot_motion_time_ = scene.robot_state.joint_state.header.stamp;
693  RCLCPP_DEBUG(LOGGER, "scene update %f robot stamp: %f", fmod(last_update_time_.seconds(), 10.),
694  fmod(last_robot_motion_time_.seconds(), 10.));
695  old_scene_name = scene_->getName();
696 
697  if (!scene.is_diff && parent_scene_)
698  {
699  // If there is no new robot_state, transfer RobotState from current scene to parent scene
700  if (scene.robot_state.is_diff)
701  parent_scene_->setCurrentState(scene_->getCurrentState());
702  // clear maintained (diff) scene_ and set the full new scene in parent_scene_ instead
703  scene_->clearDiffs();
704  result = parent_scene_->setPlanningSceneMsg(scene);
705  // There were no callbacks for individual object changes, so rebuild the octree masks
708  }
709  else
710  {
711  result = scene_->setPlanningSceneDiffMsg(scene);
712  }
713 
714  if (octomap_monitor_)
715  {
716  if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
717  {
718  octomap_monitor_->getOcTreePtr()->lockWrite();
719  octomap_monitor_->getOcTreePtr()->clear();
720  octomap_monitor_->getOcTreePtr()->unlockWrite();
721  }
722  }
723  robot_model_ = scene_->getRobotModel();
724 
725  if (octomap_monitor_)
726  {
727  excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
728  excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
729  }
730  }
731 
732  // if we have a diff, try to more accurately determine the update type
733  if (scene.is_diff)
734  {
735  bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
736  scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
737  scene.link_scale.empty();
738  if (no_other_scene_upd)
739  {
740  upd = UPDATE_NONE;
741  if (!moveit::core::isEmpty(scene.world))
742  upd = (SceneUpdateType)(static_cast<int>(upd) | static_cast<int>(UPDATE_GEOMETRY));
743 
744  if (!scene.fixed_frame_transforms.empty())
745  upd = (SceneUpdateType)(static_cast<int>(upd) | static_cast<int>(UPDATE_TRANSFORMS));
746 
747  if (!moveit::core::isEmpty(scene.robot_state))
748  {
749  upd = (SceneUpdateType)(static_cast<int>(upd) | static_cast<int>(UPDATE_STATE));
750  if (!scene.robot_state.attached_collision_objects.empty() || !scene.robot_state.is_diff)
751  upd = (SceneUpdateType)(static_cast<int>(upd) | static_cast<int>(UPDATE_GEOMETRY));
752  }
753  }
754  }
756  return result;
757 }
758 
760  const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world)
761 {
762  if (scene_)
763  {
765  {
766  std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
767  last_update_time_ = rclcpp::Clock().now();
768  scene_->getWorldNonConst()->clearObjects();
769  scene_->processPlanningSceneWorldMsg(*world);
770  if (octomap_monitor_)
771  {
772  if (world->octomap.octomap.data.empty())
773  {
774  octomap_monitor_->getOcTreePtr()->lockWrite();
775  octomap_monitor_->getOcTreePtr()->clear();
776  octomap_monitor_->getOcTreePtr()->unlockWrite();
777  }
778  }
779  }
781  }
782 }
783 
784 void PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj)
785 {
786  if (!scene_)
787  return;
788 
790  {
791  std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
792  last_update_time_ = rclcpp::Clock().now();
793  if (!scene_->processCollisionObjectMsg(*obj))
794  return;
795  }
797 }
798 
799 void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj)
800 {
801  if (scene_)
802  {
804  {
805  std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
806  last_update_time_ = rclcpp::Clock().now();
807  scene_->processAttachedCollisionObjectMsg(*obj);
808  }
810  }
811 }
812 
814 {
815  if (!octomap_monitor_)
816  return;
817 
818  std::scoped_lock _(shape_handles_lock_);
819 
821  const std::vector<const moveit::core::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
822  auto start = std::chrono::system_clock::now();
823  bool warned = false;
824  for (const moveit::core::LinkModel* link : links)
825  {
826  std::vector<shapes::ShapeConstPtr> shapes = link->getShapes(); // copy shared ptrs on purpuse
827  for (std::size_t j = 0; j < shapes.size(); ++j)
828  {
829  // merge mesh vertices up to 0.1 mm apart
830  if (shapes[j]->type == shapes::MESH)
831  {
832  shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
833  m->mergeVertices(1e-4);
834  shapes[j].reset(m);
835  }
836 
838  if (h)
839  link_shape_handles_[link].push_back(std::make_pair(h, j));
840  }
841 
842  if (!warned && ((std::chrono::system_clock::now() - start) > std::chrono::seconds(30)))
843  {
844  RCLCPP_WARN(LOGGER, "It is likely there are too many vertices in collision geometry");
845  warned = true;
846  }
847  }
848 }
849 
851 {
852  if (!octomap_monitor_)
853  return;
854 
855  std::scoped_lock _(shape_handles_lock_);
856 
857  for (std::pair<const moveit::core::LinkModel* const,
858  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
860  for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : link_shape_handle.second)
861  octomap_monitor_->forgetShape(it.first);
862  link_shape_handles_.clear();
863 }
864 
866 {
867  if (!octomap_monitor_)
868  return;
869 
870  std::scoped_lock _(shape_handles_lock_);
871 
872  // clear information about any attached body, without referring to the AttachedBody* ptr (could be invalid)
873  for (std::pair<const moveit::core::AttachedBody* const,
874  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& attached_body_shape_handle :
876  for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : attached_body_shape_handle.second)
877  octomap_monitor_->forgetShape(it.first);
879 }
880 
882 {
883  std::scoped_lock _(shape_handles_lock_);
884 
886  // add attached objects again
887  std::vector<const moveit::core::AttachedBody*> ab;
888  scene_->getCurrentState().getAttachedBodies(ab);
889  for (const moveit::core::AttachedBody* body : ab)
891 }
892 
894 {
895  if (!octomap_monitor_)
896  return;
897 
898  std::scoped_lock _(shape_handles_lock_);
899 
900  // clear information about any attached object
901  for (std::pair<const std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
902  collision_body_shape_handle : collision_body_shape_handles_)
903  for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
904  collision_body_shape_handle.second)
905  octomap_monitor_->forgetShape(it.first);
907 }
908 
910 {
911  std::scoped_lock _(shape_handles_lock_);
912 
914  for (const std::pair<const std::string, collision_detection::World::ObjectPtr>& it : *scene_->getWorld())
915  excludeWorldObjectFromOctree(it.second);
916 }
917 
919 {
920  if (!octomap_monitor_)
921  return;
922 
923  std::scoped_lock _(shape_handles_lock_);
924  bool found = false;
925  for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
926  {
927  if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
928  continue;
929  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
930  if (h)
931  {
932  found = true;
933  attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
934  }
935  }
936  if (found)
937  RCLCPP_DEBUG(LOGGER, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
938 }
939 
941 {
942  if (!octomap_monitor_)
943  return;
944 
945  std::scoped_lock _(shape_handles_lock_);
946 
947  AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
948  if (it != attached_body_shape_handles_.end())
949  {
950  for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& shape_handle : it->second)
951  octomap_monitor_->forgetShape(shape_handle.first);
952  RCLCPP_DEBUG(LOGGER, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
954  }
955 }
956 
957 void PlanningSceneMonitor::excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj)
958 {
959  if (!octomap_monitor_)
960  return;
961 
962  std::scoped_lock _(shape_handles_lock_);
963 
964  bool found = false;
965  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
966  {
967  if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
968  continue;
969  occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
970  if (h)
971  {
972  collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->global_shape_poses_[i]));
973  found = true;
974  }
975  }
976  if (found)
977  RCLCPP_DEBUG(LOGGER, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
978 }
979 
980 void PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj)
981 {
982  if (!octomap_monitor_)
983  return;
984 
985  std::scoped_lock _(shape_handles_lock_);
986 
987  CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
988  if (it != collision_body_shape_handles_.end())
989  {
990  for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& shape_handle : it->second)
991  octomap_monitor_->forgetShape(shape_handle.first);
992  RCLCPP_DEBUG(LOGGER, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
994  }
995 }
996 
998  bool just_attached)
999 {
1000  if (!octomap_monitor_)
1001  return;
1002 
1003  if (just_attached)
1004  excludeAttachedBodyFromOctree(attached_body);
1005  else
1006  includeAttachedBodyInOctree(attached_body);
1007 }
1008 
1009 void PlanningSceneMonitor::currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& obj,
1011 {
1012  if (!octomap_monitor_)
1013  return;
1015  return;
1016 
1021  else
1022  {
1025  }
1026 }
1027 
1028 bool PlanningSceneMonitor::waitForCurrentRobotState(const rclcpp::Time& t, double wait_time)
1029 {
1030  if (t.nanoseconds() == 0)
1031  return false;
1032  RCLCPP_DEBUG(LOGGER, "sync robot state to: %.3fs", fmod(t.seconds(), 10.));
1033 
1035  {
1036  // Wait for next robot update in state monitor.
1037  bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
1038 
1039  /* As robot updates are passed to the planning scene only in throttled fashion, there might
1040  be still an update pending. If so, explicitly update the planning scene here.
1041  If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
1042  if (success)
1043  {
1044  if (state_update_pending_.load()) // perform state update
1045  {
1047  }
1048  return true;
1049  }
1050 
1051  RCLCPP_WARN(LOGGER, "Failed to fetch current robot state.");
1052  return false;
1053  }
1054 
1055  // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
1056  // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
1057  // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
1058  auto start = node_->get_clock()->now();
1059  auto timeout = rclcpp::Duration::from_seconds(wait_time);
1060  std::shared_lock<std::shared_mutex> lock(scene_update_mutex_);
1061  rclcpp::Time prev_robot_motion_time = last_robot_motion_time_;
1062  while (last_robot_motion_time_ < t && // Wait until the state update actually reaches the scene.
1063  timeout > rclcpp::Duration(0, 0))
1064  {
1065  RCLCPP_DEBUG(LOGGER, "last robot motion: %f ago", (t - last_robot_motion_time_).seconds());
1066  new_scene_update_condition_.wait_for(lock, std::chrono::nanoseconds(timeout.nanoseconds()));
1067  timeout = timeout - (node_->get_clock()->now() - start); // compute remaining wait_time
1068  }
1069  bool success = last_robot_motion_time_ >= t;
1070  // suppress warning if we received an update at all
1071  if (!success && prev_robot_motion_time != last_robot_motion_time_)
1072  RCLCPP_WARN(LOGGER, "Maybe failed to update robot state, time diff: %.3fs", (t - last_robot_motion_time_).seconds());
1073 
1074  RCLCPP_DEBUG(LOGGER, "sync done: robot motion: %f scene update: %f", (t - last_robot_motion_time_).seconds(),
1075  (t - last_update_time_).seconds());
1076  return success;
1077 }
1078 
1080 {
1081  scene_update_mutex_.lock_shared();
1082  if (octomap_monitor_)
1083  octomap_monitor_->getOcTreePtr()->lockRead();
1084 }
1085 
1087 {
1088  if (octomap_monitor_)
1089  octomap_monitor_->getOcTreePtr()->unlockRead();
1090  scene_update_mutex_.unlock_shared();
1091 }
1092 
1094 {
1095  scene_update_mutex_.lock();
1096  if (octomap_monitor_)
1097  octomap_monitor_->getOcTreePtr()->lockWrite();
1098 }
1099 
1101 {
1102  if (octomap_monitor_)
1103  octomap_monitor_->getOcTreePtr()->unlockWrite();
1104  scene_update_mutex_.unlock();
1105 }
1106 
1107 void PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
1108 {
1109  stopSceneMonitor();
1110 
1111  RCLCPP_INFO(LOGGER, "Starting planning scene monitor");
1112  // listen for planning scene updates; these messages include transforms, so no need for filters
1113  if (!scene_topic.empty())
1114  {
1115  planning_scene_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::PlanningScene>(
1116  scene_topic, rclcpp::ServicesQoS(), [this](const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) {
1117  return newPlanningSceneCallback(scene);
1118  });
1119  RCLCPP_INFO(LOGGER, "Listening to '%s'", planning_scene_subscriber_->get_topic_name());
1120  }
1121 }
1122 
1124 {
1126  {
1127  RCLCPP_INFO(LOGGER, "Stopping planning scene monitor");
1129  }
1130 }
1131 
1132 bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_frame, const rclcpp::Time& target_time,
1134 {
1135  try
1136  {
1137  std::scoped_lock _(shape_handles_lock_);
1138 
1139  for (const std::pair<const moveit::core::LinkModel* const,
1140  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
1142  {
1143  if (tf_buffer_->canTransform(target_frame, link_shape_handle.first->getName(), target_time,
1144  shape_transform_cache_lookup_wait_time_))
1145  {
1146  Eigen::Isometry3d ttr = tf2::transformToEigen(
1147  tf_buffer_->lookupTransform(target_frame, link_shape_handle.first->getName(), target_time));
1148  for (std::size_t j = 0; j < link_shape_handle.second.size(); ++j)
1149  cache[link_shape_handle.second[j].first] =
1150  ttr * link_shape_handle.first->getCollisionOriginTransforms()[link_shape_handle.second[j].second];
1151  }
1152  }
1153  for (const std::pair<const moveit::core::AttachedBody* const,
1154  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>&
1155  attached_body_shape_handle : attached_body_shape_handles_)
1156  {
1157  if (tf_buffer_->canTransform(target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time,
1158  shape_transform_cache_lookup_wait_time_))
1159  {
1160  Eigen::Isometry3d transform = tf2::transformToEigen(tf_buffer_->lookupTransform(
1161  target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time));
1162  for (std::size_t k = 0; k < attached_body_shape_handle.second.size(); ++k)
1163  cache[attached_body_shape_handle.second[k].first] =
1164  transform *
1165  attached_body_shape_handle.first->getShapePosesInLinkFrame()[attached_body_shape_handle.second[k].second];
1166  }
1167  }
1168  {
1169  if (tf_buffer_->canTransform(target_frame, scene_->getPlanningFrame(), target_time,
1170  shape_transform_cache_lookup_wait_time_))
1171  {
1172  Eigen::Isometry3d transform =
1173  tf2::transformToEigen(tf_buffer_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time));
1174  for (const std::pair<const std::string,
1175  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
1176  collision_body_shape_handle : collision_body_shape_handles_)
1177  for (const std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
1178  collision_body_shape_handle.second)
1179  cache[it.first] = transform * (*it.second);
1180  }
1181  }
1182  }
1183  catch (tf2::TransformException& ex)
1184  {
1185  rclcpp::Clock steady_clock = rclcpp::Clock();
1186  RCLCPP_ERROR_THROTTLE(LOGGER, steady_clock, 1000, "Transform error: %s", ex.what());
1187  return false;
1188  }
1189  return true;
1190 }
1191 
1192 void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collision_objects_topic,
1193  const std::string& planning_scene_world_topic,
1194  const bool load_octomap_monitor)
1195 {
1197  RCLCPP_INFO(LOGGER, "Starting world geometry update monitor for collision objects, attached objects, octomap "
1198  "updates.");
1199 
1200  // Listen to the /collision_objects topic to detect requests to add/remove/update collision objects to/from the world
1201  if (!collision_objects_topic.empty())
1202  {
1203  collision_object_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::CollisionObject>(
1204  collision_objects_topic, rclcpp::ServicesQoS(),
1205  [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return collisionObjectCallback(obj); });
1206  RCLCPP_INFO(LOGGER, "Listening to '%s'", collision_objects_topic.c_str());
1207  }
1208 
1209  if (!planning_scene_world_topic.empty())
1210  {
1211  planning_scene_world_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::PlanningSceneWorld>(
1212  planning_scene_world_topic, rclcpp::ServicesQoS(),
1213  [this](const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) {
1214  return newPlanningSceneWorldCallback(world);
1215  });
1216  RCLCPP_INFO(LOGGER, "Listening to '%s' for planning scene world geometry", planning_scene_world_topic.c_str());
1217  }
1218 
1219  // Ocotomap monitor is optional
1220  if (load_octomap_monitor)
1221  {
1222  if (!octomap_monitor_)
1223  {
1225  std::make_unique<occupancy_map_monitor::OccupancyMapMonitor>(node_, tf_buffer_, scene_->getPlanningFrame());
1229 
1230  octomap_monitor_->setTransformCacheCallback([this](const std::string& frame, const rclcpp::Time& stamp,
1232  return getShapeTransformCache(frame, stamp, cache);
1233  });
1234  octomap_monitor_->setUpdateCallback([this] { octomapUpdateCallback(); });
1235  }
1236  octomap_monitor_->startMonitor();
1237  }
1238 }
1239 
1241 {
1243  {
1244  RCLCPP_INFO(LOGGER, "Stopping world geometry monitor");
1246  }
1248  {
1249  RCLCPP_INFO(LOGGER, "Stopping world geometry monitor");
1251  }
1252  if (octomap_monitor_)
1253  octomap_monitor_->stopMonitor();
1254 }
1255 
1256 void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
1257  const std::string& attached_objects_topic)
1258 {
1259  stopStateMonitor();
1260  if (scene_)
1261  {
1263  {
1265  std::make_shared<CurrentStateMonitor>(pnode_, getRobotModel(), tf_buffer_, use_sim_time_);
1266  }
1267  current_state_monitor_->addUpdateCallback(
1268  [this](const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state) { return onStateUpdate(joint_state); });
1269  current_state_monitor_->startStateMonitor(joint_states_topic);
1270 
1271  {
1272  std::unique_lock<std::mutex> lock(state_update_mutex_);
1273  if (dt_state_update_.count() > 0)
1274  // ROS original: state_update_timer_.start();
1275  // TODO: re-enable WallTimer start()
1276  state_update_timer_ =
1277  pnode_->create_wall_timer(dt_state_update_, [this]() { return stateUpdateTimerCallback(); });
1278  }
1279 
1280  if (!attached_objects_topic.empty())
1281  {
1282  // using regular message filter as there's no header
1283  attached_collision_object_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::AttachedCollisionObject>(
1284  attached_objects_topic, rclcpp::ServicesQoS(),
1285  [this](const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) {
1286  return attachObjectCallback(obj);
1287  });
1288  RCLCPP_INFO(LOGGER, "Listening to '%s' for attached collision objects",
1289  attached_collision_object_subscriber_->get_topic_name());
1290  }
1291  }
1292  else
1293  RCLCPP_ERROR(LOGGER, "Cannot monitor robot state because planning scene is not configured");
1294 }
1295 
1297 {
1299  current_state_monitor_->stopStateMonitor();
1302 
1303  if (state_update_timer_)
1304  state_update_timer_->cancel();
1305  state_update_pending_.store(false);
1306 }
1307 
1308 void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::msg::JointState::ConstSharedPtr& /*joint_state */)
1309 {
1310  state_update_pending_.store(true);
1311 
1312  // Read access to last_robot_state_update_wall_time_ and dt_state_update_ is unprotected here
1313  // as reading invalid values is not critical (just postpones the next state update)
1314  // only update every dt_state_update_ seconds
1315  if (std::chrono::system_clock::now() - last_robot_state_update_wall_time_ >= dt_state_update_)
1317 }
1318 
1319 void PlanningSceneMonitor::stateUpdateTimerCallback()
1320 {
1321  // Read access to last_robot_state_update_wall_time_ and dt_state_update_ is unprotected here
1322  // as reading invalid values is not critical (just postpones the next state update)
1323  if (state_update_pending_.load() &&
1324  std::chrono::system_clock::now() - last_robot_state_update_wall_time_ >= dt_state_update_)
1326 }
1327 
1329 {
1330  if (!octomap_monitor_)
1331  return;
1332 
1334  {
1335  std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
1336  last_update_time_ = rclcpp::Clock().now();
1337  octomap_monitor_->getOcTreePtr()->lockRead();
1338  try
1339  {
1340  scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Isometry3d::Identity());
1341  octomap_monitor_->getOcTreePtr()->unlockRead();
1342  }
1343  catch (...)
1344  {
1345  octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
1346  throw;
1347  }
1348  }
1350 }
1351 
1353 {
1354  bool update = false;
1355  if (hz > std::numeric_limits<double>::epsilon())
1356  {
1357  std::unique_lock<std::mutex> lock(state_update_mutex_);
1358  dt_state_update_ = std::chrono::duration<double>(1.0 / hz);
1359  // ROS original: state_update_timer_.start();
1360  // TODO: re-enable WallTimer start()
1361  state_update_timer_ = pnode_->create_wall_timer(dt_state_update_, [this]() { return stateUpdateTimerCallback(); });
1362  }
1363  else
1364  {
1365  // stop must be called with state_update_mutex_ unlocked to avoid deadlock
1366  // ROS original: state_update_timer_.stop();
1367  // TODO: re-enable WallTimer stop()
1368  if (state_update_timer_)
1369  state_update_timer_->cancel();
1370  std::unique_lock<std::mutex> lock(state_update_mutex_);
1371  dt_state_update_ = std::chrono::duration<double>(0.0);
1372  if (state_update_pending_.load())
1373  update = true;
1374  }
1375  RCLCPP_INFO(LOGGER, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.count());
1376 
1377  if (update)
1379 }
1380 
1382 {
1383  rclcpp::Time time = node_->now();
1384  rclcpp::Clock steady_clock = rclcpp::Clock(RCL_STEADY_TIME);
1386  {
1387  std::vector<std::string> missing;
1388  if (!current_state_monitor_->haveCompleteState(missing) &&
1389  (time - current_state_monitor_->getMonitorStartTime()).seconds() > 1.0)
1390  {
1391  std::string missing_str = boost::algorithm::join(missing, ", ");
1392  RCLCPP_WARN_THROTTLE(LOGGER, steady_clock, 1000, "The complete state of the robot is not yet known. Missing %s",
1393  missing_str.c_str());
1394  }
1395 
1396  {
1397  std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_, std::defer_lock);
1398  if (!skip_update_if_locked)
1399  {
1400  ulock.lock();
1401  }
1402  else if (!ulock.try_lock())
1403  {
1404  // Return if we can't lock scene_update_mutex, thus not blocking CurrentStateMonitor
1405  return;
1406  }
1408  RCLCPP_DEBUG(LOGGER, "robot state update %f", fmod(last_robot_motion_time_.seconds(), 10.));
1409  current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
1410  scene_->getCurrentStateNonConst().update(); // compute all transforms
1411  }
1412 
1413  // Update state_update_mutex_ and last_robot_state_update_wall_time_
1414  {
1415  std::unique_lock<std::mutex> lock(state_update_mutex_);
1416  last_robot_state_update_wall_time_ = std::chrono::system_clock::now();
1417  state_update_pending_.store(false);
1418  }
1419 
1421  }
1422  else
1423  RCLCPP_ERROR_THROTTLE(LOGGER, steady_clock, 1000,
1424  "State monitor is not active. Unable to set the planning scene state");
1425 }
1426 
1427 void PlanningSceneMonitor::addUpdateCallback(const std::function<void(SceneUpdateType)>& fn)
1428 {
1429  std::scoped_lock lock(update_lock_);
1430  if (fn)
1431  update_callbacks_.push_back(fn);
1432 }
1433 
1435 {
1436  std::scoped_lock lock(update_lock_);
1437  update_callbacks_.clear();
1438 }
1439 
1441 {
1443  RCLCPP_DEBUG(LOGGER, "Maximum frequency for publishing a planning scene is now %lf Hz",
1445 }
1446 
1447 void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms)
1448 {
1449  const std::string& target = getRobotModel()->getModelFrame();
1450 
1451  std::vector<std::string> all_frame_names;
1452  tf_buffer_->_getFrameStrings(all_frame_names);
1453  for (const std::string& all_frame_name : all_frame_names)
1454  {
1455  if (all_frame_name == target || getRobotModel()->hasLinkModel(all_frame_name))
1456  continue;
1457 
1458  geometry_msgs::msg::TransformStamped f;
1459  try
1460  {
1461  f = tf_buffer_->lookupTransform(target, all_frame_name, tf2::TimePointZero);
1462  }
1463  catch (tf2::TransformException& ex)
1464  {
1465  RCLCPP_WARN(LOGGER, "Unable to transform object from frame '%s' to planning frame'%s' (%s)",
1466  all_frame_name.c_str(), target.c_str(), ex.what());
1467  continue;
1468  }
1469  f.header.frame_id = all_frame_name;
1470  f.child_frame_id = target;
1471  transforms.push_back(f);
1472  }
1473 }
1474 
1476 {
1477  if (scene_)
1478  {
1479  std::vector<geometry_msgs::msg::TransformStamped> transforms;
1480  getUpdatedFrameTransforms(transforms);
1481  {
1482  std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
1483  scene_->getTransformsNonConst().setTransforms(transforms);
1484  last_update_time_ = rclcpp::Clock().now();
1485  }
1487  }
1488 }
1489 
1491 {
1492  if (octomap_monitor_)
1493  octomap_monitor_->publishDebugInformation(flag);
1494 }
1495 
1496 void PlanningSceneMonitor::configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene)
1497 {
1498  if (!scene || robot_description_.empty())
1499  return;
1500  // TODO: Uncomment when XmlRpc is refactored
1501  // collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
1502 
1503  // read overriding values from the param server
1504 
1505  // first we do default collision operations
1506  if (!node_->has_parameter(robot_description_ + "_planning.default_collision_operations"))
1507  RCLCPP_DEBUG(LOGGER, "No additional default collision operations specified");
1508  else
1509  {
1510  RCLCPP_DEBUG(LOGGER, "Reading additional default collision operations");
1511 
1512  // TODO: codebase wide refactoring for XmlRpc
1513  // XmlRpc::XmlRpcValue coll_ops;
1514  // node_->get_parameter(robot_description_ + "_planning/default_collision_operations", coll_ops);
1515 
1516  // if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
1517  // {
1518  // RCLCPP_WARN(LOGGER, "default_collision_operations is not an array");
1519  // return;
1520  // }
1521 
1522  // if (coll_ops.size() == 0)
1523  // {
1524  // RCLCPP_WARN(LOGGER, "No collision operations in default collision operations");
1525  // return;
1526  // }
1527 
1528  // for (int i = 0; i < coll_ops.size(); ++i) // NOLINT(modernize-loop-convert)
1529  // {
1530  // if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") ||
1531  // !coll_ops[i].hasMember("operation"))
1532  // {
1533  // RCLCPP_WARN(LOGGER, "All collision operations must have two objects and an operation");
1534  // continue;
1535  // }
1536  // acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
1537  // std::string(coll_ops[i]["operation"]) == "disable");
1538  // }
1539  }
1540 }
1541 
1543 {
1544  if (robot_description_.empty())
1545  {
1546  default_robot_padd_ = 0.0;
1547  default_robot_scale_ = 1.0;
1548  default_object_padd_ = 0.0;
1549  default_attached_padd_ = 0.0;
1550  return;
1551  }
1552 
1553  // Ensure no leading slash creates a bad param server address
1554  static const std::string ROBOT_DESCRIPTION =
1555  (robot_description_[0] == '.') ? robot_description_.substr(1) : robot_description_;
1556 
1557  node_->get_parameter_or(ROBOT_DESCRIPTION + "_planning.default_robot_padding", default_robot_padd_, 0.0);
1558  node_->get_parameter_or(ROBOT_DESCRIPTION + "_planning.default_robot_scale", default_robot_scale_, 1.0);
1559  node_->get_parameter_or(ROBOT_DESCRIPTION + "_planning.default_object_padding", default_object_padd_, 0.0);
1560  node_->get_parameter_or(ROBOT_DESCRIPTION + "_planning.default_attached_padding", default_attached_padd_, 0.0);
1561  default_robot_link_padd_ = std::map<std::string, double>();
1562  default_robot_link_scale_ = std::map<std::string, double>();
1563  // TODO: enable parameter type support to std::map
1564  // node_->get_parameter_or(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_,
1565  // std::map<std::string, double>());
1566  // node_->get_parameter_or(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_,
1567  // std::map<std::string, double>());
1568 
1569  RCLCPP_DEBUG_STREAM(LOGGER, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
1570  RCLCPP_DEBUG_STREAM(LOGGER, "Loaded " << default_robot_link_scale_.size() << " default link scales");
1571 }
1572 } // namespace planning_scene_monitor
void setupScene(const rclcpp::Node::SharedPtr &node, const planning_scene::PlanningScenePtr &scene)
Fetch plugin name from parameter server and activate the plugin for the given scene.
std::shared_lock< std::shared_mutex > ReadLock
Definition: occupancy_map.h:88
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:265
std::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
Definition: world.h:301
This may be thrown during construction of objects if errors occur.
Definition: exceptions.h:46
Object defining bodies that can be attached to robot links.
Definition: attached_body.h:58
const std::string & getName() const
Get the name of the attached body.
Definition: attached_body.h:74
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:72
This class maintains the representation of the environment as seen by a planning instance....
static PlanningScenePtr clone(const PlanningSceneConstPtr &scene)
Clone a planning scene. Even if the scene scene depends on a parent, the cloned scene will not.
static const std::string OCTOMAP_NS
PlanningSceneMonitor Subscribes to the topic planning_scene.
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
void lockSceneRead()
Lock the scene for reading (multiple threads can lock for reading at the same time)
void lockSceneWrite()
Lock the scene for writing (only one thread can lock for writing and no other thread can lock for rea...
void unlockSceneRead()
Unlock the scene from reading (multiple threads can lock for reading at the same time)
double default_attached_padd_
default attached padding
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.
std::unique_ptr< occupancy_map_monitor::OccupancyMapMonitor > octomap_monitor_
planning_scene::PlanningScenePtr copyPlanningScene(const moveit_msgs::msg::PlanningScene &diff=moveit_msgs::msg::PlanningScene())
Returns a copy of the current planning scene.
const moveit::core::RobotModelConstPtr & getRobotModel() const
void attachObjectCallback(const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr &obj)
Callback for a new attached object msg.
std::map< std::string, double > default_robot_link_padd_
default robot link padding
void collisionObjectCallback(const moveit_msgs::msg::CollisionObject::ConstSharedPtr &obj)
Callback for a new collision object msg.
void configureDefaultPadding()
Configure the default padding.
PlanningSceneMonitor(const PlanningSceneMonitor &)=delete
PlanningSceneMonitor cannot be copy-constructed.
rclcpp::Subscription< moveit_msgs::msg::AttachedCollisionObject >::SharedPtr attached_collision_object_subscriber_
void stopPublishingPlanningScene()
Stop publishing the maintained planning scene.
void excludeAttachedBodyFromOctree(const moveit::core::AttachedBody *attached_body)
void addUpdateCallback(const std::function< void(SceneUpdateType)> &fn)
Add a function to be called when an update to the scene is received.
bool newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene &scene)
void setPlanningScenePublishingFrequency(double hz)
Set the maximum frequency at which planning scenes are being published.
void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr &obj)
rclcpp::Service< moveit_msgs::srv::GetPlanningScene >::SharedPtr get_scene_service_
void initialize(const planning_scene::PlanningScenePtr &scene)
Initialize the planning scene monitor.
void getMonitoredTopics(std::vector< std::string > &topics) const
Get the topic names that the monitor is listening to.
void triggerSceneUpdateEvent(SceneUpdateType update_type)
This function is called every time there is a change to the planning scene.
void newPlanningSceneWorldCallback(const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr &world)
Callback for a new planning scene world.
planning_scene::PlanningSceneConstPtr scene_const_
rclcpp::Subscription< moveit_msgs::msg::CollisionObject >::SharedPtr collision_object_subscriber_
void includeAttachedBodyInOctree(const moveit::core::AttachedBody *attached_body)
void octomapUpdateCallback()
Callback for octomap updates.
rclcpp::Subscription< moveit_msgs::msg::PlanningScene >::SharedPtr planning_scene_subscriber_
void clearUpdateCallbacks()
Clear the functions to be called when an update to the scene is received.
void providePlanningSceneService(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
Create an optional service for getting the complete planning scene This is useful for satisfying the ...
std::shared_mutex scene_update_mutex_
if diffs are monitored, this is the pointer to the parent scene
void setStateUpdateFrequency(double hz)
Update the scene using the monitored state at a specified frequency, in Hz. This function has an effe...
std::shared_ptr< tf2_ros::TransformListener > tf_listener_
void stopWorldGeometryMonitor()
Stop the world geometry monitor.
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.
@ UPDATE_STATE
The state in the monitored scene was updated.
@ UPDATE_GEOMETRY
The geometry of the scene was updated. This includes receiving new octomaps, collision objects,...
@ UPDATE_TRANSFORMS
The maintained set of fixed transforms in the monitored scene was updated.
std::shared_ptr< rclcpp::executors::SingleThreadedExecutor > private_executor_
bool getShapeTransformCache(const std::string &target_frame, const rclcpp::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor (ROS topic-based)
void startPublishingPlanningScene(SceneUpdateType event, const std::string &planning_scene_topic=MONITORED_PLANNING_SCENE_TOPIC)
Start publishing the maintained planning scene. The first message set out is a complete planning scen...
bool waitForCurrentRobotState(const rclcpp::Time &t, double wait_time=1.)
Wait for robot state to become more recent than time t.
std::map< std::string, double > default_robot_link_scale_
default robot link scale
rclcpp::Time last_robot_motion_time_
Last time the state was updated.
void updateSceneWithCurrentState(bool skip_update_if_locked=false)
Update the scene using the monitored state. This function is automatically called when an update to t...
std::string monitor_name_
The name of this scene monitor.
bool updatesScene(const planning_scene::PlanningSceneConstPtr &scene) const
Return true if the scene scene can be updated directly or indirectly by this monitor....
std::recursive_mutex update_lock_
lock access to update_callbacks_
rclcpp::Time last_update_time_
mutex for stored scene
bool requestPlanningSceneState(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
Request a full planning scene state using a service call Be careful not to use this in conjunction wi...
void monitorDiffs(bool flag)
By default, the maintained planning scene does not reason about diffs. When the flag passed in is tru...
void currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody *attached_body, bool just_attached)
Callback for a change for an attached object of the current state of the planning scene.
std::unique_ptr< std::thread > publish_planning_scene_
std::shared_ptr< rclcpp::Node > node_
Last time the robot has moved.
void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene)
Configure the collision matrix for a particular scene.
void startWorldGeometryMonitor(const std::string &collision_objects_topic=DEFAULT_COLLISION_OBJECT_TOPIC, const std::string &planning_scene_world_topic=DEFAULT_PLANNING_SCENE_WORLD_TOPIC, const bool load_octomap_monitor=true)
Start the OccupancyMapMonitor and listening for:
rclcpp::Publisher< moveit_msgs::msg::PlanningScene >::SharedPtr planning_scene_publisher_
void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj)
std::vector< std::function< void(SceneUpdateType)> > update_callbacks_
void updateFrameTransforms()
Update the transforms for the frames that are not part of the kinematic model using tf....
planning_scene::PlanningScenePtr parent_scene_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
void unlockSceneWrite()
Lock the scene from writing (only one thread can lock for writing and no other thread can lock for re...
rclcpp::Subscription< moveit_msgs::msg::PlanningSceneWorld >::SharedPtr planning_scene_world_subscriber_
rclcpp::Parameter declare_parameter(const rclcpp::Node::SharedPtr &node, const std::string &parameter_name)
bool isEmpty(const moveit_msgs::msg::PlanningScene &msg)
Check if a message includes any information about a planning scene, or whether it is empty.
std::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Definition: attached_body.h:51
std::map< ShapeHandle, Eigen::Isometry3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Isometry3d > > > ShapeTransformCache
scene
Definition: pick.py:52
This namespace includes the central class for representing planning contexts.
name
Definition: setup.py:7
description
Definition: setup.py:19
Definition: world.h:49
const rclcpp::Logger LOGGER
Definition: async_test.h:31