Back port to humble

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<typename SdfPtrT, typename SdfElementPtrT>
 bool get_element_required(

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<DoorState>(
-    "/door_states", rclcpp::SystemDefaultsQoS());
+    "/door_states", rclcpp::SystemDefaultsQoS().keep_last(10));
 
   _door_request_sub = _ros_node->create_subscription<DoorRequest>(
-    "/door_requests", rclcpp::SystemDefaultsQoS(),
+    "/door_requests", rclcpp::SystemDefaultsQoS().keep_last(10),
     [&](DoorRequest::UniquePtr msg)
     {
       if (msg->door_name == _state.door_name)

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<LiftState>(
-    "/lift_states", rclcpp::SystemDefaultsQoS());
+    "/lift_states", rclcpp::SystemDefaultsQoS().keep_last(10));
 
   _door_request_pub = _ros_node->create_publisher<DoorRequest>(
-    "/adapter_door_requests", rclcpp::SystemDefaultsQoS());
+    "/adapter_door_requests", rclcpp::SystemDefaultsQoS().keep_last(10));
 
   _lift_request_sub = _ros_node->create_subscription<LiftRequest>(
-    "/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<DoorState>(
-    "/door_states", rclcpp::SystemDefaultsQoS(),
+    "/door_states", rclcpp::SystemDefaultsQoS().keep_last(10),
     [&](DoorState::SharedPtr msg)
     {
       std::string name = msg->door_name;

rmf_building_sim_common/src/utils.cpp

@@ -1,3 +1,4 @@
+#include <cctype>
 #include <cmath>
 #include <algorithm>
 
@@ -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

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;
 

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;
 

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<FleetState>(
-      "/fleet_states", rclcpp::SystemDefaultsQoS(),
+      "/fleet_states", rclcpp::SystemDefaultsQoS().keep_last(10),
       [&](FleetState::UniquePtr msg)
       {
         bool visible;

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<rclcpp::Node>(plugin_name);
 
     RCLCPP_INFO(_ros_node->get_logger(),

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<rclcpp::Node>(plugin_name);
 
     RCLCPP_INFO(_ros_node->get_logger(),

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<typename SdfPtrT, typename SdfElementPtrT>
 bool get_element_required(

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<FleetState>(
     "/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<DispenserRequest>(
     "/dispenser_requests",
-    rclcpp::SystemDefaultsQoS().reliable(),
+    rclcpp::SystemDefaultsQoS().keep_last(10).reliable(),
     std::bind(&TeleportDispenserCommon::dispenser_request_cb, this,
     std::placeholders::_1));
 

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<FleetState>(
     "/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<IngestorRequest>(
     "/ingestor_requests",
-    rclcpp::SystemDefaultsQoS().reliable(),
+    rclcpp::SystemDefaultsQoS().keep_last(10).reliable(),
     std::bind(&TeleportIngestorCommon::ingestor_request_cb, this,
     std::placeholders::_1));
 

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

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);
 

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<rclcpp::Node>(plugin_name);
   // TODO Check if executor is getting callbacks
   //executor = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();