From f07aae8dabebe705742c73b47efe876912184be8 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 29 Nov 2023 11:10:45 +0800 Subject: [PATCH 1/2] Sanitize node names to avoid plugin exceptions (#110) Signed-off-by: Luca Della Vedova (cherry picked from commit 53dcf3fc9c02cf26207047eb0949a6e3874b1b2f) Signed-off-by: Aaron Chong --- .../include/rmf_building_sim_common/utils.hpp | 3 +++ rmf_building_sim_common/src/utils.cpp | 19 ++++++++++++++++++- .../src/door.cpp | 3 ++- .../src/lift.cpp | 3 ++- rmf_building_sim_gz_plugins/src/door.cpp | 1 + rmf_building_sim_gz_plugins/src/lift.cpp | 1 + .../include/rmf_robot_sim_common/utils.hpp | 2 ++ rmf_robot_sim_common/src/utils.cpp | 15 +++++++++++++++ .../src/slotcar.cpp | 3 ++- rmf_robot_sim_gz_plugins/src/slotcar.cpp | 1 + 10 files changed, 47 insertions(+), 4 deletions(-) diff --git a/rmf_building_sim_common/include/rmf_building_sim_common/utils.hpp b/rmf_building_sim_common/include/rmf_building_sim_common/utils.hpp index 2558cb6..c69ec9e 100644 --- a/rmf_building_sim_common/include/rmf_building_sim_common/utils.hpp +++ b/rmf_building_sim_common/include/rmf_building_sim_common/utils.hpp @@ -33,6 +33,9 @@ double compute_desired_rate_of_change( const MotionParams& _motion_params, const double _dt); +//============================================================================== +void sanitize_node_name(std::string& node_name); + //============================================================================== template bool get_element_required( diff --git a/rmf_building_sim_common/src/utils.cpp b/rmf_building_sim_common/src/utils.cpp index 812f65c..4fea4a6 100644 --- a/rmf_building_sim_common/src/utils.cpp +++ b/rmf_building_sim_common/src/utils.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -98,4 +99,20 @@ double compute_desired_rate_of_change( // Flip the sign to the correct direction before returning the value return sign * v_next; } -} // namespace rmf_building_sim_common + +// ROS 2 nodes can only have alphanumeric characters and underscores, this +// function removes special characters and replaces dashes / spaces with +// underscores to avoid throwing exceptions. +//============================================================================== +void sanitize_node_name(std::string& node_name) +{ + std::replace(node_name.begin(), node_name.end(), ' ', '_'); + std::replace(node_name.begin(), node_name.end(), '-', '_'); + node_name.erase(std::remove_if(node_name.begin(), node_name.end(), + [](auto const& c) -> bool + { + return c != '_' && !std::isalnum(c); + }), node_name.end()); +} +} +// namespace rmf_building_sim_common diff --git a/rmf_building_sim_gz_classic_plugins/src/door.cpp b/rmf_building_sim_gz_classic_plugins/src/door.cpp index 83ae993..d1c33b0 100644 --- a/rmf_building_sim_gz_classic_plugins/src/door.cpp +++ b/rmf_building_sim_gz_classic_plugins/src/door.cpp @@ -32,7 +32,8 @@ class DoorPlugin : public gazebo::ModelPlugin void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override { - const std::string& node_name = model->GetName() + "_node"; + std::string node_name = model->GetName() + "_node"; + sanitize_node_name(node_name); auto _ros_node = gazebo_ros::Node::Get(sdf, node_name); _model = model; diff --git a/rmf_building_sim_gz_classic_plugins/src/lift.cpp b/rmf_building_sim_gz_classic_plugins/src/lift.cpp index 2b8e4e7..2f159e3 100644 --- a/rmf_building_sim_gz_classic_plugins/src/lift.cpp +++ b/rmf_building_sim_gz_classic_plugins/src/lift.cpp @@ -34,7 +34,8 @@ class LiftPlugin : public gazebo::ModelPlugin void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override { - const std::string& node_name = model->GetName() + "_node"; + std::string node_name = model->GetName() + "_node"; + sanitize_node_name(node_name); _ros_node = gazebo_ros::Node::Get(sdf, node_name); _model = model; diff --git a/rmf_building_sim_gz_plugins/src/door.cpp b/rmf_building_sim_gz_plugins/src/door.cpp index 6611254..a380d8a 100644 --- a/rmf_building_sim_gz_plugins/src/door.cpp +++ b/rmf_building_sim_gz_plugins/src/door.cpp @@ -146,6 +146,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin if (!rclcpp::ok()) rclcpp::init(0, argv); std::string plugin_name("plugin_" + name); + sanitize_node_name(plugin_name); _ros_node = std::make_shared(plugin_name); RCLCPP_INFO(_ros_node->get_logger(), diff --git a/rmf_building_sim_gz_plugins/src/lift.cpp b/rmf_building_sim_gz_plugins/src/lift.cpp index 1fb23f5..fb1df6c 100644 --- a/rmf_building_sim_gz_plugins/src/lift.cpp +++ b/rmf_building_sim_gz_plugins/src/lift.cpp @@ -142,6 +142,7 @@ class IGNITION_GAZEBO_VISIBLE LiftPlugin if (!rclcpp::ok()) rclcpp::init(0, argv); std::string plugin_name("plugin_" + model.Name(ecm)); + sanitize_node_name(plugin_name); _ros_node = std::make_shared(plugin_name); RCLCPP_INFO(_ros_node->get_logger(), diff --git a/rmf_robot_sim_common/include/rmf_robot_sim_common/utils.hpp b/rmf_robot_sim_common/include/rmf_robot_sim_common/utils.hpp index 2af233b..ab819d8 100644 --- a/rmf_robot_sim_common/include/rmf_robot_sim_common/utils.hpp +++ b/rmf_robot_sim_common/include/rmf_robot_sim_common/utils.hpp @@ -89,6 +89,8 @@ double compute_desired_rate_of_change( rclcpp::Time simulation_now(double t); +void sanitize_node_name(std::string& node_name); + //////////////////////////////////////////////////////////////////////////////////////////////////// template bool get_element_required( diff --git a/rmf_robot_sim_common/src/utils.cpp b/rmf_robot_sim_common/src/utils.cpp index 2f17e97..5d35f63 100644 --- a/rmf_robot_sim_common/src/utils.cpp +++ b/rmf_robot_sim_common/src/utils.cpp @@ -99,4 +99,19 @@ rclcpp::Time simulation_now(double t) return rclcpp::Time{t_sec, t_nsec, RCL_ROS_TIME}; } +// ROS 2 nodes can only have alphanumeric characters and underscores, this +// function removes special characters and replaces dashes / spaces with +// underscores to avoid throwing exceptions. +//============================================================================== +void sanitize_node_name(std::string& node_name) +{ + std::replace(node_name.begin(), node_name.end(), ' ', '_'); + std::replace(node_name.begin(), node_name.end(), '-', '_'); + node_name.erase(std::remove_if(node_name.begin(), node_name.end(), + [](auto const& c) -> bool + { + return c != '_' && !std::isalnum(c); + }), node_name.end()); +} + } // namespace rmf_plugins_utils diff --git a/rmf_robot_sim_gz_classic_plugins/src/slotcar.cpp b/rmf_robot_sim_gz_classic_plugins/src/slotcar.cpp index 3d11f38..702ebd0 100644 --- a/rmf_robot_sim_gz_classic_plugins/src/slotcar.cpp +++ b/rmf_robot_sim_gz_classic_plugins/src/slotcar.cpp @@ -82,7 +82,8 @@ void SlotcarPlugin::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) _model = model; dataPtr->set_model_name(_model->GetName()); dataPtr->read_sdf(sdf); - const std::string& node_name = _model->GetName() + "_node"; + std::string node_name = _model->GetName() + "_node"; + rmf_plugins_utils::sanitize_node_name(node_name); gazebo_ros::Node::SharedPtr _ros_node = gazebo_ros::Node::Get(sdf, node_name); dataPtr->init_ros_node(_ros_node); diff --git a/rmf_robot_sim_gz_plugins/src/slotcar.cpp b/rmf_robot_sim_gz_plugins/src/slotcar.cpp index dbf2d73..5eac250 100644 --- a/rmf_robot_sim_gz_plugins/src/slotcar.cpp +++ b/rmf_robot_sim_gz_plugins/src/slotcar.cpp @@ -120,6 +120,7 @@ void SlotcarPlugin::Configure(const Entity& entity, if (!rclcpp::ok()) rclcpp::init(0, argv); std::string plugin_name("plugin_" + model_name); + rmf_plugins_utils::sanitize_node_name(plugin_name); _ros_node = std::make_shared(plugin_name); // TODO Check if executor is getting callbacks //executor = std::make_unique(); From d870e2bca7a8811589e8b609e4297002f5d2f0b8 Mon Sep 17 00:00:00 2001 From: Teo Koon Peng Date: Thu, 1 Feb 2024 10:17:02 +0800 Subject: [PATCH 2/2] Explicitly specify all qos depth (#116) (cherry picked from commit f71d2d3d2fcc50e10537b995fc91f256668360da) Signed-off-by: Aaron Chong --- rmf_building_sim_common/src/door_common.cpp | 4 ++-- rmf_building_sim_common/src/lift_common.cpp | 8 ++++---- rmf_building_sim_gz_classic_plugins/src/toggle_floors.cpp | 2 +- rmf_robot_sim_common/src/dispenser_common.cpp | 4 ++-- rmf_robot_sim_common/src/ingestor_common.cpp | 4 ++-- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/rmf_building_sim_common/src/door_common.cpp b/rmf_building_sim_common/src/door_common.cpp index 729805e..67785db 100644 --- a/rmf_building_sim_common/src/door_common.cpp +++ b/rmf_building_sim_common/src/door_common.cpp @@ -72,10 +72,10 @@ DoorCommon::DoorCommon(const std::string& door_name, _request.requested_mode.value = DoorMode::MODE_CLOSED; _door_state_pub = _ros_node->create_publisher( - "/door_states", rclcpp::SystemDefaultsQoS()); + "/door_states", rclcpp::SystemDefaultsQoS().keep_last(10)); _door_request_sub = _ros_node->create_subscription( - "/door_requests", rclcpp::SystemDefaultsQoS(), + "/door_requests", rclcpp::SystemDefaultsQoS().keep_last(10), [&](DoorRequest::UniquePtr msg) { if (msg->door_name == _state.door_name) diff --git a/rmf_building_sim_common/src/lift_common.cpp b/rmf_building_sim_common/src/lift_common.cpp index fa879cc..a0d56c2 100644 --- a/rmf_building_sim_common/src/lift_common.cpp +++ b/rmf_building_sim_common/src/lift_common.cpp @@ -204,13 +204,13 @@ LiftCommon::LiftCommon(rclcpp::Node::SharedPtr node, // initialize pub & sub _lift_state_pub = _ros_node->create_publisher( - "/lift_states", rclcpp::SystemDefaultsQoS()); + "/lift_states", rclcpp::SystemDefaultsQoS().keep_last(10)); _door_request_pub = _ros_node->create_publisher( - "/adapter_door_requests", rclcpp::SystemDefaultsQoS()); + "/adapter_door_requests", rclcpp::SystemDefaultsQoS().keep_last(10)); _lift_request_sub = _ros_node->create_subscription( - "/lift_requests", rclcpp::SystemDefaultsQoS(), + "/lift_requests", rclcpp::SystemDefaultsQoS().keep_last(10), [&](LiftRequest::UniquePtr msg) { if (msg->lift_name != _lift_name) @@ -247,7 +247,7 @@ LiftCommon::LiftCommon(rclcpp::Node::SharedPtr node, }); _door_state_sub = _ros_node->create_subscription( - "/door_states", rclcpp::SystemDefaultsQoS(), + "/door_states", rclcpp::SystemDefaultsQoS().keep_last(10), [&](DoorState::SharedPtr msg) { std::string name = msg->door_name; diff --git a/rmf_building_sim_gz_classic_plugins/src/toggle_floors.cpp b/rmf_building_sim_gz_classic_plugins/src/toggle_floors.cpp index 8e18e6e..d2260a6 100644 --- a/rmf_building_sim_gz_classic_plugins/src/toggle_floors.cpp +++ b/rmf_building_sim_gz_classic_plugins/src/toggle_floors.cpp @@ -51,7 +51,7 @@ class ToggleFloors : public gazebo::GUIPlugin // toggle non-static robots fleet_state_sub = ros_node->create_subscription( - "/fleet_states", rclcpp::SystemDefaultsQoS(), + "/fleet_states", rclcpp::SystemDefaultsQoS().keep_last(10), [&](FleetState::UniquePtr msg) { bool visible; diff --git a/rmf_robot_sim_common/src/dispenser_common.cpp b/rmf_robot_sim_common/src/dispenser_common.cpp index 6b3610c..fa3b8aa 100644 --- a/rmf_robot_sim_common/src/dispenser_common.cpp +++ b/rmf_robot_sim_common/src/dispenser_common.cpp @@ -187,7 +187,7 @@ void TeleportDispenserCommon::init_ros_node(const rclcpp::Node::SharedPtr node) _fleet_state_sub = ros_node->create_subscription( "/fleet_states", - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), std::bind(&TeleportDispenserCommon::fleet_state_cb, this, std::placeholders::_1)); @@ -196,7 +196,7 @@ void TeleportDispenserCommon::init_ros_node(const rclcpp::Node::SharedPtr node) _request_sub = ros_node->create_subscription( "/dispenser_requests", - rclcpp::SystemDefaultsQoS().reliable(), + rclcpp::SystemDefaultsQoS().keep_last(10).reliable(), std::bind(&TeleportDispenserCommon::dispenser_request_cb, this, std::placeholders::_1)); diff --git a/rmf_robot_sim_common/src/ingestor_common.cpp b/rmf_robot_sim_common/src/ingestor_common.cpp index a894236..600ec1e 100644 --- a/rmf_robot_sim_common/src/ingestor_common.cpp +++ b/rmf_robot_sim_common/src/ingestor_common.cpp @@ -182,7 +182,7 @@ void TeleportIngestorCommon::init_ros_node(const rclcpp::Node::SharedPtr node) _fleet_state_sub = ros_node->create_subscription( "/fleet_states", - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), std::bind(&TeleportIngestorCommon::fleet_state_cb, this, std::placeholders::_1)); @@ -191,7 +191,7 @@ void TeleportIngestorCommon::init_ros_node(const rclcpp::Node::SharedPtr node) _request_sub = ros_node->create_subscription( "/ingestor_requests", - rclcpp::SystemDefaultsQoS().reliable(), + rclcpp::SystemDefaultsQoS().keep_last(10).reliable(), std::bind(&TeleportIngestorCommon::ingestor_request_cb, this, std::placeholders::_1));