Add test case for the async with different read/write rate

saikishor · saikishor · commit 741ba8fd6a6f · 2025-02-27T09:30:36.000+01:00
diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp
@@ -568,9 +568,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   // interface names to Handle accessed through getters/setters
   std::unordered_map<std::string, StateInterface::SharedPtr> actuator_states_;
   std::unordered_map<std::string, CommandInterface::SharedPtr> actuator_commands_;
-  std::atomic<return_type> read_return_info_;
+  std::atomic<return_type> read_return_info_ = return_type::OK;
   std::atomic<std::chrono::nanoseconds> read_execution_time_ = std::chrono::nanoseconds::zero();
-  std::atomic<return_type> write_return_info_;
+  std::atomic<return_type> write_return_info_ = return_type::OK;
   std::atomic<std::chrono::nanoseconds> write_execution_time_ = std::chrono::nanoseconds::zero();
 
 protected:
diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp
@@ -607,9 +607,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   // interface names to Handle accessed through getters/setters
   std::unordered_map<std::string, StateInterface::SharedPtr> system_states_;
   std::unordered_map<std::string, CommandInterface::SharedPtr> system_commands_;
-  std::atomic<return_type> read_return_info_;
+  std::atomic<return_type> read_return_info_ = return_type::OK;
   std::atomic<std::chrono::nanoseconds> read_execution_time_ = std::chrono::nanoseconds::zero();
-  std::atomic<return_type> write_return_info_;
+  std::atomic<return_type> write_return_info_ = return_type::OK;
   std::atomic<std::chrono::nanoseconds> write_execution_time_ = std::chrono::nanoseconds::zero();
 
 protected:
diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp
@@ -299,12 +299,12 @@ const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_c
 
 const HardwareComponentStatisticsCollector & Actuator::get_read_statistics() const
 {
-  return !impl_->get_hardware_info().is_async ? read_statistics_ : write_statistics_;
+  return read_statistics_;
 }
 
 const HardwareComponentStatisticsCollector & Actuator::get_write_statistics() const
 {
-  return !impl_->get_hardware_info().is_async ? write_statistics_ : read_statistics_;
+  return write_statistics_;
 }
 
 return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period)
diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp
@@ -297,12 +297,12 @@ const rclcpp::Time & System::get_last_write_time() const { return last_write_cyc
 
 const HardwareComponentStatisticsCollector & System::get_read_statistics() const
 {
-  return !impl_->get_hardware_info().is_async ? read_statistics_ : write_statistics_;
+  return read_statistics_;
 }
 
 const HardwareComponentStatisticsCollector & System::get_write_statistics() const
 {
-  return !impl_->get_hardware_info().is_async ? write_statistics_ : read_statistics_;
+  return write_statistics_;
 }
 
 return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period)
diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp
@@ -1739,10 +1739,16 @@ TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware)
 class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManagerTest
 {
 public:
-  void setup_resource_manager_and_do_initial_checks()
+  void setup_resource_manager_and_do_initial_checks(bool async_components)
   {
-    rm = std::make_shared<TestableResourceManager>(
-      node_, ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate, false);
+    const auto minimal_robot_urdf_with_different_hw_rw_rate_with_async =
+      std::string(ros2_control_test_assets::urdf_head) +
+      std::string(ros2_control_test_assets::hardware_resources_with_different_rw_rates_with_async) +
+      std::string(ros2_control_test_assets::urdf_tail);
+    const std::string urdf =
+      async_components ? minimal_robot_urdf_with_different_hw_rw_rate_with_async
+                       : ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate;
+    rm = std::make_shared<TestableResourceManager>(node_, urdf, false);
     activate_components(*rm);
 
     cm_update_rate_ = 100u;  // The default value inside
@@ -1767,6 +1773,9 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage
     actuator_rw_rate_ = status_map[TEST_ACTUATOR_HARDWARE_NAME].rw_rate;
     system_rw_rate_ = status_map[TEST_SYSTEM_HARDWARE_NAME].rw_rate;
 
+    actuator_is_async_ = status_map[TEST_ACTUATOR_HARDWARE_NAME].is_async;
+    system_is_async_ = status_map[TEST_SYSTEM_HARDWARE_NAME].is_async;
+
     claimed_itfs.push_back(
       rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0]));
     claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0]));
@@ -1841,8 +1850,30 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage
       }
       // Even though we skip some read and write iterations, the state interfaces should be the same
       // as previous updated one until the next cycle
-      ASSERT_EQ(state_itfs[0].get_optional().value(), prev_act_state_value);
-      ASSERT_EQ(state_itfs[1].get_optional().value(), prev_system_state_value);
+      if (actuator_is_async_)
+      {
+        // check it is either the previous value or the new one
+        EXPECT_THAT(
+          state_itfs[0].get_optional(), testing::AnyOf(
+                                       testing::DoubleEq(prev_act_state_value),
+                                       testing::DoubleEq(prev_act_state_value + 5.0)));
+      }
+      else
+      {
+        ASSERT_EQ(state_itfs[0].get_optional(), prev_act_state_value);
+      }
+      if (system_is_async_)
+      {
+        // check it is either the previous value or the new one
+        EXPECT_THAT(
+          state_itfs[1].get_optional(), testing::AnyOf(
+                                       testing::DoubleEq(prev_system_state_value),
+                                       testing::DoubleEq(prev_system_state_value + 10.0)));
+      }
+      else
+      {
+        ASSERT_EQ(state_itfs[1].get_optional(), prev_system_state_value);
+      }
       auto [ok_write, failed_hardware_names_write] = rm->write(time, duration);
       EXPECT_TRUE(ok_write);
       EXPECT_TRUE(failed_hardware_names_write.empty());
@@ -1854,9 +1885,10 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage
         {
           if (i > (cm_update_rate_ / rate))
           {
+            const double expec_execution_time = (1.e6 / (3 * rate)) + 200.0;
             EXPECT_LT(
               status_map[component_name].read_statistics->execution_time.get_statistics().average,
-              100);
+              expec_execution_time);
             EXPECT_LT(
               status_map[component_name].read_statistics->periodicity.get_statistics().average,
               1.2 * rate);
@@ -1869,7 +1901,7 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage
 
             EXPECT_LT(
               status_map[component_name].write_statistics->execution_time.get_statistics().average,
-              100);
+              expec_execution_time);
             EXPECT_LT(
               status_map[component_name].write_statistics->periodicity.get_statistics().average,
               1.2 * rate);
@@ -1893,6 +1925,7 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage
 public:
   std::shared_ptr<TestableResourceManager> rm;
   unsigned int actuator_rw_rate_, system_rw_rate_, cm_update_rate_;
+  bool actuator_is_async_, system_is_async_;
   std::vector<hardware_interface::LoanedCommandInterface> claimed_itfs;
   std::vector<hardware_interface::LoanedStateInterface> state_itfs;
 
@@ -1906,7 +1939,16 @@ TEST_F(
   ResourceManagerTestReadWriteDifferentReadWriteRate,
   test_components_with_different_read_write_freq_on_activate)
 {
-  setup_resource_manager_and_do_initial_checks();
+  setup_resource_manager_and_do_initial_checks(false);
+
+  check_read_and_write_cycles(true);
+}
+
+TEST_F(
+  ResourceManagerTestReadWriteDifferentReadWriteRate,
+  test_components_with_different_read_write_freq_on_activate_with_async)
+{
+  setup_resource_manager_and_do_initial_checks(true);
 
   check_read_and_write_cycles(true);
 }
@@ -1915,7 +1957,35 @@ TEST_F(
   ResourceManagerTestReadWriteDifferentReadWriteRate,
   test_components_with_different_read_write_freq_on_deactivate)
 {
-  setup_resource_manager_and_do_initial_checks();
+  setup_resource_manager_and_do_initial_checks(false);
+
+  // Now deactivate all the components and test the same as above
+  rclcpp_lifecycle::State state_inactive(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    hardware_interface::lifecycle_state_names::INACTIVE);
+  rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_inactive);
+  rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_inactive);
+  rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_inactive);
+
+  auto status_map = rm->get_components_status();
+  EXPECT_EQ(
+    status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+  EXPECT_EQ(
+    status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+  EXPECT_EQ(
+    status_map[TEST_SENSOR_HARDWARE_NAME].state.id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+
+  check_read_and_write_cycles(true);
+}
+
+TEST_F(
+  ResourceManagerTestReadWriteDifferentReadWriteRate,
+  test_components_with_different_read_write_freq_on_deactivate_with_async)
+{
+  setup_resource_manager_and_do_initial_checks(true);
 
   // Now deactivate all the components and test the same as above
   rclcpp_lifecycle::State state_inactive(
@@ -1943,7 +2013,35 @@ TEST_F(
   ResourceManagerTestReadWriteDifferentReadWriteRate,
   test_components_with_different_read_write_freq_on_unconfigured)
 {
-  setup_resource_manager_and_do_initial_checks();
+  setup_resource_manager_and_do_initial_checks(false);
+
+  // Now deactivate all the components and test the same as above
+  rclcpp_lifecycle::State state_unconfigured(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    hardware_interface::lifecycle_state_names::UNCONFIGURED);
+  rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_unconfigured);
+  rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_unconfigured);
+  rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_unconfigured);
+
+  auto status_map = rm->get_components_status();
+  EXPECT_EQ(
+    status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  EXPECT_EQ(
+    status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  EXPECT_EQ(
+    status_map[TEST_SENSOR_HARDWARE_NAME].state.id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+  check_read_and_write_cycles(false);
+}
+
+TEST_F(
+  ResourceManagerTestReadWriteDifferentReadWriteRate,
+  test_components_with_different_read_write_freq_on_unconfigured_with_async)
+{
+  setup_resource_manager_and_do_initial_checks(true);
 
   // Now deactivate all the components and test the same as above
   rclcpp_lifecycle::State state_unconfigured(
@@ -1971,7 +2069,35 @@ TEST_F(
   ResourceManagerTestReadWriteDifferentReadWriteRate,
   test_components_with_different_read_write_freq_on_finalized)
 {
-  setup_resource_manager_and_do_initial_checks();
+  setup_resource_manager_and_do_initial_checks(false);
+
+  // Now deactivate all the components and test the same as above
+  rclcpp_lifecycle::State state_finalized(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
+    hardware_interface::lifecycle_state_names::FINALIZED);
+  rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_finalized);
+  rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_finalized);
+  rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_finalized);
+
+  auto status_map = rm->get_components_status();
+  EXPECT_EQ(
+    status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
+  EXPECT_EQ(
+    status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
+  EXPECT_EQ(
+    status_map[TEST_SENSOR_HARDWARE_NAME].state.id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
+
+  check_read_and_write_cycles(false);
+}
+
+TEST_F(
+  ResourceManagerTestReadWriteDifferentReadWriteRate,
+  test_components_with_different_read_write_freq_on_finalized_with_async)
+{
+  setup_resource_manager_and_do_initial_checks(true);
 
   // Now deactivate all the components and test the same as above
   rclcpp_lifecycle::State state_finalized(
@@ -2072,14 +2198,14 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest
     {
       auto [ok, failed_hardware_names] = rm->read(time, duration);
       EXPECT_TRUE(ok);
-      EXPECT_TRUE(failed_hardware_names.empty());
+      EXPECT_THAT(failed_hardware_names, testing::IsEmpty());
     }
     {
       // claimed_itfs[0].set_value(10.0);
       // claimed_itfs[1].set_value(20.0);
       auto [ok, failed_hardware_names] = rm->write(time, duration);
       EXPECT_TRUE(ok);
-      EXPECT_TRUE(failed_hardware_names.empty());
+      EXPECT_THAT(failed_hardware_names, testing::IsEmpty());
     }
     node_.get_clock()->sleep_until(time + duration);
     time = node_.get_clock()->now();
diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp
@@ -758,6 +758,55 @@ const auto hardware_resources_with_different_rw_rates =
   </ros2_control>
 )";
 
+const auto hardware_resources_with_different_rw_rates_with_async =
+  R"(
+  <ros2_control name="TestActuatorHardware" type="actuator" rw_rate="50" is_async="true">
+    <hardware>
+      <plugin>test_actuator</plugin>
+    </hardware>
+    <joint name="joint1">
+      <command_interface name="position"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <command_interface name="max_velocity" />
+    </joint>
+  </ros2_control>
+  <ros2_control name="TestSensorHardware" type="sensor" rw_rate="20" is_async="true">
+    <hardware>
+      <plugin>test_sensor</plugin>
+      <param name="example_param_write_for_sec">2</param>
+      <param name="example_param_read_for_sec">2</param>
+    </hardware>
+    <sensor name="sensor1">
+      <state_interface name="velocity"/>
+    </sensor>
+  </ros2_control>
+  <ros2_control name="TestSystemHardware" type="system" rw_rate="25" is_async="true">
+    <hardware>
+      <plugin>test_system</plugin>
+      <param name="example_param_write_for_sec">2</param>
+      <param name="example_param_read_for_sec">2</param>
+    </hardware>
+    <joint name="joint2">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="acceleration"/>
+      <command_interface name="max_acceleration" />
+    </joint>
+    <joint name="joint3">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="acceleration"/>
+    </joint>
+    <gpio name="configuration">
+      <command_interface name="max_tcp_jerk"/>
+      <state_interface name="max_tcp_jerk"/>
+    </gpio>
+  </ros2_control>
+)";
+
 const auto hardware_resources_with_negative_rw_rates =
   R"(
   <ros2_control name="TestActuatorHardware" type="actuator" rw_rate="-50">

Original file line number	Diff line number	Diff line change
`@@ -299,12 +299,12 @@ const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_c`
`299`	`299`
`300`	`300`	`const HardwareComponentStatisticsCollector & Actuator::get_read_statistics() const`
`301`	`301`	`{`
`302`		`- return !impl_->get_hardware_info().is_async ? read_statistics_ : write_statistics_;`
	`302`	`+ return read_statistics_;`
`303`	`303`	`}`
`304`	`304`
`305`	`305`	`const HardwareComponentStatisticsCollector & Actuator::get_write_statistics() const`
`306`	`306`	`{`
`307`		`- return !impl_->get_hardware_info().is_async ? write_statistics_ : read_statistics_;`
	`307`	`+ return write_statistics_;`
`308`	`308`	`}`
`309`	`309`
`310`	`310`	`return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period)`
Original file line number	Diff line number	Diff line change
`@@ -297,12 +297,12 @@ const rclcpp::Time & System::get_last_write_time() const { return last_write_cyc`
`297`	`297`
`298`	`298`	`const HardwareComponentStatisticsCollector & System::get_read_statistics() const`
`299`	`299`	`{`
`300`		`- return !impl_->get_hardware_info().is_async ? read_statistics_ : write_statistics_;`
	`300`	`+ return read_statistics_;`
`301`	`301`	`}`
`302`	`302`
`303`	`303`	`const HardwareComponentStatisticsCollector & System::get_write_statistics() const`
`304`	`304`	`{`
`305`		`- return !impl_->get_hardware_info().is_async ? write_statistics_ : read_statistics_;`
	`305`	`+ return write_statistics_;`
`306`	`306`	`}`
`307`	`307`
`308`	`308`	`return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period)`