From 917ca1b2503049c5dabfa890d5e72f929dcb336f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 10 Dec 2024 19:10:13 +0100 Subject: [PATCH 1/5] Propagate read/write rate to the HardwareInfo properly --- hardware_interface/src/resource_manager.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e77917af9d..db88fc8506 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,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_rate : hardware_info) + { + hw_rate.rw_rate = + (hw_rate.rw_rate == 0 || hw_rate.rw_rate > update_rate) ? update_rate : hw_rate.rw_rate; + } const std::string system_type = "system"; const std::string sensor_type = "sensor"; From 5ae663ef85572393662daf65df70b0c349da03e1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 10 Dec 2024 23:23:06 +0100 Subject: [PATCH 2/5] Add tests checking the HardwareComponentState --- .../test/mock_components/test_generic_system.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 854b35c8d3..446634d12f 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/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 From 52eae6158cefaa5488f9bc3ef2eac3aa2e5d6f20 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 10 Dec 2024 23:23:30 +0100 Subject: [PATCH 3/5] Add condition in all the hardware component test plugins --- .../test/test_components/test_actuator.cpp | 9 +++++++++ .../test/test_components/test_sensor.cpp | 9 +++++++++ .../test/test_components/test_system.cpp | 9 +++++++++ 3 files changed, 27 insertions(+) diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 86debc1f4d..630cb9f27c 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -15,6 +15,7 @@ #include #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. diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 16692f55d9..40f27530c0 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -15,6 +15,7 @@ #include #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; } diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 795787eb9e..395935e314 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/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; } From 9ba0518a3aaea270128550f6bd34c029c3371429 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 11 Dec 2024 08:39:04 +0100 Subject: [PATCH 4/5] Add more tests for valid rw_rate --- .../test/test_component_parser.cpp | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 8c535f04a9..506092173d 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/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 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 = From 8bf3d7f53ad8c30ce813486d188296794dc327e5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 13 Dec 2024 11:40:46 +0100 Subject: [PATCH 5/5] Update hardware_interface/src/resource_manager.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- hardware_interface/src/resource_manager.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index db88fc8506..6ed5f19bed 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1126,10 +1126,10 @@ bool ResourceManager::load_and_initialize_components( auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); // Set the update rate for all hardware components - for (auto & hw_rate : hardware_info) + for (auto & hw : hardware_info) { - hw_rate.rw_rate = - (hw_rate.rw_rate == 0 || hw_rate.rw_rate > update_rate) ? update_rate : hw_rate.rw_rate; + hw.rw_rate = + (hw.rw_rate == 0 || hw.rw_rate > update_rate) ? update_rate : hw.rw_rate; } const std::string system_type = "system";