From 84d00dac50f6c442a6219a627323106a1d004ecd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 17 Jun 2024 10:39:29 +0200 Subject: [PATCH] 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 27a93ccdf5..23a5d442b5 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1678,9 +1678,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(); @@ -1755,9 +1758,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();