Propagate read/write rate to the HardwareInfo properly

hardware_interface/src/resource_manager.cpp

@@ -142,10 +142,7 @@ class ResourceStorage
         component_info.name = hardware_info.name;
         component_info.type = hardware_info.type;
         component_info.group = hardware_info.group;
-        component_info.rw_rate =
-          (hardware_info.rw_rate == 0 || hardware_info.rw_rate > cm_update_rate_)
-            ? cm_update_rate_
-            : hardware_info.rw_rate;
+        component_info.rw_rate = hardware_info.rw_rate;
         component_info.plugin_name = hardware_info.hardware_plugin_name;
         component_info.is_async = hardware_info.is_async;
 
@@ -1127,7 +1124,13 @@ bool ResourceManager::load_and_initialize_components(
 
   resource_storage_->cm_update_rate_ = update_rate;
 
-  const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
+  auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
+  // Set the update rate for all hardware components
+  for (auto & hw : hardware_info)
+  {
+    hw.rw_rate =
+      (hw.rw_rate == 0 || hw.rw_rate > update_rate) ? update_rate : hw.rw_rate;
+  }
 
   const std::string system_type = "system";
   const std::string sensor_type = "sensor";

hardware_interface/test/mock_components/test_generic_system.cpp

@@ -699,9 +699,11 @@ class TestableResourceManager : public hardware_interface::ResourceManager
   }
 
   explicit TestableResourceManager(
-    rclcpp::Node & node, const std::string & urdf, bool activate_all = false)
+    rclcpp::Node & node, const std::string & urdf, bool activate_all = false,
+    unsigned int cm_update_rate = 100)
   : hardware_interface::ResourceManager(
-      urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, 100)
+      urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all,
+      cm_update_rate)
   {
   }
 };
@@ -842,14 +844,17 @@ void generic_system_functional_test(
   EXPECT_EQ(
     status_map[component_name].state.label(),
     hardware_interface::lifecycle_state_names::UNCONFIGURED);
+  EXPECT_EQ(status_map[component_name].rw_rate, 100u);
   configure_components(rm, {component_name});
   status_map = rm.get_components_status();
   EXPECT_EQ(
     status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE);
+  EXPECT_EQ(status_map[component_name].rw_rate, 100u);
   activate_components(rm, {component_name});
   status_map = rm.get_components_status();
   EXPECT_EQ(
     status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE);
+  EXPECT_EQ(status_map[component_name].rw_rate, 100u);
 
   // Check initial values
   hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position");
@@ -935,7 +940,7 @@ void generic_system_error_group_test(
   const std::string & urdf, const std::string component_prefix, bool validate_same_group)
 {
   rclcpp::Node node("test_generic_system");
-  TestableResourceManager rm(node, urdf);
+  TestableResourceManager rm(node, urdf, false, 200u);
   const std::string component1 = component_prefix + "1";
   const std::string component2 = component_prefix + "2";
   // check is hardware is configured
@@ -944,14 +949,17 @@ void generic_system_error_group_test(
   {
     EXPECT_EQ(
       status_map[component].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED);
+    EXPECT_EQ(status_map[component].rw_rate, 200u);
     configure_components(rm, {component});
     status_map = rm.get_components_status();
     EXPECT_EQ(
       status_map[component].state.label(), hardware_interface::lifecycle_state_names::INACTIVE);
+    EXPECT_EQ(status_map[component].rw_rate, 200u);
     activate_components(rm, {component});
     status_map = rm.get_components_status();
     EXPECT_EQ(
       status_map[component].state.label(), hardware_interface::lifecycle_state_names::ACTIVE);
+    EXPECT_EQ(status_map[component].rw_rate, 200u);
   }
 
   // Check initial values

hardware_interface/test/test_component_parser.cpp

@@ -1361,6 +1361,8 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty)
 
   EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "");
   EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2");
+  // when not set, rw_rate should be 0
+  EXPECT_EQ(hardware_info.rw_rate, 0u);
 
   // Verify limits parsed from the URDF
   ASSERT_THAT(hardware_info.limits, SizeIs(1));
@@ -1425,6 +1427,8 @@ TEST_F(TestComponentParser, gripper_mimic_true_valid_config)
   EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0);
   EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0);
   EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1);
+  // when not set, rw_rate should be 0
+  EXPECT_EQ(hw_info[0].rw_rate, 0u);
 }
 
 TEST_F(TestComponentParser, gripper_no_mimic_valid_config)
@@ -1441,6 +1445,8 @@ TEST_F(TestComponentParser, gripper_no_mimic_valid_config)
   EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0);
   EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0);
   EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1);
+  // when not set, rw_rate should be 0
+  EXPECT_EQ(hw_info[0].rw_rate, 0u);
 }
 
 TEST_F(TestComponentParser, negative_rw_rates_throws_error)
@@ -1473,6 +1479,38 @@ TEST_F(TestComponentParser, invalid_rw_rates_out_of_range)
   ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
 }
 
+TEST_F(TestComponentParser, valid_rw_rate)
+{
+  std::vector<hardware_interface::HardwareInfo> hw_info;
+  ASSERT_NO_THROW(
+    hw_info = parse_control_resources_from_urdf(
+      ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate));
+  ASSERT_THAT(hw_info, SizeIs(3));
+  EXPECT_EQ(hw_info[0].name, "TestActuatorHardware");
+  EXPECT_EQ(hw_info[0].type, "actuator");
+  EXPECT_EQ(hw_info[0].hardware_plugin_name, "test_actuator");
+  ASSERT_THAT(hw_info[0].joints, SizeIs(1));
+  EXPECT_EQ(hw_info[0].joints[0].name, "joint1");
+  EXPECT_EQ(hw_info[0].rw_rate, 50u);
+
+  EXPECT_EQ(hw_info[1].name, "TestSensorHardware");
+  EXPECT_EQ(hw_info[1].type, "sensor");
+  EXPECT_EQ(hw_info[1].hardware_plugin_name, "test_sensor");
+  ASSERT_THAT(hw_info[1].sensors, SizeIs(1));
+  EXPECT_EQ(hw_info[1].sensors[0].name, "sensor1");
+  EXPECT_EQ(hw_info[1].rw_rate, 20u);
+
+  EXPECT_EQ(hw_info[2].name, "TestSystemHardware");
+  EXPECT_EQ(hw_info[2].type, "system");
+  EXPECT_EQ(hw_info[2].hardware_plugin_name, "test_system");
+  ASSERT_THAT(hw_info[2].joints, SizeIs(2));
+  EXPECT_EQ(hw_info[2].joints[0].name, "joint2");
+  EXPECT_EQ(hw_info[2].joints[1].name, "joint3");
+  ASSERT_THAT(hw_info[2].gpios, SizeIs(1));
+  EXPECT_EQ(hw_info[2].gpios[0].name, "configuration");
+  EXPECT_EQ(hw_info[2].rw_rate, 25u);
+}
+
 TEST_F(TestComponentParser, gripper_mimic_with_unknown_joint_throws_error)
 {
   const auto urdf_to_test =

hardware_interface_testing/test/test_components/test_actuator.cpp

@@ -15,6 +15,7 @@
 #include <vector>
 
 #include "hardware_interface/actuator_interface.hpp"
+#include "rclcpp/logging.hpp"
 #include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
 
 using hardware_interface::ActuatorInterface;
@@ -35,6 +36,14 @@ class TestActuator : public ActuatorInterface
     {
       return CallbackReturn::ERROR;
     }
+    if (get_hardware_info().rw_rate == 0u)
+    {
+      RCLCPP_WARN(
+        get_logger(),
+        "Actuator hardware component '%s' from plugin '%s' failed to initialize as rw_rate is 0.",
+        get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str());
+      return CallbackReturn::ERROR;
+    }
 
     /*
      * a hardware can optional prove for incorrect info here.

hardware_interface_testing/test/test_components/test_sensor.cpp

@@ -15,6 +15,7 @@
 #include <vector>
 
 #include "hardware_interface/sensor_interface.hpp"
+#include "rclcpp/logging.hpp"
 
 using hardware_interface::return_type;
 using hardware_interface::SensorInterface;
@@ -33,6 +34,14 @@ class TestSensor : public SensorInterface
     {
       return CallbackReturn::ERROR;
     }
+    if (get_hardware_info().rw_rate == 0u)
+    {
+      RCLCPP_WARN(
+        get_logger(),
+        "Sensor hardware component '%s' from plugin '%s' failed to initialize as rw_rate is 0.",
+        get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str());
+      return CallbackReturn::ERROR;
+    }
     return CallbackReturn::SUCCESS;
   }
 

hardware_interface_testing/test/test_components/test_system.cpp

@@ -17,6 +17,7 @@
 
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/logging.hpp"
 #include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
 
 using hardware_interface::CommandInterface;
@@ -39,6 +40,14 @@ class TestSystem : public SystemInterface
       return CallbackReturn::ERROR;
     }
 
+    if (get_hardware_info().rw_rate == 0u)
+    {
+      RCLCPP_WARN(
+        get_logger(),
+        "System hardware component '%s' from plugin '%s' failed to initialize as rw_rate is 0.",
+        get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str());
+      return CallbackReturn::ERROR;
+    }
     return CallbackReturn::SUCCESS;
   }