Unify write behavior between Actuator and System hardware interfaces (#2453)

Author: soham2560

hardware_interface/src/actuator.cpp

@@ -388,9 +388,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration &
     last_write_cycle_time_ = time;
     return return_type::OK;
   }
-  if (
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     const auto trigger_result = impl_->trigger_write(time, period);
     if (trigger_result.result == return_type::ERROR)

hardware_interface/test/test_component_interfaces.cpp

@@ -813,14 +813,13 @@ TEST(TestComponentInterfaces, dummy_actuator)
   EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id());
   EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label());
 
-  // Read and Write are working because it is INACTIVE
+  // Read should work but write should not update the state because it is INACTIVE
   for (auto step = 0u; step < 10; ++step)
   {
     ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
 
-    EXPECT_EQ(
-      step * velocity_value, state_interfaces[0]->get_optional().value());  // position value
-    EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_optional().value());  // velocity
+    EXPECT_EQ(0.0, state_interfaces[0]->get_optional().value());  // position value
+    EXPECT_EQ(0.0, state_interfaces[1]->get_optional().value());  // velocity
 
     ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD));
   }
@@ -835,9 +834,10 @@ TEST(TestComponentInterfaces, dummy_actuator)
     ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
 
     EXPECT_EQ(
-      (10 + step) * velocity_value,
-      state_interfaces[0]->get_optional().value());                          // position value
-    EXPECT_EQ(velocity_value, state_interfaces[1]->get_optional().value());  // velocity
+      step * velocity_value,
+      state_interfaces[0]->get_optional().value());  // position value
+    EXPECT_EQ(
+      step ? velocity_value : 0.0, state_interfaces[1]->get_optional().value());  // velocity
 
     ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD));
   }
@@ -851,7 +851,7 @@ TEST(TestComponentInterfaces, dummy_actuator)
   {
     ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
 
-    EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_optional().value());  // position value
+    EXPECT_EQ(10 * velocity_value, state_interfaces[0]->get_optional().value());  // position value
     EXPECT_EQ(0, state_interfaces[1]->get_optional().value());                    // velocity
 
     ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD));
@@ -939,17 +939,13 @@ TEST(TestComponentInterfaces, dummy_actuator_default)
   EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id());
   EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label());
 
-  // Read and Write are working because it is INACTIVE
+  // Read should work but write should not update the state because it is INACTIVE
   for (auto step = 0u; step < 10; ++step)
   {
     ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
 
-    EXPECT_EQ(
-      step * velocity_value,
-      state_interfaces[si_joint1_pos]->get_optional().value());  // position value
-    EXPECT_EQ(
-      step ? velocity_value : 0,
-      state_interfaces[si_joint1_vel]->get_optional().value());  // velocity
+    EXPECT_EQ(0.0, state_interfaces[si_joint1_pos]->get_optional().value());  // position value
+    EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_optional().value());  // velocity
 
     ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD));
   }
@@ -964,9 +960,11 @@ TEST(TestComponentInterfaces, dummy_actuator_default)
     ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
 
     EXPECT_EQ(
-      (10 + step) * velocity_value,
+      step * velocity_value,
       state_interfaces[si_joint1_pos]->get_optional().value());  // position value
-    EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_optional().value());  // velocity
+    EXPECT_EQ(
+      step ? velocity_value : 0.0,
+      state_interfaces[si_joint1_vel]->get_optional().value());  // velocity
 
     ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD));
   }
@@ -981,7 +979,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default)
     ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
 
     EXPECT_EQ(
-      20 * velocity_value,
+      10 * velocity_value,
       state_interfaces[si_joint1_pos]->get_optional().value());             // position value
     EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_optional().value());  // velocity
 

hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp

@@ -361,10 +361,10 @@ TEST_F(
 };
 
 // System  : UNCONFIGURED
-// Actuator: ERROR
+// Actuator: DEACTIVATED then WRITE_IGNORED
 TEST_F(
   ResourceManagerPreparePerformTest,
-  when_system_unconfigured_and_actuator_active_and_then_error_expect_actuator_passing)
+  when_actuator_deactivated_then_write_error_is_ignored_and_remains_inactive)
 {
   preconfigure_components(
     lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured",
@@ -468,23 +468,25 @@ TEST_F(
   EXPECT_EQ(
     status_map["TestSystemCommandModes"].state.id(),
     lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  // The component is INACTIVE, write does nothing, so no error is triggered. State remains
+  // INACTIVE.
   EXPECT_EQ(
     status_map["TestActuatorHardware"].state.id(),
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
 
-  EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
+  EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
   EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
-  EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
-  EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
+  EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7);
+  EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
   EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
-  EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
+  EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
 
   EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
   EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
-  EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
+  EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
   EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
   EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
-  EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
+  EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
 };
 
 // System  : UNCONFIGURED