Deactivate controllers with command interfaces to hardware on DEACTIVATE

controller_manager/src/controller_manager.cpp

@@ -2669,9 +2669,9 @@ std::vector<std::string> ControllerManager::get_controller_names()
 void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
   periodicity_stats_.AddMeasurement(1.0 / period.seconds());
-  auto [ok, failed_hardware_names] = resource_manager_->read(time, period);
+  auto [result, failed_hardware_names] = resource_manager_->read(time, period);
 
-  if (!ok)
+  if (result != hardware_interface::return_type::OK)
   {
     rt_buffer_.deactivate_controllers_list.clear();
     // Determine controllers to stop
@@ -2938,9 +2938,9 @@ controller_interface::return_type ControllerManager::update(
 
 void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  auto [ok, failed_hardware_names] = resource_manager_->write(time, period);
+  auto [result, failed_hardware_names] = resource_manager_->write(time, period);
 
-  if (!ok)
+  if (result == hardware_interface::return_type::ERROR)
   {
     rt_buffer_.deactivate_controllers_list.clear();
     // Determine controllers to stop
@@ -2968,6 +2968,51 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration
     deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list);
     // TODO(destogl): do auto-start of broadcasters
   }
+  else if (result == hardware_interface::return_type::DEACTIVATE)
+  {
+    rt_buffer_.deactivate_controllers_list.clear();
+    auto loaded_controllers = get_loaded_controllers();
+    // Only stop controllers with active command interfaces to the failed_hardware_names
+    for (const auto & hardware_name : failed_hardware_names)
+    {
+      auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name);
+      for (const auto & controller : controllers)
+      {
+        auto controller_spec = std::find_if(
+          loaded_controllers.begin(), loaded_controllers.end(),
+          [&](const controller_manager::ControllerSpec & spec)
+          { return spec.c->get_name() == controller; });
+        if (controller_spec == loaded_controllers.end())
+        {
+          RCLCPP_WARN(
+            get_logger(),
+            "Deactivate failed to find controller [%s] in loaded controllers. "
+            "This can happen due to multiple returns of 'DEACTIVATE' from [%s] write()",
+            controller.c_str(), hardware_name.c_str());
+          continue;
+        }
+        std::vector<std::string> command_interface_names;
+        extract_command_interfaces_for_controller(
+          *controller_spec, resource_manager_, command_interface_names);
+        // if this controller has command interfaces add it to the deactivate_controllers_list
+        if (!command_interface_names.empty())
+        {
+          rt_buffer_.deactivate_controllers_list.push_back(controller);
+        }
+      }
+    }
+    RCLCPP_ERROR_EXPRESSION(
+      get_logger(), !rt_buffer_.deactivate_controllers_list.empty(),
+      "Deactivating controllers [%s] as their command interfaces are tied to DEACTIVATEing "
+      "hardware components",
+      rt_buffer_.get_concatenated_string(rt_buffer_.deactivate_controllers_list).c_str());
+    std::vector<ControllerSpec> & rt_controller_list =
+      rt_controllers_wrapper_.update_and_get_used_by_rt_list();
+
+    perform_hardware_command_mode_change(
+      rt_controller_list, {}, rt_buffer_.deactivate_controllers_list, "write");
+    deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list);
+  }
 }
 
 std::vector<ControllerSpec> &

controller_manager/test/test_controller_manager_hardware_error_handling.cpp

@@ -47,6 +47,7 @@ class TestableControllerManager : public controller_manager::ControllerManager
   FRIEND_TEST(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardware);
   FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_error);
   FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error);
+  FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_deactivate);
   FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_multiple_hardware_error);
 
 public:
@@ -428,6 +429,95 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er
     EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher);
     EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher);
   }
+
+  // Simulate deactivate in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
+  // by setting first command interface to READ_DEACTIVATE_VALUE
+  // (this should be the same as returning ERROR)
+  test_controller_actuator->set_first_command_interface_value_to =
+    test_constants::READ_DEACTIVATE_VALUE;
+  test_controller_system->set_first_command_interface_value_to =
+    test_constants::READ_DEACTIVATE_VALUE;
+  {
+    auto previous_counter_lower = test_controller_actuator->internal_counter + 1;
+    auto previous_counter_higher = test_controller_system->internal_counter + 1;
+    auto new_counter = test_broadcaster_sensor->internal_counter + 1;
+
+    EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
+
+    EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower)
+      << "Execute without errors to write value";
+    EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher)
+      << "Execute without errors to write value";
+    EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower)
+      << "Execute without errors to write value";
+    EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
+      << "Execute without errors to write value";
+  }
+
+  {
+    auto previous_counter_lower = test_controller_actuator->internal_counter;
+    auto previous_counter_higher = test_controller_system->internal_counter;
+    auto new_counter = test_broadcaster_sensor->internal_counter + 1;
+
+    EXPECT_NO_THROW(cm_->read(time_, PERIOD));
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_controller_actuator->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_controller_system->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_broadcaster_all->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      test_broadcaster_sensor->get_lifecycle_state().id());
+
+    EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
+    EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower)
+      << "Execute has read error and it is not updated";
+    EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher)
+      << "Execute has read error and it is not updated";
+    EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower)
+      << "Broadcaster for all interfaces is not updated";
+    EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
+      << "Execute without errors to write value";
+  }
+
+  // Recover hardware and activate again all controllers
+  {
+    ASSERT_EQ(
+      cm_->resource_manager_->set_component_state(
+        ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active),
+      hardware_interface::return_type::OK);
+    ASSERT_EQ(
+      cm_->resource_manager_->set_component_state(
+        ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active),
+      hardware_interface::return_type::OK);
+    auto status_map = cm_->resource_manager_->get_components_status();
+    ASSERT_EQ(
+      status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+    ASSERT_EQ(
+      status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+
+    auto previous_counter_lower = test_controller_actuator->internal_counter;
+    auto previous_counter = test_controller_system->internal_counter;
+    auto previous_counter_higher = test_broadcaster_sensor->internal_counter;
+
+    switch_test_controllers(
+      {TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {},
+      strictness);
+
+    EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower);
+    EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher);
+    EXPECT_GT(test_controller_system->internal_counter, previous_counter);
+    EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher);
+    EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower);
+    EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher);
+    EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher);
+  }
 }
 
 TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error)
@@ -745,6 +835,209 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e
   }
 }
 
+TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_deactivate)
+{
+  auto strictness = GetParam().strictness;
+  SetupAndConfigureControllers(strictness);
+
+  rclcpp_lifecycle::State state_active(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+    hardware_interface::lifecycle_state_names::ACTIVE);
+
+  {
+    EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
+    EXPECT_GE(test_controller_actuator->internal_counter, 1u)
+      << "Controller is started at the end of update";
+    EXPECT_GE(test_controller_system->internal_counter, 1u)
+      << "Controller is started at the end of update";
+    EXPECT_GE(test_broadcaster_all->internal_counter, 1u)
+      << "Controller is started at the end of update";
+    EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u)
+      << "Controller is started at the end of update";
+  }
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+    test_controller_actuator->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+    test_controller_system->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+    test_broadcaster_all->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+    test_broadcaster_sensor->get_lifecycle_state().id());
+
+  // Execute first time without any errors
+  {
+    auto new_counter = test_controller_actuator->internal_counter + 1;
+    EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
+    EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors";
+    EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors";
+    EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors";
+    EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors";
+  }
+
+  // Simulate deactivate in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command
+  // interface to WRITE_DEACTIVATE_VALUE
+  test_controller_actuator->set_first_command_interface_value_to =
+    test_constants::WRITE_DEACTIVATE_VALUE;
+  {
+    auto new_counter = test_controller_actuator->internal_counter + 1;
+    EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
+    EXPECT_EQ(test_controller_actuator->internal_counter, new_counter)
+      << "Execute without errors to write value";
+    EXPECT_EQ(test_controller_system->internal_counter, new_counter)
+      << "Execute without errors to write value";
+    EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter)
+      << "Execute without errors to write value";
+    EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
+      << "Execute without errors to write value";
+  }
+
+  {
+    auto previous_counter = test_controller_actuator->internal_counter;
+    auto new_counter = test_controller_system->internal_counter + 1;
+
+    // here happens deactivate in hardware and
+    // "actuator controller" is deactivated but "broadcaster all" should stay active
+    EXPECT_NO_THROW(cm_->write(time_, PERIOD));
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_controller_actuator->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      test_controller_system->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      test_broadcaster_all->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      test_broadcaster_sensor->get_lifecycle_state().id());
+
+    EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
+    EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter)
+      << "Execute without errors to write value";
+    EXPECT_EQ(test_controller_system->internal_counter, new_counter)
+      << "Execute without errors to write value";
+    EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter)
+      << "Execute without errors to write value";
+    EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
+      << "Execute without errors to write value";
+  }
+
+  // Recover hardware and activate again all controllers
+  {
+    ASSERT_EQ(
+      cm_->resource_manager_->set_component_state(
+        ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active),
+      hardware_interface::return_type::OK);
+    auto status_map = cm_->resource_manager_->get_components_status();
+    ASSERT_EQ(
+      status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+
+    auto previous_counter_lower = test_controller_actuator->internal_counter;
+    auto previous_counter_higher = test_controller_system->internal_counter;
+
+    switch_test_controllers({TEST_CONTROLLER_ACTUATOR_NAME}, {}, strictness);
+
+    EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower);
+    EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher);
+    EXPECT_GT(test_controller_system->internal_counter, previous_counter_higher);
+    EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_higher);
+    EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher);
+  }
+
+  // Simulate deactivate in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
+  // by setting first command interface to WRITE_DEACTIVATE_VALUE
+  test_controller_actuator->set_first_command_interface_value_to =
+    test_constants::WRITE_DEACTIVATE_VALUE;
+  test_controller_system->set_first_command_interface_value_to =
+    test_constants::WRITE_DEACTIVATE_VALUE;
+  {
+    auto previous_counter_lower = test_controller_actuator->internal_counter + 1;
+    auto previous_counter_higher = test_controller_system->internal_counter + 1;
+
+    EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
+
+    EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower)
+      << "Execute without errors to write value";
+    EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher)
+      << "Execute without errors to write value";
+    EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_higher)
+      << "Execute without errors to write value";
+    EXPECT_EQ(test_broadcaster_sensor->internal_counter, previous_counter_higher)
+      << "Execute without errors to write value";
+  }
+
+  {
+    auto previous_counter_lower = test_controller_actuator->internal_counter;
+    auto previous_counter_higher = test_controller_system->internal_counter;
+    auto new_counter = test_broadcaster_sensor->internal_counter + 1;
+
+    // here happens deactivate in hardware and
+    // "actuator controller" is deactivated but "broadcaster's" should stay active
+    EXPECT_NO_THROW(cm_->write(time_, PERIOD));
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_controller_actuator->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_controller_system->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      test_broadcaster_all->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      test_broadcaster_sensor->get_lifecycle_state().id());
+
+    EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
+    EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower)
+      << "Execute has write error and it is not updated";
+    EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher)
+      << "Execute has write error and it is not updated";
+    EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter)
+      << "Broadcaster for all interfaces is not updated";
+    EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
+      << "Execute without errors to write value";
+  }
+
+  // Recover hardware and activate again all controllers
+  {
+    ASSERT_EQ(
+      cm_->resource_manager_->set_component_state(
+        ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active),
+      hardware_interface::return_type::OK);
+    ASSERT_EQ(
+      cm_->resource_manager_->set_component_state(
+        ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active),
+      hardware_interface::return_type::OK);
+    auto status_map = cm_->resource_manager_->get_components_status();
+    ASSERT_EQ(
+      status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+    ASSERT_EQ(
+      status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+
+    auto previous_counter_lower = test_controller_actuator->internal_counter;
+    auto previous_counter = test_controller_system->internal_counter;
+    auto previous_counter_higher = test_broadcaster_sensor->internal_counter;
+
+    switch_test_controllers(
+      {TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME}, {}, strictness);
+
+    EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower);
+    EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher);
+    EXPECT_GT(test_controller_system->internal_counter, previous_counter);
+    EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher);
+    EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_higher);
+    EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher);
+  }
+}
+
 INSTANTIATE_TEST_SUITE_P(
   test_strict_best_effort, TestControllerManagerWithTestableCM,
   testing::Values(strict, best_effort));

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -45,7 +45,7 @@ class ControllerManager;
 
 struct HardwareReadWriteStatus
 {
-  bool ok;
+  return_type result;
   std::vector<std::string> failed_hardware_names;
 };