[RM] Fix skipped cycles by adjusting rw_rate handling (#2091)

Author: RobertWilbrandt

hardware_interface/src/resource_manager.cpp

@@ -1792,7 +1792,11 @@ HardwareReadWriteStatus ResourceManager::read(
             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)
+
+          const double error_now = std::abs(actual_period.seconds() * read_rate - 1.0);
+          const double error_if_skipped = std::abs(
+            (actual_period.seconds() + 1.0 / resource_storage_->cm_update_rate_) * read_rate - 1.0);
+          if (error_now <= error_if_skipped)
           {
             ret_val = component.read(current_time, actual_period);
           }
@@ -1883,7 +1887,12 @@ HardwareReadWriteStatus ResourceManager::write(
             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)
+
+          const double error_now = std::abs(actual_period.seconds() * write_rate - 1.0);
+          const double error_if_skipped = std::abs(
+            (actual_period.seconds() + 1.0 / resource_storage_->cm_update_rate_) * write_rate -
+            1.0);
+          if (error_now <= error_if_skipped)
           {
             ret_val = component.write(current_time, actual_period);
           }

hardware_interface_testing/test/test_resource_manager.cpp

@@ -1956,6 +1956,60 @@ TEST_F(
   check_read_and_write_cycles(false);
 }
 
+TEST_F(
+  ResourceManagerTestReadWriteDifferentReadWriteRate,
+  test_components_with_different_read_write_freq_not_exact_timing)
+{
+  setup_resource_manager_and_do_initial_checks();
+
+  const auto test_jitter = std::chrono::milliseconds{1};
+
+  const auto read = [&]()
+  {
+    const auto [ok, failed_hardware_names] = rm->read(time, duration);
+    EXPECT_TRUE(ok);
+    EXPECT_TRUE(failed_hardware_names.empty());
+  };
+  const auto write = [&]()
+  {
+    const auto [ok, failed_hardware_names] = rm->write(time, duration);
+    EXPECT_TRUE(ok);
+    EXPECT_TRUE(failed_hardware_names.empty());
+  };
+
+  // t = 1 * duration
+  // State interface should not update
+  read();
+  EXPECT_DOUBLE_EQ(state_itfs[0].get_optional().value(), 0.0);
+  EXPECT_TRUE(claimed_itfs[0].set_value(10));
+  write();
+  node_.get_clock()->sleep_until(time + duration + test_jitter);
+  time = node_.get_clock()->now();
+
+  // t = 2 * duration + test_jitter
+  // State interface should update
+  read();
+  EXPECT_DOUBLE_EQ(state_itfs[0].get_optional().value(), 5.0);
+  EXPECT_TRUE(claimed_itfs[0].set_value(20));
+  write();
+  node_.get_clock()->sleep_until(time + duration - test_jitter);
+  time = node_.get_clock()->now();
+
+  // t = 3 * duration
+  // State interface should not update
+  read();
+  EXPECT_DOUBLE_EQ(state_itfs[0].get_optional().value(), 5.0);
+  EXPECT_TRUE(claimed_itfs[0].set_value(30));
+  write();
+  node_.get_clock()->sleep_until(time + duration - test_jitter);
+  time = node_.get_clock()->now();
+
+  // t = 4 * duration - test_jitter
+  // State interface should update
+  read();
+  EXPECT_DOUBLE_EQ(state_itfs[0].get_optional().value(), 15.0);
+}
+
 class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest
 {
 public: