From ef64595659d78868538c57c36230d3da317f11d2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Dec 2024 10:51:26 +0100 Subject: [PATCH] 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]));