Propagate read/write rate to the HardwareInfo properly

hardware_interface/include/hardware_interface/component_parser.hpp

@@ -27,11 +27,13 @@ namespace hardware_interface
 /// Search XML snippet from URDF for information about a control component.
 /**
  * \param[in] urdf string with robot's URDF
+ * \param[in] cm_update_rate The update rate of the controller manager
  * \return vector filled with information about robot's control resources
  * \throws std::runtime_error if a robot attribute or tag is not found
  */
 HARDWARE_INTERFACE_PUBLIC
-std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string & urdf);
+std::vector<HardwareInfo> parse_control_resources_from_urdf(
+  const std::string & urdf, const unsigned int cm_update_rate = 100);
 
 /**
  * \param[in] component_info information about a component (gpio, joint, sensor)

hardware_interface/src/component_parser.cpp

@@ -231,12 +231,13 @@ std::string parse_data_type_attribute(const tinyxml2::XMLElement * elem)
  * \param[in] elem XMLElement that has the rw_rate attribute.
  * \return unsigned int specifying the read/write rate.
  */
-unsigned int parse_rw_rate_attribute(const tinyxml2::XMLElement * elem)
+unsigned int parse_rw_rate_attribute(
+  const tinyxml2::XMLElement * elem, const unsigned int cm_update_rate)
 {
   const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kReadWriteRateAttribute);
   try
   {
-    const auto rw_rate = attr ? std::stoi(attr->Value()) : 0;
+    const int rw_rate = attr ? std::stoi(attr->Value()) : static_cast<int>(cm_update_rate);
     if (rw_rate < 0)
     {
       throw std::runtime_error(
@@ -603,16 +604,18 @@ void auto_fill_transmission_interfaces(HardwareInfo & hardware)
 /**
  * \param[in] ros2_control_it pointer to ros2_control element
  * with information about resource.
+ * \param[in] cm_update_rate The update rate of the controller manager
  * \return HardwareInfo filled with information about the robot
  * \throws std::runtime_error if a attributes or tag are not found
  */
 HardwareInfo parse_resource_from_xml(
-  const tinyxml2::XMLElement * ros2_control_it, const std::string & urdf)
+  const tinyxml2::XMLElement * ros2_control_it, const std::string & urdf,
+  const unsigned int cm_update_rate)
 {
   HardwareInfo hardware;
   hardware.name = get_attribute_value(ros2_control_it, kNameAttribute, kROS2ControlTag);
   hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag);
-  hardware.rw_rate = parse_rw_rate_attribute(ros2_control_it);
+  hardware.rw_rate = parse_rw_rate_attribute(ros2_control_it, cm_update_rate);
   hardware.is_async = parse_is_async_attribute(ros2_control_it);
 
   // Parse everything under ros2_control tag
@@ -833,7 +836,8 @@ void update_interface_limits(
 
 }  // namespace detail
 
-std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string & urdf)
+std::vector<HardwareInfo> parse_control_resources_from_urdf(
+  const std::string & urdf, const unsigned int cm_update_rate)
 {
   // Check if everything OK with URDF string
   if (urdf.empty())
@@ -880,7 +884,7 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
   std::vector<HardwareInfo> hardware_info;
   while (ros2_control_it)
   {
-    hardware_info.push_back(detail::parse_resource_from_xml(ros2_control_it, urdf));
+    hardware_info.push_back(detail::parse_resource_from_xml(ros2_control_it, urdf, cm_update_rate));
     ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag);
   }
 

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,8 @@ 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);
+  const auto hardware_info =
+    hardware_interface::parse_control_resources_from_urdf(urdf, resource_storage_->cm_update_rate_);
 
   const std::string system_type = "system";
   const std::string sensor_type = "sensor";

hardware_interface/test/test_component_parser.cpp

@@ -1418,13 +1418,14 @@ TEST_F(TestComponentParser, gripper_mimic_true_valid_config)
     std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_true_no_command_if) +
     std::string(ros2_control_test_assets::urdf_tail);
   std::vector<hardware_interface::HardwareInfo> hw_info;
-  ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test));
+  ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test, 200));
   ASSERT_THAT(hw_info, SizeIs(1));
   ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1));
   EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0);
   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);
+  EXPECT_EQ(hw_info[0].rw_rate, 200);
 }
 
 TEST_F(TestComponentParser, gripper_no_mimic_valid_config)
@@ -1441,6 +1442,7 @@ 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);
+  EXPECT_EQ(hw_info[0].rw_rate, 100);
 }
 
 TEST_F(TestComponentParser, negative_rw_rates_throws_error)