41 #include <moveit_msgs/srv/get_planning_scene.hpp>
43 #include <rclcpp/qos.hpp>
46 #if __has_include(<tf2/exceptions.hpp>)
47 #include <tf2/exceptions.hpp>
49 #include <tf2/exceptions.h>
51 #if __has_include(<tf2/LinearMath/Transform.hpp>)
52 #include <tf2/LinearMath/Transform.hpp>
54 #include <tf2/LinearMath/Transform.h>
56 #include <tf2_eigen/tf2_eigen.hpp>
57 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
59 #include <boost/algorithm/string/join.hpp>
62 #include <std_msgs/msg/string.hpp>
65 using namespace std::chrono_literals;
67 static const rclcpp::Logger
LOGGER = rclcpp::get_logger(
"moveit_ros.planning_scene_monitor.planning_scene_monitor");
71 const std::string PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC =
"joint_states";
72 const std::string PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC =
"attached_collision_object";
73 const std::string PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC =
"collision_object";
74 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC =
"planning_scene_world";
75 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC =
"planning_scene";
76 const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE =
"get_planning_scene";
77 const std::string PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC =
"monitored_planning_scene";
79 PlanningSceneMonitor::PlanningSceneMonitor(
const rclcpp::Node::SharedPtr& node,
const std::string& robot_description,
80 const std::string&
name)
86 const planning_scene::PlanningScenePtr&
scene,
87 const std::string& robot_description,
const std::string&
name)
94 const robot_model_loader::RobotModelLoaderPtr& rm_loader,
95 const std::string&
name)
101 const planning_scene::PlanningScenePtr&
scene,
102 const robot_model_loader::RobotModelLoaderPtr& rm_loader,
103 const std::string&
name)
104 : monitor_name_(
name)
106 , private_executor_(std::make_shared<
rclcpp::executors::SingleThreadedExecutor>())
107 , tf_buffer_(std::make_shared<
tf2_ros::Buffer>(node->get_clock()))
108 , dt_state_update_(0.0)
109 , shape_transform_cache_lookup_wait_time_(0, 0)
110 , rm_loader_(rm_loader)
112 std::vector<std::string> new_args = rclcpp::NodeOptions().arguments();
113 new_args.push_back(
"--ros-args");
114 new_args.push_back(
"-r");
116 new_args.push_back(std::string(
"__node:=") +
node_->get_name() +
"_private_" +
117 std::to_string(
reinterpret_cast<std::size_t
>(
this)));
118 new_args.push_back(
"--");
119 pnode_ = std::make_shared<rclcpp::Node>(
"_",
node_->get_namespace(), rclcpp::NodeOptions().arguments(new_args));
129 use_sim_time_ = node->get_parameter(
"use_sim_time").as_bool();
153 robot_model_.reset();
166 scene->setPlanningSceneDiffMsg(diff);
175 if (rm_loader_->getModel())
177 robot_model_ = rm_loader_->getModel();
183 scene_ = std::make_shared<planning_scene::PlanningScene>(rm_loader_->getModel());
191 scene_->getCollisionEnvNonConst()->setLinkPadding(it.first, it.second);
195 scene_->getCollisionEnvNonConst()->setLinkScale(it.first, it.second);
200 RCLCPP_ERROR(LOGGER,
"Configuration of planning scene failed");
213 scene_->setCollisionObjectUpdateCallback(
221 RCLCPP_ERROR(LOGGER,
"Robot model not loaded");
228 last_robot_state_update_wall_time_ = std::chrono::system_clock::now();
229 dt_state_update_ = std::chrono::duration<double>(0.03);
231 double temp_wait_time = 0.05;
234 node_->get_parameter_or(
robot_description_ +
"_planning.shape_transform_cache_lookup_wait_time", temp_wait_time,
237 shape_transform_cache_lookup_wait_time_ = rclcpp::Duration::from_seconds(temp_wait_time);
239 state_update_pending_.store(
false);
241 using std::chrono::nanoseconds;
242 state_update_timer_ =
pnode_->create_wall_timer(dt_state_update_, [
this]() {
return stateUpdateTimerCallback(); });
251 rcl_interfaces::msg::ParameterDescriptor desc;
253 return pnode_->declare_parameter(param_name, default_val, desc);
259 bool publish_planning_scene =
260 declare_parameter(
"publish_planning_scene",
false,
"Set to True to publish Planning Scenes");
261 bool publish_geometry_updates =
declare_parameter(
"publish_geometry_updates",
false,
262 "Set to True to publish geometry updates of the planning scene");
263 bool publish_state_updates =
264 declare_parameter(
"publish_state_updates",
false,
"Set to True to publish state updates of the planning scene");
266 "publish_transforms_updates",
false,
"Set to True to publish transform updates of the planning scene");
268 "publish_planning_scene_hz", 4.0,
"Set the maximum frequency at which planning scene updates are published");
269 updatePublishSettings(publish_geometry_updates, publish_state_updates, publish_transform_updates,
270 publish_planning_scene, publish_planning_scene_hz);
272 catch (
const rclcpp::exceptions::InvalidParameterTypeException& e)
274 RCLCPP_ERROR_STREAM(LOGGER,
"Invalid parameter type in PlanningSceneMonitor: " << e.what());
275 RCLCPP_ERROR(LOGGER,
"Dynamic publishing parameters won't be available");
279 auto psm_parameter_set_callback = [
this](
const std::vector<rclcpp::Parameter>& parameters) ->
auto
281 auto result = rcl_interfaces::msg::SetParametersResult();
282 result.successful =
true;
284 bool publish_planning_scene =
false, publish_geometry_updates =
false, publish_state_updates =
false,
285 publish_transform_updates =
false;
286 double publish_planning_scene_hz = 4.0;
287 bool declared_params_valid =
pnode_->get_parameter(
"publish_planning_scene", publish_planning_scene) &&
288 pnode_->get_parameter(
"publish_geometry_updates", publish_geometry_updates) &&
289 pnode_->get_parameter(
"publish_state_updates", publish_state_updates) &&
290 pnode_->get_parameter(
"publish_transforms_updates", publish_transform_updates) &&
291 pnode_->get_parameter(
"publish_planning_scene_hz", publish_planning_scene_hz);
293 if (!declared_params_valid)
295 RCLCPP_ERROR(LOGGER,
"Initially declared parameters are invalid - failed to process update callback");
296 result.successful =
false;
300 for (
const auto& parameter : parameters)
302 const auto&
name = parameter.get_name();
303 const auto&
type = parameter.get_type();
308 RCLCPP_ERROR(LOGGER,
"Invalid parameter in PlanningSceneMonitor parameter callback");
309 result.successful =
false;
314 if (
name ==
"planning_scene_monitor.publish_planning_scene")
315 publish_planning_scene = parameter.as_bool();
316 else if (
name ==
"planning_scene_monitor.publish_geometry_updates")
317 publish_geometry_updates = parameter.as_bool();
318 else if (
name ==
"planning_scene_monitor.publish_state_updates")
319 publish_state_updates = parameter.as_bool();
320 else if (
name ==
"planning_scene_monitor.publish_transforms_updates")
321 publish_transform_updates = parameter.as_bool();
322 else if (
name ==
"planning_scene_monitor.publish_planning_scene_hz")
323 publish_planning_scene_hz = parameter.as_double();
326 if (result.successful)
327 updatePublishSettings(publish_geometry_updates, publish_state_updates, publish_transform_updates,
328 publish_planning_scene, publish_planning_scene_hz);
332 callback_handler_ =
pnode_->add_on_set_parameters_callback(psm_parameter_set_callback);
353 scene_->setCollisionObjectUpdateCallback(
354 [
this](
const collision_detection::World::ObjectConstPtr&
object,
362 RCLCPP_WARN(LOGGER,
"Diff monitoring was stopped while publishing planning scene diffs. "
363 "Stopping planning scene diff publisher");
373 if (!
scene_->getName().empty())
375 if (
scene_->getName()[
scene_->getName().length() - 1] ==
'+')
388 std::unique_ptr<std::thread> copy;
394 RCLCPP_INFO(LOGGER,
"Stopped publishing maintained planning scene.");
399 const std::string& planning_scene_topic)
405 RCLCPP_INFO(LOGGER,
"Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
411 void PlanningSceneMonitor::scenePublishingThread()
413 RCLCPP_DEBUG(LOGGER,
"Started scene publishing thread ...");
417 moveit_msgs::msg::PlanningScene msg;
422 scene_->getPlanningSceneMsg(msg);
425 RCLCPP_DEBUG(LOGGER,
"Published the full planning scene: '%s'", msg.name.c_str());
430 moveit_msgs::msg::PlanningScene msg;
431 bool publish_msg =
false;
432 bool is_full =
false;
449 scene_->getPlanningSceneDiffMsg(msg);
452 msg.robot_state.attached_collision_objects.clear();
453 msg.robot_state.is_diff =
true;
468 scene_->setCollisionObjectUpdateCallback(
469 [
this](
const collision_detection::World::ObjectConstPtr&
object,
481 scene_->getPlanningSceneMsg(msg);
494 RCLCPP_DEBUG(LOGGER,
"Published full planning scene: '%s'", msg.name.c_str());
525 bool sceneIsParentOf(
const planning_scene::PlanningSceneConstPtr&
scene,
531 return sceneIsParentOf(
scene->getParent(), possible_parent);
552 update_callback(update_type);
561 RCLCPP_FATAL_STREAM(LOGGER,
"requestPlanningSceneState() to self-provided service '" << service_name <<
"'");
562 throw std::runtime_error(
"requestPlanningSceneState() to self-provided service: " + service_name);
565 auto client =
pnode_->create_client<moveit_msgs::srv::GetPlanningScene>(service_name);
566 auto srv = std::make_shared<moveit_msgs::srv::GetPlanningScene::Request>();
567 srv->components.components = srv->components.SCENE_SETTINGS | srv->components.ROBOT_STATE |
568 srv->components.ROBOT_STATE_ATTACHED_OBJECTS | srv->components.WORLD_OBJECT_NAMES |
569 srv->components.WORLD_OBJECT_GEOMETRY | srv->components.OCTOMAP |
570 srv->components.TRANSFORMS | srv->components.ALLOWED_COLLISION_MATRIX |
571 srv->components.LINK_PADDING_AND_SCALING | srv->components.OBJECT_COLORS;
574 RCLCPP_DEBUG(LOGGER,
"Waiting for GetPlanningScene service `%s` to exist.", service_name.c_str());
575 if (client->wait_for_service(std::chrono::seconds(5)))
577 RCLCPP_DEBUG(LOGGER,
"Sending service request to `%s`.", service_name.c_str());
578 auto service_result = client->async_send_request(srv);
579 const auto service_result_status = service_result.wait_for(std::chrono::seconds(5));
580 if (service_result_status == std::future_status::ready)
582 RCLCPP_DEBUG(LOGGER,
"Service request succeeded, applying new planning scene");
586 if (service_result_status == std::future_status::timeout)
588 RCLCPP_INFO(LOGGER,
"GetPlanningScene service call to %s timed out. at %s:%d", service_name.c_str(), __FILE__,
596 "Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?",
597 service_name.c_str());
606 service_name, [
this](
const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req,
607 const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res) {
608 return getPlanningSceneServiceCallback(req, res);
612 void PlanningSceneMonitor::getPlanningSceneServiceCallback(
613 const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req,
614 const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res)
616 if (req->components.components & moveit_msgs::msg::PlanningSceneComponents::TRANSFORMS)
619 moveit_msgs::msg::PlanningSceneComponents all_components;
620 all_components.components = UINT_MAX;
623 scene_->getPlanningSceneMsg(res->scene, req->components.components ? req->components : all_components);
626 void PlanningSceneMonitor::updatePublishSettings(
bool publish_geom_updates,
bool publish_state_updates,
627 bool publish_transform_updates,
bool publish_planning_scene,
628 double publish_planning_scene_hz)
631 if (publish_geom_updates)
634 if (publish_state_updates)
637 if (publish_transform_updates)
640 if (publish_planning_scene)
649 void PlanningSceneMonitor::newPlanningSceneCallback(
const moveit_msgs::msg::PlanningScene::ConstSharedPtr&
scene)
656 bool removed =
false;
659 removed =
scene_->getWorldNonConst()->removeObject(
scene_->OCTOMAP_NS);
669 RCLCPP_WARN(LOGGER,
"Unable to clear octomap since no octomap monitor has been initialized");
685 std::string old_scene_name;
693 RCLCPP_DEBUG(LOGGER,
"scene update %f robot stamp: %f", fmod(
last_update_time_.seconds(), 10.),
695 old_scene_name =
scene_->getName();
700 if (
scene.robot_state.is_diff)
716 if (!
scene.is_diff &&
scene.world.octomap.octomap.data.empty())
723 robot_model_ =
scene_->getRobotModel();
735 bool no_other_scene_upd = (
scene.name.empty() ||
scene.name == old_scene_name) &&
736 scene.allowed_collision_matrix.entry_names.empty() &&
scene.link_padding.empty() &&
737 scene.link_scale.empty();
738 if (no_other_scene_upd)
744 if (!
scene.fixed_frame_transforms.empty())
750 if (!
scene.robot_state.attached_collision_objects.empty() || !
scene.robot_state.is_diff)
760 const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world)
768 scene_->getWorldNonConst()->clearObjects();
769 scene_->processPlanningSceneWorldMsg(*world);
772 if (world->octomap.octomap.data.empty())
793 if (!
scene_->processCollisionObjectMsg(*obj))
807 scene_->processAttachedCollisionObjectMsg(*obj);
821 const std::vector<const moveit::core::LinkModel*>& links =
getRobotModel()->getLinkModelsWithCollisionGeometry();
822 auto start = std::chrono::system_clock::now();
826 std::vector<shapes::ShapeConstPtr>
shapes = link->getShapes();
827 for (std::size_t j = 0; j <
shapes.size(); ++j)
832 shapes::Mesh* m =
static_cast<shapes::Mesh*
>(
shapes[j]->clone());
833 m->mergeVertices(1e-4);
842 if (!warned && ((std::chrono::system_clock::now() - start) > std::chrono::seconds(30)))
844 RCLCPP_WARN(LOGGER,
"It is likely there are too many vertices in collision geometry");
858 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
860 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : link_shape_handle.second)
874 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& attached_body_shape_handle :
876 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : attached_body_shape_handle.second)
887 std::vector<const moveit::core::AttachedBody*> ab;
888 scene_->getCurrentState().getAttachedBodies(ab);
901 for (std::pair<
const std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
903 for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
904 collision_body_shape_handle.second)
914 for (
const std::pair<const std::string, collision_detection::World::ObjectPtr>& it : *
scene_->getWorld())
925 for (std::size_t i = 0; i < attached_body->
getShapes().size(); ++i)
927 if (attached_body->
getShapes()[i]->type == shapes::PLANE || attached_body->
getShapes()[i]->type == shapes::OCTREE)
937 RCLCPP_DEBUG(LOGGER,
"Excluding attached body '%s' from monitored octomap", attached_body->
getName().c_str());
950 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& shape_handle : it->second)
952 RCLCPP_DEBUG(LOGGER,
"Including attached body '%s' in monitored octomap", attached_body->
getName().c_str());
965 for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
967 if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
977 RCLCPP_DEBUG(LOGGER,
"Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
990 for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& shape_handle : it->second)
992 RCLCPP_DEBUG(LOGGER,
"Including collision object '%s' in monitored octomap", obj->id_.c_str());
1030 if (t.nanoseconds() == 0)
1032 RCLCPP_DEBUG(LOGGER,
"sync robot state to: %.3fs", fmod(t.seconds(), 10.));
1044 if (state_update_pending_.load())
1051 RCLCPP_WARN(LOGGER,
"Failed to fetch current robot state.");
1058 auto start =
node_->get_clock()->now();
1059 auto timeout = rclcpp::Duration::from_seconds(wait_time);
1063 timeout > rclcpp::Duration(0, 0))
1067 timeout = timeout - (
node_->get_clock()->now() - start);
1072 RCLCPP_WARN(LOGGER,
"Maybe failed to update robot state, time diff: %.3fs", (t -
last_robot_motion_time_).seconds());
1111 RCLCPP_INFO(LOGGER,
"Starting planning scene monitor");
1113 if (!scene_topic.empty())
1116 scene_topic, rclcpp::ServicesQoS(), [
this](
const moveit_msgs::msg::PlanningScene::ConstSharedPtr&
scene) {
1117 return newPlanningSceneCallback(
scene);
1127 RCLCPP_INFO(LOGGER,
"Stopping planning scene monitor");
1140 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
1143 if (
tf_buffer_->canTransform(target_frame, link_shape_handle.first->getName(), target_time,
1144 shape_transform_cache_lookup_wait_time_))
1146 Eigen::Isometry3d ttr = tf2::transformToEigen(
1147 tf_buffer_->lookupTransform(target_frame, link_shape_handle.first->getName(), target_time));
1148 for (std::size_t j = 0; j < link_shape_handle.second.size(); ++j)
1149 cache[link_shape_handle.second[j].first] =
1150 ttr * link_shape_handle.first->getCollisionOriginTransforms()[link_shape_handle.second[j].second];
1154 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>&
1157 if (
tf_buffer_->canTransform(target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time,
1158 shape_transform_cache_lookup_wait_time_))
1160 Eigen::Isometry3d transform = tf2::transformToEigen(
tf_buffer_->lookupTransform(
1161 target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time));
1162 for (std::size_t k = 0; k < attached_body_shape_handle.second.size(); ++k)
1163 cache[attached_body_shape_handle.second[k].first] =
1165 attached_body_shape_handle.first->getShapePosesInLinkFrame()[attached_body_shape_handle.second[k].second];
1169 if (
tf_buffer_->canTransform(target_frame,
scene_->getPlanningFrame(), target_time,
1170 shape_transform_cache_lookup_wait_time_))
1172 Eigen::Isometry3d transform =
1173 tf2::transformToEigen(
tf_buffer_->lookupTransform(target_frame,
scene_->getPlanningFrame(), target_time));
1174 for (
const std::pair<
const std::string,
1175 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
1177 for (
const std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
1178 collision_body_shape_handle.second)
1179 cache[it.first] = transform * (*it.second);
1183 catch (tf2::TransformException& ex)
1185 rclcpp::Clock steady_clock = rclcpp::Clock();
1186 RCLCPP_ERROR_THROTTLE(LOGGER, steady_clock, 1000,
"Transform error: %s", ex.what());
1193 const std::string& planning_scene_world_topic,
1194 const bool load_octomap_monitor)
1197 RCLCPP_INFO(LOGGER,
"Starting world geometry update monitor for collision objects, attached objects, octomap "
1201 if (!collision_objects_topic.empty())
1204 collision_objects_topic, rclcpp::ServicesQoS(),
1205 [
this](
const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) {
return collisionObjectCallback(obj); });
1206 RCLCPP_INFO(LOGGER,
"Listening to '%s'", collision_objects_topic.c_str());
1209 if (!planning_scene_world_topic.empty())
1212 planning_scene_world_topic, rclcpp::ServicesQoS(),
1213 [
this](
const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) {
1216 RCLCPP_INFO(LOGGER,
"Listening to '%s' for planning scene world geometry", planning_scene_world_topic.c_str());
1220 if (load_octomap_monitor)
1225 std::make_unique<occupancy_map_monitor::OccupancyMapMonitor>(
node_,
tf_buffer_,
scene_->getPlanningFrame());
1230 octomap_monitor_->setTransformCacheCallback([
this](
const std::string& frame,
const rclcpp::Time& stamp,
1244 RCLCPP_INFO(LOGGER,
"Stopping world geometry monitor");
1249 RCLCPP_INFO(LOGGER,
"Stopping world geometry monitor");
1257 const std::string& attached_objects_topic)
1268 [
this](
const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state) {
return onStateUpdate(joint_state); });
1272 std::unique_lock<std::mutex> lock(state_update_mutex_);
1273 if (dt_state_update_.count() > 0)
1276 state_update_timer_ =
1277 pnode_->create_wall_timer(dt_state_update_, [
this]() {
return stateUpdateTimerCallback(); });
1280 if (!attached_objects_topic.empty())
1284 attached_objects_topic, rclcpp::ServicesQoS(),
1285 [
this](
const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) {
1288 RCLCPP_INFO(LOGGER,
"Listening to '%s' for attached collision objects",
1293 RCLCPP_ERROR(LOGGER,
"Cannot monitor robot state because planning scene is not configured");
1303 if (state_update_timer_)
1304 state_update_timer_->cancel();
1305 state_update_pending_.store(
false);
1308 void PlanningSceneMonitor::onStateUpdate(
const sensor_msgs::msg::JointState::ConstSharedPtr& )
1310 state_update_pending_.store(
true);
1315 if (std::chrono::system_clock::now() - last_robot_state_update_wall_time_ >= dt_state_update_)
1319 void PlanningSceneMonitor::stateUpdateTimerCallback()
1323 if (state_update_pending_.load() &&
1324 std::chrono::system_clock::now() - last_robot_state_update_wall_time_ >= dt_state_update_)
1354 bool update =
false;
1355 if (hz > std::numeric_limits<double>::epsilon())
1357 std::unique_lock<std::mutex> lock(state_update_mutex_);
1358 dt_state_update_ = std::chrono::duration<double>(1.0 / hz);
1361 state_update_timer_ =
pnode_->create_wall_timer(dt_state_update_, [
this]() {
return stateUpdateTimerCallback(); });
1368 if (state_update_timer_)
1369 state_update_timer_->cancel();
1370 std::unique_lock<std::mutex> lock(state_update_mutex_);
1371 dt_state_update_ = std::chrono::duration<double>(0.0);
1372 if (state_update_pending_.load())
1375 RCLCPP_INFO(LOGGER,
"Updating internal planning scene state at most every %lf seconds", dt_state_update_.count());
1383 rclcpp::Time time =
node_->now();
1384 rclcpp::Clock steady_clock = rclcpp::Clock(RCL_STEADY_TIME);
1387 std::vector<std::string> missing;
1391 std::string missing_str = boost::algorithm::join(missing,
", ");
1392 RCLCPP_WARN_THROTTLE(LOGGER, steady_clock, 1000,
"The complete state of the robot is not yet known. Missing %s",
1393 missing_str.c_str());
1398 if (!skip_update_if_locked)
1402 else if (!ulock.try_lock())
1410 scene_->getCurrentStateNonConst().update();
1415 std::unique_lock<std::mutex> lock(state_update_mutex_);
1416 last_robot_state_update_wall_time_ = std::chrono::system_clock::now();
1417 state_update_pending_.store(
false);
1423 RCLCPP_ERROR_THROTTLE(LOGGER, steady_clock, 1000,
1424 "State monitor is not active. Unable to set the planning scene state");
1443 RCLCPP_DEBUG(LOGGER,
"Maximum frequency for publishing a planning scene is now %lf Hz",
1447 void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms)
1449 const std::string& target =
getRobotModel()->getModelFrame();
1451 std::vector<std::string> all_frame_names;
1452 tf_buffer_->_getFrameStrings(all_frame_names);
1453 for (
const std::string& all_frame_name : all_frame_names)
1455 if (all_frame_name == target ||
getRobotModel()->hasLinkModel(all_frame_name))
1458 geometry_msgs::msg::TransformStamped
f;
1461 f =
tf_buffer_->lookupTransform(target, all_frame_name, tf2::TimePointZero);
1463 catch (tf2::TransformException& ex)
1465 RCLCPP_WARN(LOGGER,
"Unable to transform object from frame '%s' to planning frame'%s' (%s)",
1466 all_frame_name.c_str(), target.c_str(), ex.what());
1469 f.header.frame_id = all_frame_name;
1470 f.child_frame_id = target;
1471 transforms.push_back(
f);
1479 std::vector<geometry_msgs::msg::TransformStamped> transforms;
1480 getUpdatedFrameTransforms(transforms);
1483 scene_->getTransformsNonConst().setTransforms(transforms);
1507 RCLCPP_DEBUG(LOGGER,
"No additional default collision operations specified");
1510 RCLCPP_DEBUG(LOGGER,
"Reading additional default collision operations");
1554 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 PlanningScenePtr clone(const PlanningSceneConstPtr &scene)
Clone a planning scene. Even if the scene scene depends on a parent, the cloned scene will not.
static const std::string OCTOMAP_NS
PlanningSceneMonitor Subscribes to the topic planning_scene.
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
void lockSceneRead()
Lock the scene for reading (multiple threads can lock for reading at the same time)
void lockSceneWrite()
Lock the scene for writing (only one thread can lock for writing and no other thread can lock for rea...
void unlockSceneRead()
Unlock the scene from reading (multiple threads can lock for reading at the same time)
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_
planning_scene::PlanningScenePtr copyPlanningScene(const moveit_msgs::msg::PlanningScene &diff=moveit_msgs::msg::PlanningScene())
Returns a copy of the current planning scene.
const moveit::core::RobotModelConstPtr & getRobotModel() const
AttachedBodyShapeHandles attached_body_shape_handles_
void attachObjectCallback(const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr &obj)
Callback for a new attached object msg.
std::map< std::string, double > default_robot_link_padd_
default robot link padding
LinkShapeHandles link_shape_handles_
void collisionObjectCallback(const moveit_msgs::msg::CollisionObject::ConstSharedPtr &obj)
Callback for a new collision object msg.
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.
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 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...
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::Parameter declare_parameter(const rclcpp::Node::SharedPtr &node, const std::string ¶meter_name)
bool isEmpty(const moveit_msgs::msg::PlanningScene &msg)
Check if a message includes any information about a planning scene, or whether it is empty.
std::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
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.
const rclcpp::Logger LOGGER