From b9416a13387add5abb486a4d257a5cafa5240de9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 13 Jun 2024 07:23:03 +0200 Subject: [PATCH 01/25] added the initial changes for read_rate and write_rate --- .../hardware_component_info.hpp | 13 +++++++++++++ hardware_interface/src/resource_manager.cpp | 16 ++++++++++++++-- 2 files changed, 27 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index 092ed21000..75f1126538 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -22,6 +22,7 @@ #include #include +#include #include "rclcpp_lifecycle/state.hpp" namespace hardware_interface @@ -47,6 +48,18 @@ struct HardwareComponentInfo /// Component is async bool is_async; + //// read rate + unsigned int read_rate; + + //// write rate + unsigned int write_rate; + + //// Next read cycle time + std::shared_ptr next_read_cycle_time; + + //// Next write cycle time + std::shared_ptr next_write_cycle_time; + /// Component current state. rclcpp_lifecycle::State state; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e5445491c8..44ed871786 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -142,6 +142,8 @@ class ResourceStorage component_info.name = hardware_info.name; component_info.type = hardware_info.type; component_info.group = hardware_info.group; + component_info.read_rate = cm_update_rate_; + component_info.write_rate = cm_update_rate_; component_info.plugin_name = hardware_info.hardware_plugin_name; component_info.is_async = hardware_info.is_async; @@ -1839,7 +1841,12 @@ HardwareReadWriteStatus ResourceManager::read( auto ret_val = return_type::OK; try { - ret_val = component.read(time, period); + if ( + resource_storage_->hardware_info_map_[component.get_name()].read_rate == + resource_storage_->cm_update_rate_) + { + ret_val = component.read(time, period); + } const auto component_group = component.get_group_name(); ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); @@ -1900,7 +1907,12 @@ HardwareReadWriteStatus ResourceManager::write( auto ret_val = return_type::OK; try { - ret_val = component.write(time, period); + if ( + resource_storage_->hardware_info_map_[component.get_name()].write_rate == + resource_storage_->cm_update_rate_) + { + ret_val = component.write(time, period); + } const auto component_group = component.get_group_name(); ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); From bbfb1e55efd85bbff6073fdc1d70108cb74231fa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 04:33:45 +0200 Subject: [PATCH 02/25] add last read and write cycle time to control the rate --- .../include/hardware_interface/actuator.hpp | 10 +++++++++ .../hardware_component_info.hpp | 6 ----- .../include/hardware_interface/sensor.hpp | 5 +++++ .../include/hardware_interface/system.hpp | 10 +++++++++ hardware_interface/src/actuator.cpp | 8 +++++++ hardware_interface/src/resource_manager.cpp | 22 +++++++++++++++++++ hardware_interface/src/sensor.cpp | 4 ++++ hardware_interface/src/system.cpp | 8 +++++++ 8 files changed, 67 insertions(+), 6 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 3b16a65261..89fe331143 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -95,6 +95,12 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_lifecycle_state() const; + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_read_time() const; + + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_write_time() const; + HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); @@ -104,6 +110,10 @@ class Actuator final private: std::unique_ptr impl_; mutable std::recursive_mutex actuators_mutex_; + // Last read cycle time + rclcpp::Time last_read_cycle_time_; + // Last write cycle time + rclcpp::Time last_write_cycle_time_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index 75f1126538..b897f12aaf 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -54,12 +54,6 @@ struct HardwareComponentInfo //// write rate unsigned int write_rate; - //// Next read cycle time - std::shared_ptr next_read_cycle_time; - - //// Next write cycle time - std::shared_ptr next_write_cycle_time; - /// Component current state. rclcpp_lifecycle::State state; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index ca570b78aa..01fb9a51db 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -82,6 +82,9 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_lifecycle_state() const; + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_read_time() const; + HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); @@ -91,6 +94,8 @@ class Sensor final private: std::unique_ptr impl_; mutable std::recursive_mutex sensors_mutex_; + // Last read cycle time + rclcpp::Time last_read_cycle_time_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 09adaa7190..47a84fb2dc 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -95,6 +95,12 @@ class System final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_lifecycle_state() const; + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_read_time() const; + + HARDWARE_INTERFACE_PUBLIC + const rclcpp::Time & get_last_write_time() const; + HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); @@ -104,6 +110,10 @@ class System final private: std::unique_ptr impl_; mutable std::recursive_mutex system_mutex_; + // Last read cycle time + rclcpp::Time last_read_cycle_time_; + // Last write cycle time + rclcpp::Time last_write_cycle_time_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index b0ed1f7c80..e46a1894f8 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -282,6 +282,10 @@ const rclcpp_lifecycle::State & Actuator::get_lifecycle_state() const return impl_->get_lifecycle_state(); } +const rclcpp::Time & Actuator::get_last_read_time() const { return last_read_cycle_time_; } + +const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_cycle_time_; } + return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { std::unique_lock lock(actuators_mutex_, std::try_to_lock); @@ -294,6 +298,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { + last_read_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); return return_type::OK; } return_type result = return_type::ERROR; @@ -306,6 +311,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p { error(); } + last_read_cycle_time_ = time; } return result; } @@ -322,6 +328,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { + last_write_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); return return_type::OK; } return_type result = return_type::ERROR; @@ -334,6 +341,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & { error(); } + last_write_cycle_time_ = time; } return result; } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 44ed871786..cdda03c3ea 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1847,6 +1847,17 @@ HardwareReadWriteStatus ResourceManager::read( { ret_val = component.read(time, period); } + else + { + const double hw_read_period = + 1.0 / resource_storage_->hardware_info_map_[component.get_name()].read_rate; + if ( + component.get_last_read_time().seconds() == 0 || + (time - component.get_last_read_time()).seconds() >= hw_read_period) + { + ret_val = component.read(time, period); + } + } const auto component_group = component.get_group_name(); ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); @@ -1913,6 +1924,17 @@ HardwareReadWriteStatus ResourceManager::write( { ret_val = component.write(time, period); } + else + { + const double hw_write_period = + 1.0 / resource_storage_->hardware_info_map_[component.get_name()].write_rate; + if ( + component.get_last_write_time().seconds() == 0 || + (time - component.get_last_write_time()).seconds() >= hw_write_period) + { + ret_val = component.write(time, period); + } + } const auto component_group = component.get_group_name(); ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 5da01368c9..6314ccdf3d 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -240,6 +240,8 @@ const rclcpp_lifecycle::State & Sensor::get_lifecycle_state() const return impl_->get_lifecycle_state(); } +const rclcpp::Time & Sensor::get_last_read_time() const { return last_read_cycle_time_; } + return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { std::unique_lock lock(sensors_mutex_, std::try_to_lock); @@ -252,6 +254,7 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { + last_read_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); return return_type::OK; } return_type result = return_type::ERROR; @@ -264,6 +267,7 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per { error(); } + last_read_cycle_time_ = time; } return result; } diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index abae924dfc..b8c19d3701 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -280,6 +280,10 @@ const rclcpp_lifecycle::State & System::get_lifecycle_state() const return impl_->get_lifecycle_state(); } +const rclcpp::Time & System::get_last_read_time() const { return last_read_cycle_time_; } + +const rclcpp::Time & System::get_last_write_time() const { return last_write_cycle_time_; } + return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { std::unique_lock lock(system_mutex_, std::try_to_lock); @@ -292,6 +296,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { + last_read_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); return return_type::OK; } return_type result = return_type::ERROR; @@ -304,6 +309,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per { error(); } + last_read_cycle_time_ = time; } return result; } @@ -320,6 +326,7 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { + last_write_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); return return_type::OK; } return_type result = return_type::ERROR; @@ -332,6 +339,7 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe { error(); } + last_write_cycle_time_ = time; } return result; } From ef24cdd3e1422766fdb53a3b5e74830bbc1a3619 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 09:15:32 +0200 Subject: [PATCH 03/25] add read write rate parsing from the ros2_control tag in component parser --- .../hardware_interface/hardware_info.hpp | 2 ++ hardware_interface/src/component_parser.cpp | 23 +++++++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index eea8b6ca8a..f62329ee62 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -174,6 +174,8 @@ struct HardwareInfo std::string type; /// Hardware group to which the hardware belongs. std::string group; + /// Component's read and write rates in Hz. + unsigned int rw_rate; /// Component is async bool is_async; /// Name of the pluginlib plugin of the hardware that will be loaded. diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 0f186531e9..1b29e7bdb4 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -58,6 +58,7 @@ constexpr const auto kTypeAttribute = "type"; constexpr const auto kRoleAttribute = "role"; constexpr const auto kReductionAttribute = "mechanical_reduction"; constexpr const auto kOffsetAttribute = "offset"; +constexpr const auto kReadWriteRateAttribute = "rw_rate"; constexpr const auto kIsAsyncAttribute = "is_async"; } // namespace @@ -222,6 +223,27 @@ std::string parse_data_type_attribute(const tinyxml2::XMLElement * elem) return data_type; } +/// Parse rw_rate attribute +/** + * Parses an XMLElement and returns the value of the rw_rate attribute. + * Defaults to 0 if not specified. + * + * \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) +{ + const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kReadWriteRateAttribute); + const auto rw_rate = attr ? std::stoi(attr->Value()) : 0; + if (rw_rate < 0) + { + throw std::runtime_error( + "Could not parse rw_rate tag in \"" + std::string(elem->Name()) + "\"." + "Got \"" + + std::to_string(rw_rate) + "\", but expected a positive integer."); + } + return static_cast(rw_rate); +} + /// Parse is_async attribute /** * Parses an XMLElement and returns the value of the is_async attribute. @@ -575,6 +597,7 @@ HardwareInfo parse_resource_from_xml( 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.is_async = parse_is_async_attribute(ros2_control_it); // Parse everything under ros2_control tag From 50e325739b350781af5c13660be3fd3f8c03804f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 09:16:27 +0200 Subject: [PATCH 04/25] fix the component info using the rw_rate in the HardwareInfo --- hardware_interface/src/resource_manager.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index cdda03c3ea..b340c48c6b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -142,8 +142,14 @@ class ResourceStorage component_info.name = hardware_info.name; component_info.type = hardware_info.type; component_info.group = hardware_info.group; - component_info.read_rate = cm_update_rate_; - component_info.write_rate = cm_update_rate_; + component_info.read_rate = + (hardware_info.rw_rate == 0 || hardware_info.rw_rate > cm_update_rate_) + ? cm_update_rate_ + : hardware_info.rw_rate; + component_info.write_rate = + (hardware_info.rw_rate == 0 || hardware_info.rw_rate > cm_update_rate_) + ? cm_update_rate_ + : hardware_info.rw_rate; component_info.plugin_name = hardware_info.hardware_plugin_name; component_info.is_async = hardware_info.is_async; From e6baee7a4f40660923ef09bdf331115933cfb8c3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 09:17:05 +0200 Subject: [PATCH 05/25] Add tests to the parsed rw_rate --- .../test/test_resource_manager.cpp | 11 +++- .../ros2_control_test_assets/descriptions.hpp | 52 +++++++++++++++++++ 2 files changed, 62 insertions(+), 1 deletion(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index e72a4a8214..be20c01809 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -444,7 +444,8 @@ TEST_F(ResourceManagerTest, default_prepare_perform_switch) TEST_F(ResourceManagerTest, resource_status) { - TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm( + node_, ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate); auto status_map = rm.get_components_status(); @@ -456,6 +457,14 @@ TEST_F(ResourceManagerTest, resource_status) EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].type, TEST_ACTUATOR_HARDWARE_TYPE); EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].type, TEST_SENSOR_HARDWARE_TYPE); EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].type, TEST_SYSTEM_HARDWARE_TYPE); + // read_rate + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].read_rate, 50u); + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].read_rate, 20u); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].read_rate, 100u); + // write_rate + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].write_rate, 50u); + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].write_rate, 20u); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].write_rate, 100u); // plugin_name EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].plugin_name, TEST_ACTUATOR_HARDWARE_PLUGIN_NAME); diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index e94d4e6736..170e648923 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -709,6 +709,55 @@ const auto async_hardware_resources = )"; +const auto hardware_resources_with_different_rw_rates = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + const auto uninitializable_hardware_resources = R"( @@ -1938,6 +1987,9 @@ const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); const auto minimal_async_robot_urdf = std::string(urdf_head) + std::string(async_hardware_resources) + std::string(urdf_tail); +const auto minimal_robot_urdf_with_different_hw_rw_rate = + std::string(urdf_head) + std::string(hardware_resources_with_different_rw_rates) + + std::string(urdf_tail); const auto minimal_uninitializable_robot_urdf = std::string(urdf_head) + std::string(uninitializable_hardware_resources) + std::string(urdf_tail); From 15fc2a4b12e7cd3dd0e48d760e2fe6c67b6abfe6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 10:48:07 +0200 Subject: [PATCH 06/25] change the rw_rate between system and sensor interface for tests --- hardware_interface_testing/test/test_resource_manager.cpp | 4 ++-- .../include/ros2_control_test_assets/descriptions.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index be20c01809..9492ad1537 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -460,11 +460,11 @@ TEST_F(ResourceManagerTest, resource_status) // read_rate EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].read_rate, 50u); EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].read_rate, 20u); - EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].read_rate, 100u); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].read_rate, 25u); // write_rate EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].write_rate, 50u); EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].write_rate, 20u); - EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].write_rate, 100u); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].write_rate, 25u); // plugin_name EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].plugin_name, TEST_ACTUATOR_HARDWARE_PLUGIN_NAME); diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 170e648923..b57400809e 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -732,7 +732,7 @@ const auto hardware_resources_with_different_rw_rates = - + test_system 2 From 328f59a1cbb72aed21e2047e62079f39b085f079 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:18:52 +0200 Subject: [PATCH 07/25] for hardware read and write period switch to rclcpp::Duration --- hardware_interface/src/resource_manager.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index b340c48c6b..17e400d76d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1855,13 +1855,13 @@ HardwareReadWriteStatus ResourceManager::read( } else { - const double hw_read_period = - 1.0 / resource_storage_->hardware_info_map_[component.get_name()].read_rate; + const rclcpp::Duration hw_read_period = rclcpp::Duration::from_seconds( + 1.0 / resource_storage_->hardware_info_map_[component.get_name()].read_rate); if ( component.get_last_read_time().seconds() == 0 || - (time - component.get_last_read_time()).seconds() >= hw_read_period) + (time - component.get_last_read_time()).seconds() >= hw_read_period.seconds()) { - ret_val = component.read(time, period); + ret_val = component.read(time, hw_read_period); } } const auto component_group = component.get_group_name(); @@ -1932,13 +1932,13 @@ HardwareReadWriteStatus ResourceManager::write( } else { - const double hw_write_period = - 1.0 / resource_storage_->hardware_info_map_[component.get_name()].write_rate; + const rclcpp::Duration hw_write_period = rclcpp::Duration::from_seconds( + 1.0 / resource_storage_->hardware_info_map_[component.get_name()].write_rate); if ( component.get_last_write_time().seconds() == 0 || - (time - component.get_last_write_time()).seconds() >= hw_write_period) + (time - component.get_last_write_time()).seconds() >= hw_write_period.seconds()) { - ret_val = component.write(time, period); + ret_val = component.write(time, hw_write_period); } } const auto component_group = component.get_group_name(); From eed5e3bda28209066d09cf9a2f5ec1914cf18fef Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:19:31 +0200 Subject: [PATCH 08/25] update the state of the system test component similar to actuator test component --- .../test/test_components/test_system.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index e30b74488e..795787eb9e 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -104,6 +104,12 @@ class TestSystem : public SystemInterface { return return_type::DEACTIVATE; } + // The next line is for the testing purposes. We need value to be changed to + // be sure that the feedback from hardware to controllers in the chain is + // working as it should. This makes value checks clearer and confirms there + // is no "state = command" line or some other mixture of interfaces + // somewhere in the test stack. + velocity_state_[0] = velocity_command_[0] / 2.0; return return_type::OK; } From 0a868ac7eed9bb41639d10a0fd24747e9a80e825 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:20:34 +0200 Subject: [PATCH 09/25] Add a test for a different components update rate and verifying with test components --- .../test/test_resource_manager.cpp | 127 ++++++++++++++++++ 1 file changed, 127 insertions(+) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 9492ad1537..6b4f099aae 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1742,6 +1742,133 @@ TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) } } +class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManagerTest +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + rm = std::make_shared( + node_, ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate, false); + activate_components(*rm); + + cm_update_rate_ = 100u; // The default value inside + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + actuator_rw_rate_ = status_map[TEST_ACTUATOR_HARDWARE_NAME].read_rate; + system_rw_rate_ = status_map[TEST_SYSTEM_HARDWARE_NAME].read_rate; + + claimed_itfs.push_back( + rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); + claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); + + state_itfs.push_back(rm->claim_state_interface(TEST_ACTUATOR_HARDWARE_STATE_INTERFACES[1])); + state_itfs.push_back(rm->claim_state_interface(TEST_SYSTEM_HARDWARE_STATE_INTERFACES[1])); + + check_if_interface_available(true, true); + // with default values read and write should run without any problems + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + { + claimed_itfs[0].set_value(10.0); + claimed_itfs[1].set_value(20.0); + auto [ok, failed_hardware_names] = rm->write(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + + time = time + duration; + check_if_interface_available(true, true); + } + + // check if all interfaces are available + void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces) + { + for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces); + } + }; + + using FunctionT = + std::function; + + void check_read_and_write_cycles() + { + double prev_act_state_value = state_itfs[0].get_value(); + double prev_system_state_value = state_itfs[1].get_value(); + + for (size_t i = 1; i < 100; i++) + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + if (i % (cm_update_rate_ / system_rw_rate_) == 0) + { + // The values are computations exactly within the mock systems + prev_act_state_value = claimed_itfs[0].get_value() / 2.0; + prev_system_state_value = claimed_itfs[1].get_value() / 2.0; + claimed_itfs[0].set_value(claimed_itfs[0].get_value() + 10.0); + claimed_itfs[1].set_value(claimed_itfs[1].get_value() + 20.0); + } + else if (i % (cm_update_rate_ / actuator_rw_rate_) == 0) + { + prev_act_state_value = claimed_itfs[0].get_value() / 2.0; + claimed_itfs[0].set_value(claimed_itfs[0].get_value() + 10.0); + } + ASSERT_EQ(state_itfs[0].get_value(), prev_act_state_value) << "Iteration i is " << i; + ASSERT_EQ(state_itfs[1].get_value(), prev_system_state_value) << "Iteration i is " << i; + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + time = time + duration; + } + } + +public: + std::shared_ptr rm; + unsigned int actuator_rw_rate_, system_rw_rate_, cm_update_rate_; + std::vector claimed_itfs; + std::vector state_itfs; + + rclcpp::Time time = rclcpp::Time(1657232, 0); + const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); + + // values to set to hardware to simulate failure on read and write +}; + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_activate) +{ + setup_resource_manager_and_do_initial_checks(); + + check_read_and_write_cycles(); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); From 13feface991b2b63179f930b7c8bb6cd3fd8d076 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:34:54 +0200 Subject: [PATCH 10/25] Add more checks and clean a part of the testing code --- .../test/test_resource_manager.cpp | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 6b4f099aae..cb0ad77075 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1763,6 +1763,16 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage EXPECT_EQ( status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // read_rate + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].read_rate, 50u); + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].read_rate, 20u); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].read_rate, 25u); + // write_rate + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].write_rate, 50u); + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].write_rate, 20u); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].write_rate, 25u); + actuator_rw_rate_ = status_map[TEST_ACTUATOR_HARDWARE_NAME].read_rate; system_rw_rate_ = status_map[TEST_SYSTEM_HARDWARE_NAME].read_rate; @@ -1828,19 +1838,20 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage EXPECT_TRUE(failed_hardware_names.empty()); if (i % (cm_update_rate_ / system_rw_rate_) == 0) { - // The values are computations exactly within the mock systems - prev_act_state_value = claimed_itfs[0].get_value() / 2.0; + // The values are computations exactly within the test_components prev_system_state_value = claimed_itfs[1].get_value() / 2.0; - claimed_itfs[0].set_value(claimed_itfs[0].get_value() + 10.0); claimed_itfs[1].set_value(claimed_itfs[1].get_value() + 20.0); } - else if (i % (cm_update_rate_ / actuator_rw_rate_) == 0) + if (i % (cm_update_rate_ / actuator_rw_rate_) == 0) { + // The values are computations exactly within the test_components prev_act_state_value = claimed_itfs[0].get_value() / 2.0; claimed_itfs[0].set_value(claimed_itfs[0].get_value() + 10.0); } - ASSERT_EQ(state_itfs[0].get_value(), prev_act_state_value) << "Iteration i is " << i; - ASSERT_EQ(state_itfs[1].get_value(), prev_system_state_value) << "Iteration i is " << i; + // Even though we skip some read and write iterations, the state interafces should be the same + // as previous updated one until the next cycle + ASSERT_EQ(state_itfs[0].get_value(), prev_act_state_value); + ASSERT_EQ(state_itfs[1].get_value(), prev_system_state_value); auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); EXPECT_TRUE(ok_write); EXPECT_TRUE(failed_hardware_names_write.empty()); From 081ead36177af7ea77e2f21e1281b2d60a45214a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:36:16 +0200 Subject: [PATCH 11/25] Add the test case of the deactivated case --- .../test/test_resource_manager.cpp | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index cb0ad77075..ca71a55df6 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1880,6 +1880,34 @@ TEST_F( check_read_and_write_cycles(); } +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_deactivate) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_inactive( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_inactive); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + check_read_and_write_cycles(); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); From f8b76e200ed3c4c2a78d84af4bab163f97cf34f1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:45:51 +0200 Subject: [PATCH 12/25] parameterize the tests for flexibility --- .../test/test_resource_manager.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index ca71a55df6..1de86683cb 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1826,7 +1826,7 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage using FunctionT = std::function; - void check_read_and_write_cycles() + void check_read_and_write_cycles(bool test_for_changing_values) { double prev_act_state_value = state_itfs[0].get_value(); double prev_system_state_value = state_itfs[1].get_value(); @@ -1836,13 +1836,13 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage auto [ok, failed_hardware_names] = rm->read(time, duration); EXPECT_TRUE(ok); EXPECT_TRUE(failed_hardware_names.empty()); - if (i % (cm_update_rate_ / system_rw_rate_) == 0) + if (i % (cm_update_rate_ / system_rw_rate_) == 0 && test_for_changing_values) { // The values are computations exactly within the test_components prev_system_state_value = claimed_itfs[1].get_value() / 2.0; claimed_itfs[1].set_value(claimed_itfs[1].get_value() + 20.0); } - if (i % (cm_update_rate_ / actuator_rw_rate_) == 0) + if (i % (cm_update_rate_ / actuator_rw_rate_) == 0 && test_for_changing_values) { // The values are computations exactly within the test_components prev_act_state_value = claimed_itfs[0].get_value() / 2.0; @@ -1877,7 +1877,7 @@ TEST_F( { setup_resource_manager_and_do_initial_checks(); - check_read_and_write_cycles(); + check_read_and_write_cycles(true); } TEST_F( @@ -1905,7 +1905,7 @@ TEST_F( status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - check_read_and_write_cycles(); + check_read_and_write_cycles(true); } int main(int argc, char ** argv) From c8b35509feade883af1fa32b692817c50e8b8e30 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:46:23 +0200 Subject: [PATCH 13/25] Test also for the unconfigured and finalized lifecycle states --- .../test/test_resource_manager.cpp | 56 +++++++++++++++++++ 1 file changed, 56 insertions(+) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 1de86683cb..2c62941b04 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1908,6 +1908,62 @@ TEST_F( check_read_and_write_cycles(true); } +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_unconfigured) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_unconfigured( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + hardware_interface::lifecycle_state_names::UNCONFIGURED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_unconfigured); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + check_read_and_write_cycles(false); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_finalized) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_finalized( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, + hardware_interface::lifecycle_state_names::FINALIZED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_finalized); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + check_read_and_write_cycles(false); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); From a8a19c2f8e1e6a8288737a3f2064e14be537b0c1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 15:22:55 +0200 Subject: [PATCH 14/25] Add testing for the component parser of the read and write rate --- hardware_interface/src/component_parser.cpp | 25 ++++++++--- .../test/test_component_parser.cpp | 30 +++++++++++++ .../ros2_control_test_assets/descriptions.hpp | 45 +++++++++++++++++++ 3 files changed, 95 insertions(+), 5 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 1b29e7bdb4..f32db49dcb 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -234,14 +234,29 @@ std::string parse_data_type_attribute(const tinyxml2::XMLElement * elem) unsigned int parse_rw_rate_attribute(const tinyxml2::XMLElement * elem) { const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kReadWriteRateAttribute); - const auto rw_rate = attr ? std::stoi(attr->Value()) : 0; - if (rw_rate < 0) + try + { + const auto rw_rate = attr ? std::stoi(attr->Value()) : 0; + if (rw_rate < 0) + { + throw std::runtime_error( + "Could not parse rw_rate tag in \"" + std::string(elem->Name()) + "\"." + "Got \"" + + std::to_string(rw_rate) + "\", but expected a positive integer."); + } + return static_cast(rw_rate); + } + catch (const std::invalid_argument & e) + { + throw std::runtime_error( + "Could not parse rw_rate tag in \"" + std::string(elem->Name()) + "\"." + + " Invalid value : \"" + attr->Value() + "\", expected a positive integer."); + } + catch (const std::out_of_range & e) { throw std::runtime_error( - "Could not parse rw_rate tag in \"" + std::string(elem->Name()) + "\"." + "Got \"" + - std::to_string(rw_rate) + "\", but expected a positive integer."); + "Could not parse rw_rate tag in \"" + std::string(elem->Name()) + "\"." + + " Out of range value : \"" + attr->Value() + "\", expected a positive valid integer."); } - return static_cast(rw_rate); } /// Parse is_async attribute diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 3c24b0cc2a..8c535f04a9 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1443,6 +1443,36 @@ TEST_F(TestComponentParser, gripper_no_mimic_valid_config) EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); } +TEST_F(TestComponentParser, negative_rw_rates_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::hardware_resources_with_negative_rw_rates) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, invalid_rw_rates_throws_error) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::hardware_resources_invalid_with_text_in_rw_rates) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, invalid_rw_rates_out_of_range) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::hardware_resources_invalid_out_of_range_in_rw_rates) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + TEST_F(TestComponentParser, gripper_mimic_with_unknown_joint_throws_error) { const auto urdf_to_test = diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index b57400809e..5f4512a9d1 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -758,6 +758,51 @@ const auto hardware_resources_with_different_rw_rates = )"; +const auto hardware_resources_with_negative_rw_rates = + R"( + + + test_actuator + + + + + + + + +)"; + +const auto hardware_resources_invalid_with_text_in_rw_rates = + R"( + + + test_actuator + + + + + + + + +)"; + +const auto hardware_resources_invalid_out_of_range_in_rw_rates = + R"( + + + test_actuator + + + + + + + + +)"; + const auto uninitializable_hardware_resources = R"( From 6815e42b02409aa1782552dca41b27c8ca2d8172 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 17 Jun 2024 08:11:40 +0200 Subject: [PATCH 15/25] Update documentation and the release notes --- doc/release_notes.rst | 38 +++ .../doc/different_update_rates_userdoc.rst | 241 ++++++------------ 2 files changed, 119 insertions(+), 160 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 52fcf7cc20..e6cf32626b 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -112,6 +112,44 @@ hardware_interface * Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) * Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) +* The ``ros2_control`` tag now supports parsing read/write rate ``rw_rate`` for the each hardware component parsed through the URDF (`#1570 `_) + + .. code:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + + + + + + + + + + + ros2_control_demo_hardware/MultimodalGripper + + + + 0 + 100 + + + + + + + + + * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. diff --git a/hardware_interface/doc/different_update_rates_userdoc.rst b/hardware_interface/doc/different_update_rates_userdoc.rst index 23f5e3564a..5a71587d44 100644 --- a/hardware_interface/doc/different_update_rates_userdoc.rst +++ b/hardware_interface/doc/different_update_rates_userdoc.rst @@ -5,165 +5,86 @@ Different update rates for Hardware Components =============================================================================== -In the following sections you can find some advice which will help you to implement Hardware -Components with update rates different from the main control loop. - -By counting loops -------------------------------------------------------------------------------- - -Current implementation of -`ros2_control main node `_ -has one update rate that controls the rate of the -`read(...) `_ -and `write(...) `_ -calls in `hardware_interface(s) `_. -To achieve different update rates for your hardware component you can use the following steps: - -.. _step-1: - -1. Add parameters of main control loop update rate and desired update rate for your hardware component - - .. code:: xml - - - - - - - - - my_system_interface/MySystemHardware - ${main_loop_update_rate} - ${desired_hw_update_rate} - - ... - - - - - - -.. _step-2: - -2. In you ``on_init()`` specific implementation fetch the desired parameters - - .. code:: cpp - - namespace my_system_interface - { - hardware_interface::CallbackReturn MySystemHardware::on_init( - const hardware_interface::HardwareInfo & info) - { - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { - return hardware_interface::CallbackReturn::ERROR; - } - - // declaration in *.hpp file --> unsigned int main_loop_update_rate_, desired_hw_update_rate_ = 100 ; - main_loop_update_rate_ = stoi(info_.hardware_parameters["main_loop_update_rate"]); - desired_hw_update_rate_ = stoi(info_.hardware_parameters["desired_hw_update_rate"]); - - ... - } - ... - } // my_system_interface - -3. In your ``on_activate`` specific implementation reset internal loop counter - - .. code:: cpp - - hardware_interface::CallbackReturn MySystemHardware::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) - { - // declaration in *.hpp file --> unsigned int update_loop_counter_ ; - update_loop_counter_ = 0; - ... - } - -4. In your ``read(const rclcpp::Time & time, const rclcpp::Duration & period)`` - and/or ``write(const rclcpp::Time & time, const rclcpp::Duration & period)`` - specific implementations decide if you should interfere with your hardware - - .. code:: cpp - - hardware_interface::return_type MySystemHardware::read(const rclcpp::Time & time, const rclcpp::Duration & period) - { - - bool hardware_go = main_loop_update_rate_ == 0 || - desired_hw_update_rate_ == 0 || - ((update_loop_counter_ % desired_hw_update_rate_) == 0); - - if (hardware_go){ - // hardware comms and operations - ... - } - ... - - // update counter - ++update_loop_counter_; - update_loop_counter_ %= main_loop_update_rate_; - } - -By measuring elapsed time -------------------------------------------------------------------------------- - -Another way to decide if hardware communication should be executed in the -``read(const rclcpp::Time & time, const rclcpp::Duration & period)`` and/or -``write(const rclcpp::Time & time, const rclcpp::Duration & period)`` -implementations is to measure elapsed time since last pass: - -1. In your ``on_activate`` specific implementation reset the flags that indicate - that this is the first pass of the ``read`` and ``write`` methods - - .. code:: cpp - - hardware_interface::CallbackReturn MySystemHardware::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) - { - // declaration in *.hpp file --> bool first_read_pass_, first_write_pass_ = true ; - first_read_pass_ = first_write_pass_ = true; - ... - } - -2. In your ``read(const rclcpp::Time & time, const rclcpp::Duration & period)`` - and/or ``write(const rclcpp::Time & time, const rclcpp::Duration & period)`` - specific implementations decide if you should interfere with your hardware - - .. code:: cpp - - hardware_interface::return_type MySystemHardware::read(const rclcpp::Time & time, const rclcpp::Duration & period) - { - if (first_read_pass_ || (time - last_read_time_ ) > desired_hw_update_period_) - { - first_read_pass_ = false; - // declaration in *.hpp file --> rclcpp::Time last_read_time_ ; - last_read_time_ = time; - // hardware comms and operations - ... - } - ... - } - - hardware_interface::return_type MySystemHardware::write(const rclcpp::Time & time, const rclcpp::Duration & period) - { - if (first_write_pass_ || (time - last_write_time_ ) > desired_hw_update_period_) - { - first_write_pass_ = false; - // declaration in *.hpp file --> rclcpp::Time last_write_time_ ; - last_write_time_ = time; - // hardware comms and operations - ... - } - ... - } +The ``ros2_control`` framework allows to run different hardware components at different update rates. This is useful when some of the hardware components needs to run at a different frequency than the traditional control loop frequency which is same as the one of the ``controller_manager``. It is very typical to have different components with different frequencies in a robotic system with different sensors or different components using different communication protocols. +This is useful when you have a hardware component that needs to run at a higher rate than the rest of the components. For example, you might have a sensor that needs to be read at 1000Hz, while the rest of the components can run at 500Hz, given that the control loop frequency of the ``controller_manager`` is higher than 1000Hz. The read/write rate can be defined easily by adding the parameter ``rw_rate`` to the ``ros2_control`` tag of the hardware component. + +Examples +***************************** +The following examples show how to use the different hardware interface types with different update frequencies in a ``ros2_control`` URDF. +They can be combined together within the different hardware component types (system, actuator, sensor) (:ref:`see detailed documentation `) as follows + +For a RRBot with Multimodal gripper and external sensor running at different rates: + +.. code-block:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + + + + + + + ros2_control_demo_hardware/MultimodalGripper + + + + 0 + 100 + + + + + + + + + + + ros2_control_demo_hardware/ForceTorqueSensor2DHardware + 0.43 + + + + + kuka_tcp + 100 + 100 + + + + + + + + + + +In the above example, the system hardware component that controls the joints of the RRBot is running at 500 Hz, the multimodal gripper is running at 200 Hz and the force torque sensor is running at 250 Hz. .. note:: - - The approach to get the desired update period value from the URDF and assign it to the variable - ``desired_hw_update_period_`` is the same as in the previous section (|step-1|_ and |step-2|_) but - with a different parameter name. - -.. |step-1| replace:: step 1 -.. |step-2| replace:: step 2 + In the above example, the ``rw_rate`` parameter is set to 500 Hz, 200 Hz and 250 Hz for the system, actuator and sensor hardware components respectively. This parameter is optional and if not set, the default value of 0 will be used which means that the hardware component will run at the same rate as the ``controller_manager``. However, if the specified rate is higher than the ``controller_manager`` rate, the hardware component will then run at the rate of the ``controller_manager``. From eb357e148adcb4e0a745ac2546a3c11e626b0173 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 17 Jun 2024 10:39:29 +0200 Subject: [PATCH 16/25] Parse the actual period to the read and write methods --- hardware_interface/src/resource_manager.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 17e400d76d..3ff8bdc295 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1859,9 +1859,12 @@ HardwareReadWriteStatus ResourceManager::read( 1.0 / resource_storage_->hardware_info_map_[component.get_name()].read_rate); if ( component.get_last_read_time().seconds() == 0 || - (time - component.get_last_read_time()).seconds() >= hw_read_period.seconds()) + (time - component.get_last_read_time()) >= hw_read_period) { - ret_val = component.read(time, hw_read_period); + const rclcpp::Duration actual_period = component.get_last_read_time().seconds() == 0 + ? hw_read_period + : time - component.get_last_read_time(); + ret_val = component.read(time, actual_period); } } const auto component_group = component.get_group_name(); @@ -1936,9 +1939,12 @@ HardwareReadWriteStatus ResourceManager::write( 1.0 / resource_storage_->hardware_info_map_[component.get_name()].write_rate); if ( component.get_last_write_time().seconds() == 0 || - (time - component.get_last_write_time()).seconds() >= hw_write_period.seconds()) + (time - component.get_last_write_time()) >= hw_write_period) { - ret_val = component.write(time, hw_write_period); + const rclcpp::Duration actual_period = component.get_last_write_time().seconds() == 0 + ? hw_write_period + : time - component.get_last_read_time(); + ret_val = component.write(time, actual_period); } } const auto component_group = component.get_group_name(); From 19fc9bb19c5bfd5b982a76b95b03d0007df8c4c6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 6 Aug 2024 18:03:32 +0200 Subject: [PATCH 17/25] Use the current time to trigger the components based on their update rate --- hardware_interface/src/resource_manager.cpp | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 3ff8bdc295..bdafd341a6 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1857,13 +1857,15 @@ HardwareReadWriteStatus ResourceManager::read( { const rclcpp::Duration hw_read_period = rclcpp::Duration::from_seconds( 1.0 / resource_storage_->hardware_info_map_[component.get_name()].read_rate); + const auto current_time = resource_storage_->get_clock()->now(); if ( component.get_last_read_time().seconds() == 0 || - (time - component.get_last_read_time()) >= hw_read_period) + (current_time - component.get_last_read_time()) >= hw_read_period) { - const rclcpp::Duration actual_period = component.get_last_read_time().seconds() == 0 - ? hw_read_period - : time - component.get_last_read_time(); + const rclcpp::Duration actual_period = + component.get_last_read_time().seconds() == 0 + ? hw_read_period + : current_time - component.get_last_read_time(); ret_val = component.read(time, actual_period); } } @@ -1937,13 +1939,15 @@ HardwareReadWriteStatus ResourceManager::write( { const rclcpp::Duration hw_write_period = rclcpp::Duration::from_seconds( 1.0 / resource_storage_->hardware_info_map_[component.get_name()].write_rate); + const auto current_time = resource_storage_->get_clock()->now(); if ( component.get_last_write_time().seconds() == 0 || - (time - component.get_last_write_time()) >= hw_write_period) + (current_time - component.get_last_write_time()) >= hw_write_period) { - const rclcpp::Duration actual_period = component.get_last_write_time().seconds() == 0 - ? hw_write_period - : time - component.get_last_read_time(); + const rclcpp::Duration actual_period = + component.get_last_write_time().seconds() == 0 + ? hw_write_period + : current_time - component.get_last_write_time(); ret_val = component.write(time, actual_period); } } From b97f806abb72fd162f89cc8054d4f766a2e986de Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 7 Aug 2024 09:22:30 +0200 Subject: [PATCH 18/25] Update tests to reflect the change w.r.t reality --- hardware_interface/src/resource_manager.cpp | 16 ++++++++-------- .../test/test_resource_manager.cpp | 6 ++++-- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index bdafd341a6..ab54648478 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1855,16 +1855,16 @@ HardwareReadWriteStatus ResourceManager::read( } else { - const rclcpp::Duration hw_read_period = rclcpp::Duration::from_seconds( - 1.0 / resource_storage_->hardware_info_map_[component.get_name()].read_rate); + const double read_rate = + resource_storage_->hardware_info_map_[component.get_name()].read_rate; const auto current_time = resource_storage_->get_clock()->now(); if ( component.get_last_read_time().seconds() == 0 || - (current_time - component.get_last_read_time()) >= hw_read_period) + (current_time - component.get_last_read_time()).seconds() * read_rate >= 0.99) { const rclcpp::Duration actual_period = component.get_last_read_time().seconds() == 0 - ? hw_read_period + ? rclcpp::Duration::from_seconds(1.0 / read_rate) : current_time - component.get_last_read_time(); ret_val = component.read(time, actual_period); } @@ -1937,16 +1937,16 @@ HardwareReadWriteStatus ResourceManager::write( } else { - const rclcpp::Duration hw_write_period = rclcpp::Duration::from_seconds( - 1.0 / resource_storage_->hardware_info_map_[component.get_name()].write_rate); + const double write_rate = + resource_storage_->hardware_info_map_[component.get_name()].write_rate; const auto current_time = resource_storage_->get_clock()->now(); if ( component.get_last_write_time().seconds() == 0 || - (current_time - component.get_last_write_time()) >= hw_write_period) + (current_time - component.get_last_write_time()).seconds() * write_rate >= 0.99) { const rclcpp::Duration actual_period = component.get_last_write_time().seconds() == 0 - ? hw_write_period + ? rclcpp::Duration::from_seconds(1.0 / write_rate) : current_time - component.get_last_write_time(); ret_val = component.write(time, actual_period); } diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 2c62941b04..97838776c3 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1752,6 +1752,7 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage activate_components(*rm); cm_update_rate_ = 100u; // The default value inside + time = node_.get_clock()->now(); auto status_map = rm->get_components_status(); EXPECT_EQ( @@ -1848,14 +1849,15 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage prev_act_state_value = claimed_itfs[0].get_value() / 2.0; claimed_itfs[0].set_value(claimed_itfs[0].get_value() + 10.0); } - // Even though we skip some read and write iterations, the state interafces should be the same + // Even though we skip some read and write iterations, the state interfaces should be the same // as previous updated one until the next cycle ASSERT_EQ(state_itfs[0].get_value(), prev_act_state_value); ASSERT_EQ(state_itfs[1].get_value(), prev_system_state_value); auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); EXPECT_TRUE(ok_write); EXPECT_TRUE(failed_hardware_names_write.empty()); - time = time + duration; + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); } } From cc648208ea0b77e2f6f7b597603b20a467f83068 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 7 Aug 2024 10:56:48 +0200 Subject: [PATCH 19/25] update the logic to use the clock to initialize the last read and write cycle time --- hardware_interface/src/actuator.cpp | 6 ++++-- hardware_interface/src/resource_manager.cpp | 22 ++++++--------------- hardware_interface/src/sensor.cpp | 3 ++- hardware_interface/src/system.cpp | 6 ++++-- 4 files changed, 16 insertions(+), 21 deletions(-) diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index e46a1894f8..1612d2c291 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -53,6 +53,8 @@ const rclcpp_lifecycle::State & Actuator::initialize( switch (impl_->init(actuator_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: + last_read_cycle_time_ = clock_interface->get_clock()->now(); + last_write_cycle_time_ = clock_interface->get_clock()->now(); impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); @@ -298,7 +300,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { - last_read_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); + last_read_cycle_time_ = time; return return_type::OK; } return_type result = return_type::ERROR; @@ -328,7 +330,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { - last_write_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); + last_write_cycle_time_ = time; return return_type::OK; } return_type result = return_type::ERROR; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index ab54648478..eb166fc648 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1858,15 +1858,10 @@ HardwareReadWriteStatus ResourceManager::read( const double read_rate = resource_storage_->hardware_info_map_[component.get_name()].read_rate; const auto current_time = resource_storage_->get_clock()->now(); - if ( - component.get_last_read_time().seconds() == 0 || - (current_time - component.get_last_read_time()).seconds() * read_rate >= 0.99) + const rclcpp::Duration actual_period = current_time - component.get_last_read_time(); + if (actual_period.seconds() * read_rate >= 0.99) { - const rclcpp::Duration actual_period = - component.get_last_read_time().seconds() == 0 - ? rclcpp::Duration::from_seconds(1.0 / read_rate) - : current_time - component.get_last_read_time(); - ret_val = component.read(time, actual_period); + ret_val = component.read(current_time, actual_period); } } const auto component_group = component.get_group_name(); @@ -1940,15 +1935,10 @@ HardwareReadWriteStatus ResourceManager::write( const double write_rate = resource_storage_->hardware_info_map_[component.get_name()].write_rate; const auto current_time = resource_storage_->get_clock()->now(); - if ( - component.get_last_write_time().seconds() == 0 || - (current_time - component.get_last_write_time()).seconds() * write_rate >= 0.99) + const rclcpp::Duration actual_period = current_time - component.get_last_write_time(); + if (actual_period.seconds() * write_rate >= 0.99) { - const rclcpp::Duration actual_period = - component.get_last_write_time().seconds() == 0 - ? rclcpp::Duration::from_seconds(1.0 / write_rate) - : current_time - component.get_last_write_time(); - ret_val = component.write(time, actual_period); + ret_val = component.write(current_time, actual_period); } } const auto component_group = component.get_group_name(); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 6314ccdf3d..cc15464f98 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -52,6 +52,7 @@ const rclcpp_lifecycle::State & Sensor::initialize( switch (impl_->init(sensor_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: + last_read_cycle_time_ = clock_interface->get_clock()->now(); impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); @@ -254,7 +255,7 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { - last_read_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); + last_read_cycle_time_ = time; return return_type::OK; } return_type result = return_type::ERROR; diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index b8c19d3701..8e8c889289 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -51,6 +51,8 @@ const rclcpp_lifecycle::State & System::initialize( switch (impl_->init(system_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: + last_read_cycle_time_ = clock_interface->get_clock()->now(); + last_write_cycle_time_ = clock_interface->get_clock()->now(); impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); @@ -296,7 +298,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { - last_read_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); + last_read_cycle_time_ = time; return return_type::OK; } return_type result = return_type::ERROR; @@ -326,7 +328,7 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { - last_write_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); + last_write_cycle_time_ = time; return return_type::OK; } return_type result = return_type::ERROR; From 3b46b09a57004d923886ec42856a35aeae1c52aa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 7 Aug 2024 11:49:47 +0200 Subject: [PATCH 20/25] Fix the test_component_interface for missing valid node_clock_interface --- .../test/test_component_interfaces.cpp | 53 ++++++++++++------- 1 file changed, 35 insertions(+), 18 deletions(-) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 9b90d81dfd..928adde4bc 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -33,6 +33,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/node.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" @@ -683,8 +684,9 @@ TEST(TestComponentInterfaces, dummy_actuator) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); - auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_components"); + auto state = + actuator_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -896,8 +898,9 @@ TEST(TestComponentInterfaces, dummy_sensor) hardware_interface::Sensor sensor_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); - auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_sensor_components"); + auto state = + sensor_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -974,8 +977,9 @@ TEST(TestComponentInterfaces, dummy_system) hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); - auto state = system_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1300,8 +1304,9 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) hardware_interface::System system_hw( std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); - auto state = system_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1333,8 +1338,9 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); - auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_components"); + auto state = + actuator_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1466,8 +1472,9 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); - auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_components"); + auto state = + actuator_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1598,8 +1605,9 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) hardware_interface::Sensor sensor_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); - auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_sensor_components"); + auto state = + sensor_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE @@ -1725,8 +1733,9 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); - auto state = system_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1863,8 +1872,9 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); - auto state = system_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(mock_hw_info, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1994,3 +2004,10 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From c4014f03e3dd526fd346b70935363b3945e643e1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 27 Aug 2024 11:35:13 +0200 Subject: [PATCH 21/25] use the internal mutex to skip when occupied by other nonRT things --- .../include/hardware_interface/actuator.hpp | 3 +++ .../include/hardware_interface/sensor.hpp | 3 +++ .../include/hardware_interface/system.hpp | 3 +++ hardware_interface/src/actuator.cpp | 17 +---------------- hardware_interface/src/resource_manager.cpp | 16 ++++++++++++++++ hardware_interface/src/sensor.cpp | 9 +-------- hardware_interface/src/system.cpp | 17 +---------------- 7 files changed, 28 insertions(+), 40 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 89fe331143..bfeb5d969d 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -107,6 +107,9 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); + HARDWARE_INTERFACE_PUBLIC + std::recursive_mutex & get_mutex(); + private: std::unique_ptr impl_; mutable std::recursive_mutex actuators_mutex_; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 01fb9a51db..ac7f3f6f6a 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -91,6 +91,9 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; } + HARDWARE_INTERFACE_PUBLIC + std::recursive_mutex & get_mutex(); + private: std::unique_ptr impl_; mutable std::recursive_mutex sensors_mutex_; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 47a84fb2dc..749e10d5e4 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -107,6 +107,9 @@ class System final HARDWARE_INTERFACE_PUBLIC return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); + HARDWARE_INTERFACE_PUBLIC + std::recursive_mutex & get_mutex(); + private: std::unique_ptr impl_; mutable std::recursive_mutex system_mutex_; diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 1612d2c291..304e27387a 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -290,14 +290,6 @@ const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_c return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(actuators_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping read() call for actuator '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { last_read_cycle_time_ = time; @@ -320,14 +312,6 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(actuators_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping write() call for actuator '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { last_write_cycle_time_ = time; @@ -348,4 +332,5 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & return result; } +std::recursive_mutex & Actuator::get_mutex() { return actuators_mutex_; } } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index eb166fc648..dd2aa61c05 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1844,6 +1844,14 @@ HardwareReadWriteStatus ResourceManager::read( { for (auto & component : components) { + std::unique_lock lock(component.get_mutex(), std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + get_logger(), "Skipping read() call for the component '%s' since it is locked", + component.get_name().c_str()); + continue; + } auto ret_val = return_type::OK; try { @@ -1921,6 +1929,14 @@ HardwareReadWriteStatus ResourceManager::write( { for (auto & component : components) { + std::unique_lock lock(component.get_mutex(), std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + get_logger(), "Skipping write() call for the component '%s' since it is locked", + component.get_name().c_str()); + continue; + } auto ret_val = return_type::OK; try { diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index cc15464f98..26b66ffd47 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -245,14 +245,6 @@ const rclcpp::Time & Sensor::get_last_read_time() const { return last_read_cycle return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(sensors_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping read() call for the sensor '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { last_read_cycle_time_ = time; @@ -273,4 +265,5 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per return result; } +std::recursive_mutex & Sensor::get_mutex() { return sensors_mutex_; } } // namespace hardware_interface diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 8e8c889289..bc625f22e7 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -288,14 +288,6 @@ const rclcpp::Time & System::get_last_write_time() const { return last_write_cyc return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(system_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping read() call for system '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { last_read_cycle_time_ = time; @@ -318,14 +310,6 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(system_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping write() call for system '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) { last_write_cycle_time_ = time; @@ -346,4 +330,5 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe return result; } +std::recursive_mutex & System::get_mutex() { return system_mutex_; } } // namespace hardware_interface From b46f5819575d640f1f077855d0b86ee81b098113 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 27 Aug 2024 12:23:43 +0200 Subject: [PATCH 22/25] add missing copying of read and write time in the copy constructor --- hardware_interface/src/actuator.cpp | 2 ++ hardware_interface/src/sensor.cpp | 1 + hardware_interface/src/system.cpp | 2 ++ 3 files changed, 5 insertions(+) diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 304e27387a..0e4ae95ca9 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -41,6 +41,8 @@ Actuator::Actuator(Actuator && other) noexcept { std::lock_guard lock(other.actuators_mutex_); impl_ = std::move(other.impl_); + last_read_cycle_time_ = other.last_read_cycle_time_; + last_write_cycle_time_ = other.last_write_cycle_time_; } const rclcpp_lifecycle::State & Actuator::initialize( diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 26b66ffd47..ff193ff8e1 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -40,6 +40,7 @@ Sensor::Sensor(Sensor && other) noexcept { std::lock_guard lock(other.sensors_mutex_); impl_ = std::move(other.impl_); + last_read_cycle_time_ = other.last_read_cycle_time_; } const rclcpp_lifecycle::State & Sensor::initialize( diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index bc625f22e7..9a27aa4f68 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -39,6 +39,8 @@ System::System(System && other) noexcept { std::lock_guard lock(other.system_mutex_); impl_ = std::move(other.impl_); + last_read_cycle_time_ = other.last_read_cycle_time_; + last_write_cycle_time_ = other.last_write_cycle_time_; } const rclcpp_lifecycle::State & System::initialize( From 743763009cf86c56af885b73f27957ff62c9e36c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 27 Aug 2024 15:12:35 +0200 Subject: [PATCH 23/25] added the corner case of the simulation --- hardware_interface/src/resource_manager.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index dd2aa61c05..6af02b87d1 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1856,8 +1856,9 @@ HardwareReadWriteStatus ResourceManager::read( try { if ( + resource_storage_->hardware_info_map_[component.get_name()].read_rate == 0 || resource_storage_->hardware_info_map_[component.get_name()].read_rate == - resource_storage_->cm_update_rate_) + resource_storage_->cm_update_rate_) { ret_val = component.read(time, period); } @@ -1941,8 +1942,9 @@ HardwareReadWriteStatus ResourceManager::write( try { if ( + resource_storage_->hardware_info_map_[component.get_name()].write_rate == 0 || resource_storage_->hardware_info_map_[component.get_name()].write_rate == - resource_storage_->cm_update_rate_) + resource_storage_->cm_update_rate_) { ret_val = component.write(time, period); } From bc47f7d2790ae72f300e02afa947e7628010fa78 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 25 Nov 2024 23:00:23 +0100 Subject: [PATCH 24/25] Fix test_component_interfaces for newly added tests --- .../test/test_component_interfaces.cpp | 40 +++++++++++-------- ...est_component_interfaces_custom_export.cpp | 23 ++++++++--- 2 files changed, 41 insertions(+), 22 deletions(-) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 928adde4bc..df2147bd1a 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -783,8 +783,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); - auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -938,8 +939,9 @@ TEST(TestComponentInterfaces, dummy_sensor_default) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); - auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + sensor_hw.initialize(voltage_sensor_res, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1110,8 +1112,9 @@ TEST(TestComponentInterfaces, dummy_system_default) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); - auto state = system_hw.initialize(dummy_system, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1407,8 +1410,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); - auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1541,8 +1545,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); - auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1678,8 +1683,9 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); - auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + sensor_hw.initialize(voltage_sensor_res, node->get_logger(), node->get_node_clock_interface()); auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE @@ -1806,8 +1812,9 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); - auto state = system_hw.initialize(dummy_system, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1945,8 +1952,9 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); - auto state = system_hw.initialize(dummy_system, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_components"); + auto state = + system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index ab6f490b92..b64ea81bc8 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -33,6 +33,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/node.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" @@ -169,8 +170,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default_custom_export) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); - auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_actuator_component"); + auto state = + actuator_hw.initialize(dummy_actuator, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -234,8 +236,9 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); - auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_sensor_component"); + auto state = + sensor_hw.initialize(voltage_sensor_res, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -271,8 +274,9 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); - auto state = system_hw.initialize(dummy_system, logger, nullptr); + rclcpp::Node::SharedPtr node = std::make_shared("test_system_component"); + auto state = + system_hw.initialize(dummy_system, node->get_logger(), node->get_node_clock_interface()); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -373,3 +377,10 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); } } + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From ef64595659d78868538c57c36230d3da317f11d2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Dec 2024 10:51:26 +0100 Subject: [PATCH 25/25] Move to single rw_rate variable instead of read and write separate rates --- .../hardware_component_info.hpp | 7 ++--- hardware_interface/src/resource_manager.cpp | 18 +++++------ .../test/test_resource_manager.cpp | 30 +++++++------------ 3 files changed, 20 insertions(+), 35 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index b897f12aaf..70a0482811 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -48,11 +48,8 @@ struct HardwareComponentInfo /// Component is async bool is_async; - //// read rate - unsigned int read_rate; - - //// write rate - unsigned int write_rate; + //// read/write rate + unsigned int rw_rate; /// Component current state. rclcpp_lifecycle::State state; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 6af02b87d1..d338250e4f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -142,11 +142,7 @@ class ResourceStorage component_info.name = hardware_info.name; component_info.type = hardware_info.type; component_info.group = hardware_info.group; - component_info.read_rate = - (hardware_info.rw_rate == 0 || hardware_info.rw_rate > cm_update_rate_) - ? cm_update_rate_ - : hardware_info.rw_rate; - component_info.write_rate = + component_info.rw_rate = (hardware_info.rw_rate == 0 || hardware_info.rw_rate > cm_update_rate_) ? cm_update_rate_ : hardware_info.rw_rate; @@ -1856,8 +1852,8 @@ HardwareReadWriteStatus ResourceManager::read( try { if ( - resource_storage_->hardware_info_map_[component.get_name()].read_rate == 0 || - resource_storage_->hardware_info_map_[component.get_name()].read_rate == + resource_storage_->hardware_info_map_[component.get_name()].rw_rate == 0 || + resource_storage_->hardware_info_map_[component.get_name()].rw_rate == resource_storage_->cm_update_rate_) { ret_val = component.read(time, period); @@ -1865,7 +1861,7 @@ HardwareReadWriteStatus ResourceManager::read( else { const double read_rate = - resource_storage_->hardware_info_map_[component.get_name()].read_rate; + resource_storage_->hardware_info_map_[component.get_name()].rw_rate; const auto current_time = resource_storage_->get_clock()->now(); const rclcpp::Duration actual_period = current_time - component.get_last_read_time(); if (actual_period.seconds() * read_rate >= 0.99) @@ -1942,8 +1938,8 @@ HardwareReadWriteStatus ResourceManager::write( try { if ( - resource_storage_->hardware_info_map_[component.get_name()].write_rate == 0 || - resource_storage_->hardware_info_map_[component.get_name()].write_rate == + resource_storage_->hardware_info_map_[component.get_name()].rw_rate == 0 || + resource_storage_->hardware_info_map_[component.get_name()].rw_rate == resource_storage_->cm_update_rate_) { ret_val = component.write(time, period); @@ -1951,7 +1947,7 @@ HardwareReadWriteStatus ResourceManager::write( else { const double write_rate = - resource_storage_->hardware_info_map_[component.get_name()].write_rate; + resource_storage_->hardware_info_map_[component.get_name()].rw_rate; const auto current_time = resource_storage_->get_clock()->now(); const rclcpp::Duration actual_period = current_time - component.get_last_write_time(); if (actual_period.seconds() * write_rate >= 0.99) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 97838776c3..5f7640546a 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -457,14 +457,10 @@ TEST_F(ResourceManagerTest, resource_status) EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].type, TEST_ACTUATOR_HARDWARE_TYPE); EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].type, TEST_SENSOR_HARDWARE_TYPE); EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].type, TEST_SYSTEM_HARDWARE_TYPE); - // read_rate - EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].read_rate, 50u); - EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].read_rate, 20u); - EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].read_rate, 25u); - // write_rate - EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].write_rate, 50u); - EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].write_rate, 20u); - EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].write_rate, 25u); + // read/write_rate + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].rw_rate, 50u); + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].rw_rate, 20u); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].rw_rate, 25u); // plugin_name EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].plugin_name, TEST_ACTUATOR_HARDWARE_PLUGIN_NAME); @@ -1765,17 +1761,13 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - // read_rate - EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].read_rate, 50u); - EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].read_rate, 20u); - EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].read_rate, 25u); - // write_rate - EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].write_rate, 50u); - EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].write_rate, 20u); - EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].write_rate, 25u); - - actuator_rw_rate_ = status_map[TEST_ACTUATOR_HARDWARE_NAME].read_rate; - system_rw_rate_ = status_map[TEST_SYSTEM_HARDWARE_NAME].read_rate; + // read/write_rate + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].rw_rate, 50u); + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].rw_rate, 20u); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].rw_rate, 25u); + + actuator_rw_rate_ = status_map[TEST_ACTUATOR_HARDWARE_NAME].rw_rate; + system_rw_rate_ = status_map[TEST_SYSTEM_HARDWARE_NAME].rw_rate; claimed_itfs.push_back( rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0]));