From 71f1e85a53ac4cfb951f225b8190d9deee2fcc1f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 13 Jun 2024 07:23:03 +0200 Subject: [PATCH 01/16] 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 e7d47bcaa4..412e8d8814 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -23,6 +23,7 @@ #include #include +#include #include "rclcpp_lifecycle/state.hpp" namespace hardware_interface @@ -48,6 +49,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 a6eadd49fe..6bf2f365e7 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; @@ -1781,7 +1783,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); @@ -1842,7 +1849,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 92d20061f48e7aef18d4433b5a3cf99511f6fe52 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 04:33:45 +0200 Subject: [PATCH 02/16] 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 3a1d7a5974..c2b84f1806 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -93,6 +93,12 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_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); @@ -101,6 +107,10 @@ class Actuator final private: std::unique_ptr impl_; + // 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 412e8d8814..448cfd3ed6 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -55,12 +55,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 d8e55aa4ad..95feeaab29 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -81,6 +81,9 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_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); @@ -89,6 +92,8 @@ class Sensor final private: std::unique_ptr impl_; + // 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 1ca4260750..327941991d 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -94,6 +94,12 @@ class System final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & get_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); @@ -102,6 +108,10 @@ class System final private: std::unique_ptr impl_; + // 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 7e50c07eb0..81902269e5 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -220,12 +220,17 @@ std::string Actuator::get_group_name() const { return impl_->get_group_name(); } const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_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) { if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { + last_read_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); return return_type::OK; } return_type result = return_type::ERROR; @@ -238,6 +243,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p { error(); } + last_read_cycle_time_ = time; } return result; } @@ -248,6 +254,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { + last_write_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); return return_type::OK; } return_type result = return_type::ERROR; @@ -260,6 +267,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 6bf2f365e7..29aa283cf6 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1789,6 +1789,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); @@ -1855,6 +1866,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 9b7f1f69d6..f28a395aca 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -197,12 +197,15 @@ std::string Sensor::get_group_name() const { return impl_->get_group_name(); } const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_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) { if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { + last_read_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); return return_type::OK; } return_type result = return_type::ERROR; @@ -215,6 +218,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 ba327d8ab2..223233accf 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -216,12 +216,17 @@ std::string System::get_group_name() const { return impl_->get_group_name(); } const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_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) { if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { + last_read_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); return return_type::OK; } return_type result = return_type::ERROR; @@ -234,6 +239,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per { error(); } + last_read_cycle_time_ = time; } return result; } @@ -244,6 +250,7 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { + last_write_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type()); return return_type::OK; } return_type result = return_type::ERROR; @@ -256,6 +263,7 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe { error(); } + last_write_cycle_time_ = time; } return result; } From 030b759572215c3804a39eafa77b468ad73c29be Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 09:15:32 +0200 Subject: [PATCH 03/16] 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 2bd2099e69..477a1339b7 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -135,6 +135,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 938deeb0fb..6c9bf61549 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -57,6 +57,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 @@ -221,6 +222,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. @@ -567,6 +589,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 a5cdbb9fe5b4ff8b86b7cf96692e1590f80ccf27 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 09:16:27 +0200 Subject: [PATCH 04/16] 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 29aa283cf6..725f118d54 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 e98c2bf0f68dd831bdeca19fdd0251334705e021 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 09:17:05 +0200 Subject: [PATCH 05/16] 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 5fb155fa3a..3f7ecc18fc 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 cc2b1798d4..60c6e8f62a 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"( @@ -1719,6 +1768,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 e4a09a62d3e6cadb3986299778e7b8d0f6279618 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 10:48:07 +0200 Subject: [PATCH 06/16] 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 3f7ecc18fc..f2d684ab2c 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 60c6e8f62a..ce89e22e80 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 45c3f0e07ffacc6576f3cfb22bd59abf7794de12 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:18:52 +0200 Subject: [PATCH 07/16] 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 725f118d54..b7d62772ce 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1797,13 +1797,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(); @@ -1874,13 +1874,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 31ef85e7906e3a69cf016378a64e5aaaef5e3267 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:19:31 +0200 Subject: [PATCH 08/16] 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 502f5b4c21..b9ac1cd1ec 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 802db2c83d893eb34dbfb35b5e321a9a2b7f6751 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:20:34 +0200 Subject: [PATCH 09/16] 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 f2d684ab2c..802efc819a 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 c524ca552eda9446f3520210cdf6c41cef98ad57 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:34:54 +0200 Subject: [PATCH 10/16] 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 802efc819a..5b195807ea 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 bd4ff9b3c13c51169011782d8994d13b19fb18dc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:36:16 +0200 Subject: [PATCH 11/16] 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 5b195807ea..987b51a5cc 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 f4fc17b33106343e63f1befd10cbcdcdc86433a3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:45:51 +0200 Subject: [PATCH 12/16] 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 987b51a5cc..9366c7f1bc 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 c81057683b90e9ddde92721b86c3e7db95f4a57d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 14:46:23 +0200 Subject: [PATCH 13/16] 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 9366c7f1bc..43b3d22984 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 482a3558a042ed8fac480b4fd58535fd0f9fb4aa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 15:22:55 +0200 Subject: [PATCH 14/16] 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 6c9bf61549..25e989a6f5 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -233,14 +233,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 be891787f3..59ab369a1a 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1341,6 +1341,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 ce89e22e80..6308416266 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 23fde8245acca5368ab3a79e36d643ed99d145d7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 17 Jun 2024 08:11:40 +0200 Subject: [PATCH 15/16] Update documentation and the release notes --- doc/release_notes/Jazzy.rst | 37 +++ .../doc/different_update_rates_userdoc.rst | 241 ++++++------------ 2 files changed, 118 insertions(+), 160 deletions(-) diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst index 5d1773afe8..b3ac0dd987 100644 --- a/doc/release_notes/Jazzy.rst +++ b/doc/release_notes/Jazzy.rst @@ -98,6 +98,43 @@ 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 + + + + + + + + joint_limits ************ 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 ca62a0b2de9db995de75de7a3e820f95f6597a5d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 17 Jun 2024 10:39:29 +0200 Subject: [PATCH 16/16] 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 b7d62772ce..a9fd2a7c85 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1801,9 +1801,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(); @@ -1878,9 +1881,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();