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();