From a3866686ac89831271398060a1a9ffccdff0daa2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 10 Dec 2024 18:14:15 +0100 Subject: [PATCH] Propagate read/write rate to the HardwareInfo properly --- .../hardware_interface/component_parser.hpp | 4 +++- hardware_interface/src/component_parser.cpp | 16 ++++++++++------ hardware_interface/src/resource_manager.cpp | 8 +++----- .../test/test_component_parser.cpp | 4 +++- 4 files changed, 19 insertions(+), 13 deletions(-) diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index fc195d0a65..b11f944603 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/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 parse_control_resources_from_urdf(const std::string & urdf); +std::vector 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) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 146a5aef05..909ea0b0b0 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/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(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 parse_control_resources_from_urdf(const std::string & urdf) +std::vector 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 parse_control_resources_from_urdf(const std::string & std::vector 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); } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e77917af9d..edc2a82327 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/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"; diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 8c535f04a9..fe76504921 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/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 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)