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