41 #include <moveit_msgs/srv/get_planning_scene.hpp>
44 #include <tf2/exceptions.h>
45 #include <tf2/LinearMath/Transform.h>
46 #include <tf2_eigen/tf2_eigen.hpp>
47 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
49 #include <fmt/format.h>
52 #include <std_msgs/msg/string.hpp>
55 using namespace std::chrono_literals;
59 const std::string PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC =
"joint_states";
60 const std::string PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC =
"attached_collision_object";
61 const std::string PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC =
"collision_object";
62 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC =
"planning_scene_world";
63 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC =
"planning_scene";
64 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE =
"get_planning_scene";
65 const std::string PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC =
"monitored_planning_scene";
67 PlanningSceneMonitor::PlanningSceneMonitor(
const rclcpp::Node::SharedPtr& node,
const std::string& robot_description,
68 const std::string&
name)
74 const planning_scene::PlanningScenePtr& scene,
75 const std::string& robot_description,
const std::string&
name)
82 const robot_model_loader::RobotModelLoaderPtr& rm_loader,
83 const std::string&
name)
89 const planning_scene::PlanningScenePtr& scene,
90 const robot_model_loader::RobotModelLoaderPtr& rm_loader,
91 const std::string&
name)
94 , private_executor_(std::make_shared<
rclcpp::executors::SingleThreadedExecutor>())
95 , tf_buffer_(std::make_shared<
tf2_ros::Buffer>(node->get_clock()))
96 , dt_state_update_(0.0)
97 , shape_transform_cache_lookup_wait_time_(0, 0)
98 , rm_loader_(rm_loader)
101 std::vector<std::string> new_args = rclcpp::NodeOptions().arguments();
102 new_args.push_back(
"--ros-args");
103 new_args.push_back(
"-r");
105 new_args.push_back(std::string(
"__node:=") +
node_->get_name() +
"_private_" +
106 std::to_string(
reinterpret_cast<std::size_t
>(
this)));
107 new_args.push_back(
"--");
108 pnode_ = std::make_shared<rclcpp::Node>(
"_",
node_->get_namespace(), rclcpp::NodeOptions().arguments(new_args));
118 use_sim_time_ = node->get_parameter(
"use_sim_time").as_bool();
142 robot_model_.reset();
151 if (rm_loader_->getModel())
153 robot_model_ = rm_loader_->getModel();
159 scene_ = std::make_shared<planning_scene::PlanningScene>(rm_loader_->getModel());
167 scene_->getCollisionEnvNonConst()->setLinkPadding(it.first, it.second);
171 scene_->getCollisionEnvNonConst()->setLinkScale(it.first, it.second);
176 RCLCPP_ERROR(logger_,
"Configuration of planning scene failed");
189 scene_->setCollisionObjectUpdateCallback(
197 RCLCPP_ERROR(logger_,
"Robot model not loaded");
204 last_robot_state_update_wall_time_ = std::chrono::system_clock::now();
205 dt_state_update_ = std::chrono::duration<double>(0.03);
207 double temp_wait_time = 0.05;
211 node_->get_parameter_or(
robot_description_ +
"_planning.shape_transform_cache_lookup_wait_time", temp_wait_time,
215 shape_transform_cache_lookup_wait_time_ = rclcpp::Duration::from_seconds(temp_wait_time);
217 state_update_pending_ =
false;
219 using std::chrono::nanoseconds;
220 state_update_timer_ =
pnode_->create_wall_timer(dt_state_update_, [
this]() {
return stateUpdateTimerCallback(); });
226 auto declare_parameter = [
this](
const std::string& param_name,
auto default_val,
229 rcl_interfaces::msg::ParameterDescriptor desc;
231 return pnode_->declare_parameter(param_name, default_val, desc);
236 bool publish_planning_scene =
false;
237 bool publish_geometry_updates =
false;
238 bool publish_state_updates =
false;
239 bool publish_transform_updates =
false;
240 double publish_planning_scene_hz = 4.0;
241 if (
node_->has_parameter(
"publish_planning_scene"))
243 publish_planning_scene =
node_->get_parameter(
"publish_planning_scene").as_bool();
245 if (
node_->has_parameter(
"publish_geometry_updates"))
247 publish_geometry_updates =
node_->get_parameter(
"publish_geometry_updates").as_bool();
249 if (
node_->has_parameter(
"publish_state_updates"))
251 publish_state_updates =
node_->get_parameter(
"publish_state_updates").as_bool();
253 if (
node_->has_parameter(
"publish_transforms_updates"))
255 publish_transform_updates =
node_->get_parameter(
"publish_transforms_updates").as_bool();
257 if (
node_->has_parameter(
"publish_planning_scene_hz"))
259 publish_planning_scene_hz =
node_->get_parameter(
"publish_planning_scene_hz").as_double();
263 publish_planning_scene =
264 declare_parameter(
"publish_planning_scene", publish_planning_scene,
"Set to True to publish Planning Scenes");
265 publish_geometry_updates = declare_parameter(
"publish_geometry_updates", publish_geometry_updates,
266 "Set to True to publish geometry updates of the planning scene");
267 publish_state_updates = declare_parameter(
"publish_state_updates", publish_state_updates,
268 "Set to True to publish state updates of the planning scene");
269 publish_transform_updates = declare_parameter(
"publish_transforms_updates", publish_transform_updates,
270 "Set to True to publish transform updates of the planning scene");
271 publish_planning_scene_hz =
272 declare_parameter(
"publish_planning_scene_hz", publish_planning_scene_hz,
273 "Set the maximum frequency at which planning scene updates are published");
274 updatePublishSettings(publish_geometry_updates, publish_state_updates, publish_transform_updates,
275 publish_planning_scene, publish_planning_scene_hz);
277 catch (
const rclcpp::exceptions::InvalidParameterTypeException& e)
279 RCLCPP_ERROR_STREAM(logger_,
"Invalid parameter type in PlanningSceneMonitor: " << e.what());
280 RCLCPP_ERROR(logger_,
"Dynamic publishing parameters won't be available");
284 auto psm_parameter_set_callback = [
this](
const std::vector<rclcpp::Parameter>& parameters) ->
auto
286 auto result = rcl_interfaces::msg::SetParametersResult();
287 result.successful =
true;
289 bool publish_planning_scene =
false, publish_geometry_updates =
false, publish_state_updates =
false,
290 publish_transform_updates =
false;
291 double publish_planning_scene_hz = 4.0;
292 bool declared_params_valid =
pnode_->get_parameter(
"publish_planning_scene", publish_planning_scene) &&
293 pnode_->get_parameter(
"publish_geometry_updates", publish_geometry_updates) &&
294 pnode_->get_parameter(
"publish_state_updates", publish_state_updates) &&
295 pnode_->get_parameter(
"publish_transforms_updates", publish_transform_updates) &&
296 pnode_->get_parameter(
"publish_planning_scene_hz", publish_planning_scene_hz);
298 if (!declared_params_valid)
300 RCLCPP_ERROR(logger_,
"Initially declared parameters are invalid - failed to process update callback");
301 result.successful =
false;
305 for (
const auto& parameter : parameters)
307 const auto&
name = parameter.get_name();
308 const auto& type = parameter.get_type();
313 RCLCPP_ERROR(logger_,
"Invalid parameter in PlanningSceneMonitor parameter callback");
314 result.successful =
false;
319 if (
name ==
"planning_scene_monitor.publish_planning_scene")
321 publish_planning_scene = parameter.as_bool();
323 else if (
name ==
"planning_scene_monitor.publish_geometry_updates")
325 publish_geometry_updates = parameter.as_bool();
327 else if (
name ==
"planning_scene_monitor.publish_state_updates")
329 publish_state_updates = parameter.as_bool();
331 else if (
name ==
"planning_scene_monitor.publish_transforms_updates")
333 publish_transform_updates = parameter.as_bool();
335 else if (
name ==
"planning_scene_monitor.publish_planning_scene_hz")
337 publish_planning_scene_hz = parameter.as_double();
341 if (result.successful)
343 updatePublishSettings(publish_geometry_updates, publish_state_updates, publish_transform_updates,
344 publish_planning_scene, publish_planning_scene_hz);
349 callback_handler_ =
pnode_->add_on_set_parameters_callback(psm_parameter_set_callback);
370 scene_->setCollisionObjectUpdateCallback(
371 [
this](
const collision_detection::World::ObjectConstPtr&
object,
379 RCLCPP_WARN(logger_,
"Diff monitoring was stopped while publishing planning scene diffs. "
380 "Stopping planning scene diff publisher");
390 if (!
scene_->getName().empty())
392 if (
scene_->getName()[
scene_->getName().length() - 1] ==
'+')
405 std::unique_ptr<std::thread> copy;
411 RCLCPP_INFO(logger_,
"Stopped publishing maintained planning scene.");
416 const std::string& planning_scene_topic)
422 RCLCPP_INFO(logger_,
"Stopping existing planning scene publisher.");
429 RCLCPP_INFO(logger_,
"Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
435 RCLCPP_WARN(logger_,
"Did not find a planning scene, so cannot publish it.");
439 void PlanningSceneMonitor::scenePublishingThread()
441 RCLCPP_DEBUG(logger_,
"Started scene publishing thread ...");
445 moveit_msgs::msg::PlanningScene msg;
450 scene_->getPlanningSceneMsg(msg);
453 RCLCPP_DEBUG(logger_,
"Published the full planning scene: '%s'", msg.name.c_str());
458 moveit_msgs::msg::PlanningScene msg;
459 bool publish_msg =
false;
460 bool is_full =
false;
479 scene_->getPlanningSceneDiffMsg(msg);
482 msg.robot_state.attached_collision_objects.clear();
483 msg.robot_state.is_diff =
true;
498 scene_->setCollisionObjectUpdateCallback(
499 [
this](
const collision_detection::World::ObjectConstPtr&
object,
511 scene_->getPlanningSceneMsg(msg);
524 RCLCPP_DEBUG(logger_,
"Published full planning scene: '%s'", msg.name.c_str());
554 bool sceneIsParentOf(
const planning_scene::PlanningSceneConstPtr& scene,
557 if (scene && scene.get() == possible_parent)
560 return sceneIsParentOf(scene->getParent(), possible_parent);
581 update_callback(update_type);
590 RCLCPP_FATAL_STREAM(logger_,
"requestPlanningSceneState() to self-provided service '" << service_name <<
'\'');
591 throw std::runtime_error(
"requestPlanningSceneState() to self-provided service: " + service_name);
594 auto client =
pnode_->create_client<moveit_msgs::srv::GetPlanningScene>(service_name);
595 auto srv = std::make_shared<moveit_msgs::srv::GetPlanningScene::Request>();
596 srv->components.components = srv->components.SCENE_SETTINGS | srv->components.ROBOT_STATE |
597 srv->components.ROBOT_STATE_ATTACHED_OBJECTS | srv->components.WORLD_OBJECT_NAMES |
598 srv->components.WORLD_OBJECT_GEOMETRY | srv->components.OCTOMAP |
599 srv->components.TRANSFORMS | srv->components.ALLOWED_COLLISION_MATRIX |
600 srv->components.LINK_PADDING_AND_SCALING | srv->components.OBJECT_COLORS;
603 RCLCPP_DEBUG(logger_,
"Waiting for GetPlanningScene service `%s` to exist.", service_name.c_str());
604 if (client->wait_for_service(std::chrono::seconds(5)))
606 RCLCPP_DEBUG(logger_,
"Sending service request to `%s`.", service_name.c_str());
607 auto service_result = client->async_send_request(srv);
608 const auto service_result_status = service_result.wait_for(std::chrono::seconds(5));
609 if (service_result_status == std::future_status::ready)
611 RCLCPP_DEBUG(logger_,
"Service request succeeded, applying new planning scene");
615 if (service_result_status == std::future_status::timeout)
617 RCLCPP_INFO(logger_,
"GetPlanningScene service call to %s timed out. at %s:%d", service_name.c_str(), __FILE__,
625 "Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?",
626 service_name.c_str());
635 service_name, [
this](
const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req,
636 const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res) {
637 return getPlanningSceneServiceCallback(req, res);
641 void PlanningSceneMonitor::getPlanningSceneServiceCallback(
642 const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req,
643 const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res)
645 if (req->components.components & moveit_msgs::msg::PlanningSceneComponents::TRANSFORMS)
648 moveit_msgs::msg::PlanningSceneComponents all_components;
649 all_components.components = UINT_MAX;
652 scene_->getPlanningSceneMsg(res->scene, req->components.components ? req->components : all_components);
655 void PlanningSceneMonitor::updatePublishSettings(
bool publish_geom_updates,
bool publish_state_updates,
656 bool publish_transform_updates,
bool publish_planning_scene,
657 double publish_planning_scene_hz)
660 if (publish_geom_updates)
665 if (publish_state_updates)
670 if (publish_transform_updates)
675 if (publish_planning_scene)
684 void PlanningSceneMonitor::newPlanningSceneCallback(
const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene)
691 bool removed =
false;
694 removed =
scene_->getWorldNonConst()->removeObject(
scene_->OCTOMAP_NS);
704 RCLCPP_WARN(logger_,
"Unable to clear octomap since no octomap monitor has been initialized");
720 std::string old_scene_name;
728 RCLCPP_DEBUG(logger_,
"scene update %f robot stamp: %f", fmod(
last_update_time_.seconds(), 10.),
730 old_scene_name =
scene_->getName();
731 result =
scene_->usePlanningSceneMsg(scene);
734 if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
741 robot_model_ =
scene_->getRobotModel();
755 scene_->setCollisionObjectUpdateCallback(
770 bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
771 scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
772 scene.link_scale.empty();
773 if (no_other_scene_upd)
779 if (!scene.fixed_frame_transforms.empty())
785 if (!scene.robot_state.attached_collision_objects.empty() || !scene.robot_state.is_diff)
795 const std::optional<moveit_msgs::msg::ObjectColor>& color_msg)
804 if (!
scene_->processCollisionObjectMsg(*
object))
806 if (color_msg.has_value())
807 scene_->setObjectColor(color_msg.value().id, color_msg.value().color);
810 RCLCPP_INFO(logger_,
"Published update collision object");
815 const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr&
object)
826 if (!
scene_->processAttachedCollisionObjectMsg(*
object))
830 RCLCPP_INFO(logger_,
"Published update attached");
835 const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world)
843 scene_->getWorldNonConst()->clearObjects();
844 scene_->processPlanningSceneWorldMsg(*world);
847 if (world->octomap.octomap.data.empty())
867 const std::vector<const moveit::core::LinkModel*>& links =
getRobotModel()->getLinkModelsWithCollisionGeometry();
868 auto start = std::chrono::system_clock::now();
872 std::vector<shapes::ShapeConstPtr>
shapes = link->getShapes();
873 for (std::size_t j = 0; j <
shapes.size(); ++j)
876 if (
shapes[j]->type == shapes::MESH)
878 shapes::Mesh* m =
static_cast<shapes::Mesh*
>(
shapes[j]->clone());
879 m->mergeVertices(1e-4);
888 if (!warned && ((std::chrono::system_clock::now() - start) > std::chrono::seconds(30)))
890 RCLCPP_WARN(logger_,
"It is likely there are too many vertices in collision geometry");
904 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
907 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : link_shape_handle.second)
922 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& attached_body_shape_handle :
925 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : attached_body_shape_handle.second)
937 std::vector<const moveit::core::AttachedBody*> ab;
938 scene_->getCurrentState().getAttachedBodies(ab);
951 for (std::pair<
const std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
954 for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
955 collision_body_shape_handle.second)
966 for (
const std::pair<const std::string, collision_detection::World::ObjectPtr>& it : *
scene_->getWorld())
977 for (std::size_t i = 0; i < attached_body->
getShapes().size(); ++i)
979 if (attached_body->
getShapes()[i]->type == shapes::PLANE || attached_body->
getShapes()[i]->type == shapes::OCTREE)
989 RCLCPP_DEBUG(logger_,
"Excluding attached body '%s' from monitored octomap", attached_body->
getName().c_str());
1002 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& shape_handle : it->second)
1004 RCLCPP_DEBUG(logger_,
"Including attached body '%s' in monitored octomap", attached_body->
getName().c_str());
1017 for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
1019 if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
1029 RCLCPP_DEBUG(logger_,
"Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
1042 for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& shape_handle : it->second)
1044 RCLCPP_DEBUG(logger_,
"Including collision object '%s' in monitored octomap", obj->id_.c_str());
1090 if (t.nanoseconds() == 0)
1092 RCLCPP_DEBUG(logger_,
"sync robot state to: %.3fs", fmod(t.seconds(), 10.));
1104 std::unique_lock<std::mutex> lock(state_pending_mutex_);
1105 if (state_update_pending_)
1107 state_update_pending_ =
false;
1108 last_robot_state_update_wall_time_ = std::chrono::system_clock::now();
1115 RCLCPP_WARN(logger_,
"Failed to fetch current robot state.");
1122 auto start =
node_->get_clock()->now();
1123 auto timeout = rclcpp::Duration::from_seconds(wait_time);
1127 timeout > rclcpp::Duration(0, 0))
1131 timeout = timeout - (
node_->get_clock()->now() - start);
1137 RCLCPP_WARN(logger_,
"Maybe failed to update robot state, time diff: %.3fs",
1141 RCLCPP_DEBUG(logger_,
"sync done: robot motion: %f scene update: %f", (t -
last_robot_motion_time_).seconds(),
1178 RCLCPP_INFO(logger_,
"Starting planning scene monitor");
1180 if (!scene_topic.empty())
1183 scene_topic, rclcpp::SystemDefaultsQoS(), [
this](
const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene) {
1184 return newPlanningSceneCallback(scene);
1194 RCLCPP_INFO(logger_,
"Stopping planning scene monitor");
1207 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
1210 if (
tf_buffer_->canTransform(target_frame, link_shape_handle.first->getName(), target_time,
1211 shape_transform_cache_lookup_wait_time_))
1213 Eigen::Isometry3d ttr = tf2::transformToEigen(
1214 tf_buffer_->lookupTransform(target_frame, link_shape_handle.first->getName(), target_time));
1215 for (std::size_t j = 0; j < link_shape_handle.second.size(); ++j)
1217 cache[link_shape_handle.second[j].first] =
1218 ttr * link_shape_handle.first->getCollisionOriginTransforms()[link_shape_handle.second[j].second];
1223 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>&
1226 if (
tf_buffer_->canTransform(target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time,
1227 shape_transform_cache_lookup_wait_time_))
1229 Eigen::Isometry3d transform = tf2::transformToEigen(
tf_buffer_->lookupTransform(
1230 target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time));
1231 for (std::size_t k = 0; k < attached_body_shape_handle.second.size(); ++k)
1233 cache[attached_body_shape_handle.second[k].first] =
1235 attached_body_shape_handle.first->getShapePosesInLinkFrame()[attached_body_shape_handle.second[k].second];
1240 if (
tf_buffer_->canTransform(target_frame,
scene_->getPlanningFrame(), target_time,
1241 shape_transform_cache_lookup_wait_time_))
1243 Eigen::Isometry3d transform =
1244 tf2::transformToEigen(
tf_buffer_->lookupTransform(target_frame,
scene_->getPlanningFrame(), target_time));
1245 for (
const std::pair<
const std::string,
1246 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
1249 for (
const std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
1250 collision_body_shape_handle.second)
1251 cache[it.first] = transform * (*it.second);
1256 catch (tf2::TransformException& ex)
1258 rclcpp::Clock steady_clock = rclcpp::Clock();
1259 #pragma GCC diagnostic push
1260 #pragma GCC diagnostic ignored "-Wold-style-cast"
1261 RCLCPP_ERROR_THROTTLE(logger_, steady_clock, 1000,
"Transform error: %s", ex.what());
1262 #pragma GCC diagnostic pop
1269 const std::string& planning_scene_world_topic,
1270 const bool load_octomap_monitor)
1273 RCLCPP_INFO(logger_,
"Starting world geometry update monitor for collision objects, attached objects, octomap "
1277 if (!collision_objects_topic.empty())
1280 collision_objects_topic, rclcpp::SystemDefaultsQoS(),
1282 RCLCPP_INFO(logger_,
"Listening to '%s'", collision_objects_topic.c_str());
1285 if (!planning_scene_world_topic.empty())
1288 planning_scene_world_topic, rclcpp::SystemDefaultsQoS(),
1289 [
this](
const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) {
1292 RCLCPP_INFO(logger_,
"Listening to '%s' for planning scene world geometry", planning_scene_world_topic.c_str());
1296 if (load_octomap_monitor)
1301 std::make_unique<occupancy_map_monitor::OccupancyMapMonitor>(
node_,
tf_buffer_,
scene_->getPlanningFrame());
1306 octomap_monitor_->setTransformCacheCallback([
this](
const std::string& frame,
const rclcpp::Time& stamp,
1320 RCLCPP_INFO(logger_,
"Stopping world geometry monitor");
1325 RCLCPP_INFO(logger_,
"Stopping world geometry monitor");
1333 const std::string& attached_objects_topic)
1344 [
this](
const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state) {
return onStateUpdate(joint_state); });
1348 std::unique_lock<std::mutex> lock(state_pending_mutex_);
1349 if (dt_state_update_.count() > 0)
1353 state_update_timer_ =
1354 pnode_->create_wall_timer(dt_state_update_, [
this]() {
return stateUpdateTimerCallback(); });
1358 if (!attached_objects_topic.empty())
1362 attached_objects_topic, rclcpp::SystemDefaultsQoS(),
1363 [
this](
const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) {
1366 RCLCPP_INFO(logger_,
"Listening to '%s' for attached collision objects",
1371 RCLCPP_ERROR(logger_,
"Cannot monitor robot state because planning scene is not configured");
1382 if (state_update_timer_)
1383 state_update_timer_->cancel();
1385 std::unique_lock<std::mutex> lock(state_pending_mutex_);
1386 state_update_pending_ =
false;
1390 void PlanningSceneMonitor::onStateUpdate(
const sensor_msgs::msg::JointState::ConstSharedPtr& )
1392 const std::chrono::system_clock::time_point& n = std::chrono::system_clock::now();
1393 std::chrono::duration<double> dt = n - last_robot_state_update_wall_time_;
1397 std::unique_lock<std::mutex> lock(state_pending_mutex_);
1399 if (dt.count() < dt_state_update_.count())
1401 state_update_pending_ =
true;
1405 state_update_pending_ =
false;
1406 last_robot_state_update_wall_time_ = n;
1415 void PlanningSceneMonitor::stateUpdateTimerCallback()
1417 if (state_update_pending_)
1421 std::chrono::system_clock::time_point n = std::chrono::system_clock::now();
1422 std::chrono::duration<double> dt = n - last_robot_state_update_wall_time_;
1426 std::unique_lock<std::mutex> lock(state_pending_mutex_);
1427 if (state_update_pending_ && dt.count() >= dt_state_update_.count())
1429 state_update_pending_ =
false;
1430 last_robot_state_update_wall_time_ = std::chrono::system_clock::now();
1431 auto sec = std::chrono::duration<double>(last_robot_state_update_wall_time_.time_since_epoch()).count();
1433 RCLCPP_DEBUG(logger_,
"performPendingStateUpdate: %f", fmod(sec, 10));
1441 RCLCPP_DEBUG(logger_,
"performPendingStateUpdate done");
1473 if (hz > std::numeric_limits<double>::epsilon())
1475 std::unique_lock<std::mutex> lock(state_pending_mutex_);
1476 dt_state_update_ = std::chrono::duration<double>(1.0 / hz);
1479 state_update_timer_ =
pnode_->create_wall_timer(dt_state_update_, [
this]() {
return stateUpdateTimerCallback(); });
1486 if (state_update_timer_)
1487 state_update_timer_->cancel();
1488 std::unique_lock<std::mutex> lock(state_pending_mutex_);
1489 dt_state_update_ = std::chrono::duration<double>(0.0);
1490 if (state_update_pending_)
1493 RCLCPP_INFO(logger_,
"Updating internal planning scene state at most every %lf seconds", dt_state_update_.count());
1501 rclcpp::Time time =
node_->now();
1502 rclcpp::Clock steady_clock = rclcpp::Clock(RCL_STEADY_TIME);
1505 std::vector<std::string> missing;
1509 std::string missing_str = fmt::format(
"{}", fmt::join(missing,
", "));
1510 #pragma GCC diagnostic push
1511 #pragma GCC diagnostic ignored "-Wold-style-cast"
1512 RCLCPP_WARN_THROTTLE(logger_, steady_clock, 1000,
"The complete state of the robot is not yet known. Missing %s",
1513 missing_str.c_str());
1514 #pragma GCC diagnostic pop
1522 scene_->getCurrentStateNonConst().update();
1528 #pragma GCC diagnostic push
1529 #pragma GCC diagnostic ignored "-Wold-style-cast"
1530 RCLCPP_ERROR_THROTTLE(logger_, steady_clock, 1000,
1531 "State monitor is not active. Unable to set the planning scene state");
1532 #pragma GCC diagnostic pop
1552 RCLCPP_DEBUG(logger_,
"Maximum frequency for publishing a planning scene is now %lf Hz",
1556 void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms)
1558 const std::string& target =
getRobotModel()->getModelFrame();
1560 std::vector<std::string> all_frame_names;
1561 tf_buffer_->_getFrameStrings(all_frame_names);
1562 for (
const std::string& all_frame_name : all_frame_names)
1564 if (all_frame_name == target ||
getRobotModel()->hasLinkModel(all_frame_name))
1567 geometry_msgs::msg::TransformStamped
f;
1570 f =
tf_buffer_->lookupTransform(target, all_frame_name, tf2::TimePointZero);
1572 catch (tf2::TransformException& ex)
1574 RCLCPP_WARN(logger_,
"Unable to transform object from frame '%s' to planning frame'%s' (%s)",
1575 all_frame_name.c_str(), target.c_str(), ex.what());
1578 f.header.frame_id = all_frame_name;
1579 f.child_frame_id = target;
1580 transforms.push_back(
f);
1588 std::vector<geometry_msgs::msg::TransformStamped> transforms;
1589 getUpdatedFrameTransforms(transforms);
1592 scene_->getTransformsNonConst().setTransforms(transforms);
1617 RCLCPP_DEBUG(logger_,
"No additional default collision operations specified");
1621 RCLCPP_DEBUG(logger_,
"Reading additional default collision operations");
1665 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.
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)
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_
const moveit::core::RobotModelConstPtr & getRobotModel() const
AttachedBodyShapeHandles attached_body_shape_handles_
std::map< std::string, double > default_robot_link_padd_
default robot link padding
LinkShapeHandles link_shape_handles_
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.
void newPlanningSceneWorldCallback(const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr &world)
Callback for a new planning scene world.
planning_scene::PlanningSceneConstPtr scene_const_
rclcpp::Subscription< moveit_msgs::msg::CollisionObject >::SharedPtr collision_object_subscriber_
void includeAttachedBodyInOctree(const moveit::core::AttachedBody *attached_body)
void octomapUpdateCallback()
Callback for octomap updates.
rclcpp::Subscription< moveit_msgs::msg::PlanningScene >::SharedPtr planning_scene_subscriber_
void clearUpdateCallbacks()
Clear the functions to be called when an update to the scene is received.
void providePlanningSceneService(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
Create an optional service for getting the complete planning scene This is useful for satisfying the ...
std::shared_mutex scene_update_mutex_
if diffs are monitored, this is the pointer to the parent scene
void setStateUpdateFrequency(double hz)
Update the scene using the monitored state at a specified frequency, in Hz. This function has an effe...
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
void updateSceneWithCurrentState()
Update the scene using the monitored state. This function is automatically called when an update to t...
rclcpp::Time last_robot_motion_time_
Last time the state was updated.
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...
void currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody *attached_body, bool just_attached)
Callback for a change for an attached object of the current state of the planning scene.
std::unique_ptr< std::thread > publish_planning_scene_
std::shared_ptr< rclcpp::Node > node_
Last time the robot has moved.
void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene)
Configure the collision matrix for a particular scene.
void startWorldGeometryMonitor(const std::string &collision_objects_topic=DEFAULT_COLLISION_OBJECT_TOPIC, const std::string &planning_scene_world_topic=DEFAULT_PLANNING_SCENE_WORLD_TOPIC, const bool load_octomap_monitor=true)
Start the OccupancyMapMonitor and listening for:
rclcpp::Publisher< moveit_msgs::msg::PlanningScene >::SharedPtr planning_scene_publisher_
void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj)
std::vector< std::function< void(SceneUpdateType)> > update_callbacks_
void updateFrameTransforms()
Update the transforms for the frames that are not part of the kinematic model using tf....
planning_scene::PlanningScenePtr parent_scene_
std::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_
rclcpp::Logger getLogger()
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
void update(moveit::core::RobotState *self, bool force, std::string &category)
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.