fix the logic back for failing tests

ros-controls · Jan 19, 2025 · a59873f · a59873f
1 parent 27eea3e
commit a59873f
Showing 1 changed file with 10 additions and 12 deletions.
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -1759,21 +1759,20 @@ HardwareReadWriteStatus ResourceManager::read(
         auto & hardware_component_info =
           resource_storage_->hardware_info_map_[component.get_name()];
         const auto current_time = resource_storage_->get_clock()->now();
-        const rclcpp::Duration actual_period =
-          component.get_last_read_time().get_clock_type() != RCL_CLOCK_UNINITIALIZED
-            ? current_time - component.get_last_read_time()
-            : rclcpp::Duration::from_seconds(
-                1.0 / static_cast<double>(hardware_component_info.rw_rate));
         // const rclcpp::Duration actual_period = current_time - component.get_last_read_time();
         if (
           hardware_component_info.rw_rate == 0 ||
           hardware_component_info.rw_rate == resource_storage_->cm_update_rate_)
         {
-          ret_val = component.read(current_time, actual_period);
+          ret_val = component.read(current_time, period);
         }
         else
         {
           const double read_rate = hardware_component_info.rw_rate;
+          const rclcpp::Duration actual_period =
+            component.get_last_read_time().get_clock_type() != RCL_CLOCK_UNINITIALIZED
+              ? current_time - component.get_last_read_time()
+              : rclcpp::Duration::from_seconds(1.0 / static_cast<double>(read_rate));
           if (actual_period.seconds() * read_rate >= 0.99)
           {
             ret_val = component.read(current_time, actual_period);
@@ -1858,20 +1857,19 @@ HardwareReadWriteStatus ResourceManager::write(
         auto & hardware_component_info =
           resource_storage_->hardware_info_map_[component.get_name()];
         const auto current_time = resource_storage_->get_clock()->now();
-        const rclcpp::Duration actual_period =
-          component.get_last_write_time().get_clock_type() != RCL_CLOCK_UNINITIALIZED
-            ? current_time - component.get_last_write_time()
-            : rclcpp::Duration::from_seconds(
-                1.0 / static_cast<double>(hardware_component_info.rw_rate));
         if (
           hardware_component_info.rw_rate == 0 ||
           hardware_component_info.rw_rate == resource_storage_->cm_update_rate_)
         {
-          ret_val = component.write(current_time, actual_period);
+          ret_val = component.write(current_time, period);
         }
         else
         {
           const double write_rate = hardware_component_info.rw_rate;
+          const rclcpp::Duration actual_period =
+            component.get_last_write_time().get_clock_type() != RCL_CLOCK_UNINITIALIZED
+              ? current_time - component.get_last_write_time()
+              : rclcpp::Duration::from_seconds(1.0 / static_cast<double>(write_rate));
           if (actual_period.seconds() * write_rate >= 0.99)
           {
             ret_val = component.write(current_time, actual_period);