41#include <moveit_msgs/srv/get_planning_scene.hpp>
45#if __has_include(<tf2/exceptions.hpp>)
46#include <tf2/exceptions.hpp>
48#include <tf2/exceptions.h>
50#if __has_include(<tf2/LinearMath/Transform.hpp>)
51#include <tf2/LinearMath/Transform.hpp>
53#include <tf2/LinearMath/Transform.h>
55#include <tf2_eigen/tf2_eigen.hpp>
56#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
58#include <fmt/format.h>
61#include <std_msgs/msg/string.hpp>
64using namespace std::chrono_literals;
77 const std::string& name)
83 const planning_scene::PlanningScenePtr& scene,
84 const std::string& robot_description,
const std::string& name)
91 const robot_model_loader::RobotModelLoaderPtr& rm_loader,
92 const std::string& name)
98 const planning_scene::PlanningScenePtr& scene,
99 const robot_model_loader::RobotModelLoaderPtr& rm_loader,
100 const std::string& name)
101 : monitor_name_(name)
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"))
110 std::vector<std::string> new_args = rclcpp::NodeOptions().arguments();
111 new_args.push_back(
"--ros-args");
112 new_args.push_back(
"-r");
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));
127 use_sim_time_ = node->get_parameter(
"use_sim_time").as_bool();
151 robot_model_.reset();
160 if (rm_loader_->getModel())
162 robot_model_ = rm_loader_->getModel();
168 scene_ = std::make_shared<planning_scene::PlanningScene>(rm_loader_->getModel());
176 scene_->getCollisionEnvNonConst()->setLinkPadding(it.first, it.second);
180 scene_->getCollisionEnvNonConst()->setLinkScale(it.first, it.second);
185 RCLCPP_ERROR(logger_,
"Configuration of planning scene failed");
198 scene_->setCollisionObjectUpdateCallback(
206 RCLCPP_ERROR(logger_,
"Robot model not loaded");
213 last_robot_state_update_wall_time_ = std::chrono::system_clock::now();
214 dt_state_update_ = std::chrono::duration<double>(0.03);
216 double temp_wait_time = 0.05;
220 node_->get_parameter_or(
robot_description_ +
"_planning.shape_transform_cache_lookup_wait_time", temp_wait_time,
224 shape_transform_cache_lookup_wait_time_ = rclcpp::Duration::from_seconds(temp_wait_time);
226 state_update_pending_.store(
false);
228 using std::chrono::nanoseconds;
229 state_update_timer_ =
pnode_->create_wall_timer(dt_state_update_, [
this]() {
return stateUpdateTimerCallback(); });
235 auto declare_parameter = [
this](
const std::string& param_name,
auto default_val,
236 const std::string& description) ->
auto
238 rcl_interfaces::msg::ParameterDescriptor desc;
239 desc.set__description(description);
240 return pnode_->declare_parameter(param_name, default_val, desc);
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"))
252 publish_planning_scene =
node_->get_parameter(
"publish_planning_scene").as_bool();
254 if (
node_->has_parameter(
"publish_geometry_updates"))
256 publish_geometry_updates =
node_->get_parameter(
"publish_geometry_updates").as_bool();
258 if (
node_->has_parameter(
"publish_state_updates"))
260 publish_state_updates =
node_->get_parameter(
"publish_state_updates").as_bool();
262 if (
node_->has_parameter(
"publish_transforms_updates"))
264 publish_transform_updates =
node_->get_parameter(
"publish_transforms_updates").as_bool();
266 if (
node_->has_parameter(
"publish_planning_scene_hz"))
268 publish_planning_scene_hz =
node_->get_parameter(
"publish_planning_scene_hz").as_double();
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);
286 catch (
const rclcpp::exceptions::InvalidParameterTypeException& e)
288 RCLCPP_ERROR_STREAM(logger_,
"Invalid parameter type in PlanningSceneMonitor: " << e.what());
289 RCLCPP_ERROR(logger_,
"Dynamic publishing parameters won't be available");
293 auto psm_parameter_set_callback = [
this](
const std::vector<rclcpp::Parameter>& parameters) ->
auto
295 auto result = rcl_interfaces::msg::SetParametersResult();
296 result.successful =
true;
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);
307 if (!declared_params_valid)
309 RCLCPP_ERROR(logger_,
"Initially declared parameters are invalid - failed to process update callback");
310 result.successful =
false;
314 for (
const auto& parameter : parameters)
316 const auto& name = parameter.get_name();
317 const auto& type = parameter.get_type();
320 if (!
pnode_->has_parameter(name) ||
pnode_->get_parameter(name).get_type() != type)
322 RCLCPP_ERROR(logger_,
"Invalid parameter in PlanningSceneMonitor parameter callback");
323 result.successful =
false;
328 if (name ==
"planning_scene_monitor.publish_planning_scene")
330 publish_planning_scene = parameter.as_bool();
332 else if (name ==
"planning_scene_monitor.publish_geometry_updates")
334 publish_geometry_updates = parameter.as_bool();
336 else if (name ==
"planning_scene_monitor.publish_state_updates")
338 publish_state_updates = parameter.as_bool();
340 else if (name ==
"planning_scene_monitor.publish_transforms_updates")
342 publish_transform_updates = parameter.as_bool();
344 else if (name ==
"planning_scene_monitor.publish_planning_scene_hz")
346 publish_planning_scene_hz = parameter.as_double();
350 if (result.successful)
352 updatePublishSettings(publish_geometry_updates, publish_state_updates, publish_transform_updates,
353 publish_planning_scene, publish_planning_scene_hz);
358 callback_handler_ =
pnode_->add_on_set_parameters_callback(psm_parameter_set_callback);
379 scene_->setCollisionObjectUpdateCallback(
380 [
this](
const collision_detection::World::ObjectConstPtr&
object,
388 RCLCPP_WARN(logger_,
"Diff monitoring was stopped while publishing planning scene diffs. "
389 "Stopping planning scene diff publisher");
399 if (!
scene_->getName().empty())
401 if (
scene_->getName()[
scene_->getName().length() - 1] ==
'+')
414 std::unique_ptr<std::thread> copy;
420 RCLCPP_INFO(logger_,
"Stopped publishing maintained planning scene.");
425 const std::string& planning_scene_topic)
431 RCLCPP_INFO(logger_,
"Stopping existing planning scene publisher.");
438 RCLCPP_INFO(logger_,
"Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
444 RCLCPP_WARN(logger_,
"Did not find a planning scene, so cannot publish it.");
448void PlanningSceneMonitor::scenePublishingThread()
450 RCLCPP_DEBUG(logger_,
"Started scene publishing thread ...");
454 moveit_msgs::msg::PlanningScene msg;
459 scene_->getPlanningSceneMsg(msg);
462 RCLCPP_DEBUG(logger_,
"Published the full planning scene: '%s'", msg.name.c_str());
467 moveit_msgs::msg::PlanningScene msg;
468 bool publish_msg =
false;
469 bool is_full =
false;
488 scene_->getPlanningSceneDiffMsg(msg);
491 msg.robot_state.attached_collision_objects.clear();
492 msg.robot_state.is_diff =
true;
507 scene_->setCollisionObjectUpdateCallback(
508 [
this](
const collision_detection::World::ObjectConstPtr&
object,
520 scene_->getPlanningSceneMsg(msg);
533 RCLCPP_DEBUG(logger_,
"Published full planning scene: '%s'", msg.name.c_str());
563bool sceneIsParentOf(
const planning_scene::PlanningSceneConstPtr& scene,
566 if (scene && scene.get() == possible_parent)
569 return sceneIsParentOf(scene->getParent(), possible_parent);
590 update_callback(update_type);
599 RCLCPP_FATAL_STREAM(logger_,
"requestPlanningSceneState() to self-provided service '" << service_name <<
'\'');
600 throw std::runtime_error(
"requestPlanningSceneState() to self-provided service: " + service_name);
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;
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)))
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)
620 RCLCPP_DEBUG(logger_,
"Service request succeeded, applying new planning scene");
624 if (service_result_status == std::future_status::timeout)
626 RCLCPP_INFO(logger_,
"GetPlanningScene service call to %s timed out. at %s:%d", service_name.c_str(), __FILE__,
634 "Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?",
635 service_name.c_str());
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);
650void PlanningSceneMonitor::getPlanningSceneServiceCallback(
651 const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req,
652 const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res)
654 if (req->components.components & moveit_msgs::msg::PlanningSceneComponents::TRANSFORMS)
657 moveit_msgs::msg::PlanningSceneComponents all_components;
658 all_components.components = UINT_MAX;
661 scene_->getPlanningSceneMsg(res->scene, req->components.components ? req->components : all_components);
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)
669 if (publish_geom_updates)
674 if (publish_state_updates)
679 if (publish_transform_updates)
684 if (publish_planning_scene)
693void PlanningSceneMonitor::newPlanningSceneCallback(
const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene)
700 bool removed =
false;
703 removed =
scene_->getWorldNonConst()->removeObject(
scene_->OCTOMAP_NS);
713 RCLCPP_WARN(logger_,
"Unable to clear octomap since no octomap monitor has been initialized");
729 std::string old_scene_name;
737 RCLCPP_DEBUG(logger_,
"scene update %f robot stamp: %f", fmod(
last_update_time_.seconds(), 10.),
739 old_scene_name =
scene_->getName();
744 if (scene.robot_state.is_diff)
755 result =
scene_->usePlanningSceneMsg(scene);
760 if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
767 robot_model_ =
scene_->getRobotModel();
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)
788 if (!scene.fixed_frame_transforms.empty())
794 if (!scene.robot_state.attached_collision_objects.empty() || !scene.robot_state.is_diff)
804 const std::optional<moveit_msgs::msg::ObjectColor>& color_msg)
813 if (!
scene_->processCollisionObjectMsg(*
object))
815 if (color_msg.has_value())
816 scene_->setObjectColor(color_msg.value().id, color_msg.value().color);
819 RCLCPP_INFO(logger_,
"Published update collision object");
824 const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr&
object)
835 if (!
scene_->processAttachedCollisionObjectMsg(*
object))
839 RCLCPP_INFO(logger_,
"Published update attached");
844 const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world)
852 scene_->getWorldNonConst()->clearObjects();
853 scene_->processPlanningSceneWorldMsg(*world);
856 if (world->octomap.octomap.data.empty())
876 const std::vector<const moveit::core::LinkModel*>& links =
getRobotModel()->getLinkModelsWithCollisionGeometry();
877 auto start = std::chrono::system_clock::now();
881 std::vector<shapes::ShapeConstPtr>
shapes = link->getShapes();
882 for (std::size_t j = 0; j <
shapes.size(); ++j)
885 if (
shapes[j]->type == shapes::MESH)
887 shapes::Mesh* m =
static_cast<shapes::Mesh*
>(
shapes[j]->clone());
888 m->mergeVertices(1e-4);
897 if (!warned && ((std::chrono::system_clock::now() - start) > std::chrono::seconds(30)))
899 RCLCPP_WARN(logger_,
"It is likely there are too many vertices in collision geometry");
913 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
916 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : link_shape_handle.second)
931 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& attached_body_shape_handle :
934 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : attached_body_shape_handle.second)
946 std::vector<const moveit::core::AttachedBody*> ab;
947 scene_->getCurrentState().getAttachedBodies(ab);
960 for (std::pair<
const std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
963 for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
964 collision_body_shape_handle.second)
975 for (
const std::pair<const std::string, collision_detection::World::ObjectPtr>& it : *
scene_->getWorld())
986 for (std::size_t i = 0; i < attached_body->
getShapes().size(); ++i)
988 if (attached_body->
getShapes()[i]->type == shapes::PLANE || attached_body->
getShapes()[i]->type == shapes::OCTREE)
998 RCLCPP_DEBUG(logger_,
"Excluding attached body '%s' from monitored octomap", attached_body->
getName().c_str());
1011 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& shape_handle : it->second)
1013 RCLCPP_DEBUG(logger_,
"Including attached body '%s' in monitored octomap", attached_body->
getName().c_str());
1026 for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
1028 if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
1038 RCLCPP_DEBUG(logger_,
"Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
1051 for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& shape_handle : it->second)
1053 RCLCPP_DEBUG(logger_,
"Including collision object '%s' in monitored octomap", obj->id_.c_str());
1099 if (t.nanoseconds() == 0)
1101 RCLCPP_DEBUG(logger_,
"sync robot state to: %.3fs", fmod(t.seconds(), 10.));
1113 if (state_update_pending_.load())
1120 RCLCPP_WARN(logger_,
"Failed to fetch current robot state.");
1127 auto start =
node_->get_clock()->now();
1128 auto timeout = rclcpp::Duration::from_seconds(wait_time);
1132 timeout > rclcpp::Duration(0, 0))
1136 timeout = timeout - (
node_->get_clock()->now() - start);
1142 RCLCPP_WARN(logger_,
"Maybe failed to update robot state, time diff: %.3fs",
1146 RCLCPP_DEBUG(logger_,
"sync done: robot motion: %f scene update: %f", (t -
last_robot_motion_time_).seconds(),
1183 RCLCPP_INFO(logger_,
"Starting planning scene monitor");
1185 if (!scene_topic.empty())
1188 scene_topic, rclcpp::SystemDefaultsQoS(), [
this](
const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) {
1189 return newPlanningSceneCallback(scene);
1199 RCLCPP_INFO(logger_,
"Stopping planning scene monitor");
1212 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
1215 if (
tf_buffer_->canTransform(target_frame, link_shape_handle.first->getName(), target_time,
1216 shape_transform_cache_lookup_wait_time_))
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)
1222 cache[link_shape_handle.second[j].first] =
1223 ttr * link_shape_handle.first->getCollisionOriginTransforms()[link_shape_handle.second[j].second];
1228 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>&
1231 if (
tf_buffer_->canTransform(target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time,
1232 shape_transform_cache_lookup_wait_time_))
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)
1238 cache[attached_body_shape_handle.second[k].first] =
1240 attached_body_shape_handle.first->getShapePosesInLinkFrame()[attached_body_shape_handle.second[k].second];
1245 if (
tf_buffer_->canTransform(target_frame,
scene_->getPlanningFrame(), target_time,
1246 shape_transform_cache_lookup_wait_time_))
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*>>>&
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);
1261 catch (tf2::TransformException& ex)
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
1274 const std::string& planning_scene_world_topic,
1275 const bool load_octomap_monitor)
1278 RCLCPP_INFO(logger_,
"Starting world geometry update monitor for collision objects, attached objects, octomap "
1282 if (!collision_objects_topic.empty())
1285 collision_objects_topic, rclcpp::SystemDefaultsQoS(),
1287 RCLCPP_INFO(logger_,
"Listening to '%s'", collision_objects_topic.c_str());
1290 if (!planning_scene_world_topic.empty())
1293 planning_scene_world_topic, rclcpp::SystemDefaultsQoS(),
1294 [
this](
const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) {
1297 RCLCPP_INFO(logger_,
"Listening to '%s' for planning scene world geometry", planning_scene_world_topic.c_str());
1301 if (load_octomap_monitor)
1306 std::make_unique<occupancy_map_monitor::OccupancyMapMonitor>(
node_,
tf_buffer_,
scene_->getPlanningFrame());
1311 octomap_monitor_->setTransformCacheCallback([
this](
const std::string& frame,
const rclcpp::Time& stamp,
1325 RCLCPP_INFO(logger_,
"Stopping world geometry monitor");
1330 RCLCPP_INFO(logger_,
"Stopping world geometry monitor");
1338 const std::string& attached_objects_topic)
1349 [
this](
const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state) {
return onStateUpdate(joint_state); });
1353 std::unique_lock<std::mutex> lock(state_update_mutex_);
1354 if (dt_state_update_.count() > 0)
1358 state_update_timer_ =
1359 pnode_->create_wall_timer(dt_state_update_, [
this]() {
return stateUpdateTimerCallback(); });
1363 if (!attached_objects_topic.empty())
1367 attached_objects_topic, rclcpp::SystemDefaultsQoS(),
1368 [
this](
const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) {
1371 RCLCPP_INFO(logger_,
"Listening to '%s' for attached collision objects",
1376 RCLCPP_ERROR(logger_,
"Cannot monitor robot state because planning scene is not configured");
1386 if (state_update_timer_)
1387 state_update_timer_->cancel();
1388 state_update_pending_.store(
false);
1391void PlanningSceneMonitor::onStateUpdate(
const sensor_msgs::msg::JointState::ConstSharedPtr& )
1393 state_update_pending_.store(
true);
1398 if (std::chrono::system_clock::now() - last_robot_state_update_wall_time_ >= dt_state_update_)
1402void PlanningSceneMonitor::stateUpdateTimerCallback()
1406 if (state_update_pending_.load() &&
1407 std::chrono::system_clock::now() - last_robot_state_update_wall_time_ >= dt_state_update_)
1437 bool update =
false;
1438 if (hz > std::numeric_limits<double>::epsilon())
1440 std::unique_lock<std::mutex> lock(state_update_mutex_);
1441 dt_state_update_ = std::chrono::duration<double>(1.0 / hz);
1444 state_update_timer_ =
pnode_->create_wall_timer(dt_state_update_, [
this]() {
return stateUpdateTimerCallback(); });
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())
1458 RCLCPP_INFO(logger_,
"Updating internal planning scene state at most every %lf seconds", dt_state_update_.count());
1466 rclcpp::Time time =
node_->now();
1467 rclcpp::Clock steady_clock = rclcpp::Clock(RCL_STEADY_TIME);
1470 std::vector<std::string> missing;
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
1484 if (!skip_update_if_locked)
1488 else if (!ulock.try_lock())
1496 scene_->getCurrentStateNonConst().update();
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);
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
1534 RCLCPP_DEBUG(logger_,
"Maximum frequency for publishing a planning scene is now %lf Hz",
1538void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms)
1540 const std::string& target =
getRobotModel()->getModelFrame();
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)
1546 if (all_frame_name == target ||
getRobotModel()->hasLinkModel(all_frame_name))
1549 geometry_msgs::msg::TransformStamped f;
1552 f =
tf_buffer_->lookupTransform(target, all_frame_name, tf2::TimePointZero);
1554 catch (tf2::TransformException& ex)
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());
1560 f.header.frame_id = all_frame_name;
1561 f.child_frame_id = target;
1562 transforms.push_back(f);
1570 std::vector<geometry_msgs::msg::TransformStamped> transforms;
1571 getUpdatedFrameTransforms(transforms);
1574 scene_->getTransformsNonConst().setTransforms(transforms);
1599 RCLCPP_DEBUG(logger_,
"No additional default collision operations specified");
1603 RCLCPP_DEBUG(logger_,
"Reading additional default collision operations");
1647 static const std::string ROBOT_DESCRIPTION =
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...
std::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
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 excludeAttachedBodiesFromOctree()
planning_scene::PlanningScenePtr scene_
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.
double default_robot_scale_
default robot scaling
std::unique_ptr< occupancy_map_monitor::OccupancyMapMonitor > octomap_monitor_
AttachedBodyShapeHandles attached_body_shape_handles_
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.
LinkShapeHandles link_shape_handles_
static const std::string DEFAULT_COLLISION_OBJECT_TOPIC
The name of the topic used by default for receiving collision objects in the world.
SceneUpdateType publish_update_types_
void configureDefaultPadding()
Configure the default padding.
void publishDebugInformation(bool flag)
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)
double default_object_padd_
default object padding
void addUpdateCallback(const std::function< void(SceneUpdateType)> &fn)
Add a function to be called when an update to the scene is received.
std::shared_ptr< rclcpp::Node > pnode_
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.
static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC
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...
double publish_planning_scene_frequency_
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.
void stopStateMonitor()
Stop the state monitor.
@ UPDATE_SCENE
The entire scene was updated.
@ 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::atomic< SceneUpdateType > new_scene_update_
std::shared_ptr< rclcpp::executors::SingleThreadedExecutor > private_executor_
void includeAttachedBodiesInOctree()
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...
std::string robot_description_
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::recursive_mutex shape_handles_lock_
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::thread private_executor_thread_
void excludeWorldObjectsFromOctree()
double default_robot_padd_
default robot padding
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....
CurrentStateMonitorPtr current_state_monitor_
std::recursive_mutex update_lock_
lock access to update_callbacks_
void includeWorldObjectsInOctree()
CollisionBodyShapeHandles collision_body_shape_handles_
void stopSceneMonitor()
Stop the scene monitor.
void includeRobotLinksInOctree()
rclcpp::Time last_update_time_
mutex for stored scene
void excludeRobotLinksFromOctree()
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...
static const std::string MONITORED_PLANNING_SCENE_TOPIC
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_
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....
planning_scene::PlanningScenePtr parent_scene_
std::condition_variable_any new_scene_update_condition_
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_
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.