Parse the actual period to the read and write methods

ros-controls · Jul 4, 2024 · 84d00da · 84d00da
1 parent 603935f
commit 84d00da
Showing 1 changed file with 10 additions and 4 deletions.
diff --git 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();