Deactivate the controller chain upon failed group activation

controller_manager/include/controller_manager/controller_manager.hpp

@@ -280,10 +280,11 @@ class ControllerManager : public rclcpp::Node
    *
    * \param[in] rt_controller_list controllers in the real-time list.
    * \param[in] controllers_to_activate names of the controller that have to be activated.
+   * \param[in] strictness level of strictness for activation.
    */
   void activate_controllers(
     const std::vector<ControllerSpec> & rt_controller_list,
-    const std::vector<std::string> & controllers_to_activate);
+    const std::vector<std::string> & controllers_to_activate, int strictness);
 
   void list_controllers_srv_cb(
     const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,

controller_manager/src/controller_manager.cpp

@@ -1307,6 +1307,10 @@ controller_interface::return_type ControllerManager::configure_controller(
     resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces);
     resource_manager_->import_controller_exported_state_interfaces(
       controller_name, state_interfaces);
+    // make all the exported interfaces of the controller unavailable
+    controller->set_chained_mode(false);
+    resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name);
+    resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
   }
 
   // let's update the list of following and preceding controllers
@@ -2226,9 +2230,10 @@ void ControllerManager::switch_chained_mode(
 
 void ControllerManager::activate_controllers(
   const std::vector<ControllerSpec> & rt_controller_list,
-  const std::vector<std::string> & controllers_to_activate)
+  const std::vector<std::string> & controllers_to_activate, int strictness)
 {
   std::vector<std::string> failed_controllers_command_interfaces;
+  bool is_successful = true;
   for (const auto & controller_name : controllers_to_activate)
   {
     auto found_it = std::find_if(
@@ -2295,6 +2300,7 @@ void ControllerManager::activate_controllers(
     // something went wrong during command interfaces, go skip the controller
     if (!assignment_successful)
     {
+      is_successful = false;
       continue;
     }
 
@@ -2333,6 +2339,7 @@ void ControllerManager::activate_controllers(
     // something went wrong during state interfaces, go skip the controller
     if (!assignment_successful)
     {
+      is_successful = false;
       continue;
     }
     controller->assign_interfaces(std::move(command_loans), std::move(state_loans));
@@ -2365,10 +2372,15 @@ void ControllerManager::activate_controllers(
         controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(),
         hardware_interface::lifecycle_state_names::ACTIVE,
         lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+      is_successful = false;
       controller->release_interfaces();
-      failed_controllers_command_interfaces.insert(
-        failed_controllers_command_interfaces.end(), command_interface_names.begin(),
-        command_interface_names.end());
+      ros2_control::add_items(failed_controllers_command_interfaces, command_interface_names);
+      if (controller->is_chainable())
+      {
+        // make all the exported interfaces of the controller unavailable
+        resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name);
+        resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
+      }
       continue;
     }
 
@@ -2380,6 +2392,27 @@ void ControllerManager::activate_controllers(
       resource_manager_->make_controller_reference_interfaces_available(controller_name);
     }
   }
+  if (
+    !is_successful && strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
+  {
+    RCLCPP_ERROR(
+      get_logger(),
+      "At least one controller failed to be activated. Releasing all interfaces and stopping all "
+      "activated controllers because the switch strictness is set to STRICT.");
+    // deactivate all controllers that were activated in this switch
+    deactivate_controllers(rt_controller_list, controllers_to_activate);
+    if (
+      !resource_manager_->prepare_command_mode_switch(
+        {}, switch_params_.activate_command_interface_request) ||
+      !resource_manager_->perform_command_mode_switch(
+        {}, switch_params_.activate_command_interface_request))
+    {
+      RCLCPP_ERROR(
+        get_logger(),
+        "Error switching back the interfaces in the hardware when the controller activation "
+        "failed.");
+    }
+  }
   // Now prepare and perform the stop interface switching as this is needed for exclusive
   // interfaces
   if (
@@ -2916,7 +2949,8 @@ void ControllerManager::manage_switch()
 
   // activate controllers once the switch is fully complete
   const auto act_start_time = std::chrono::steady_clock::now();
-  activate_controllers(rt_controller_list, switch_params_.activate_request);
+  activate_controllers(
+    rt_controller_list, switch_params_.activate_request, switch_params_.strictness);
   execution_time_.activation_time =
     std::chrono::duration<double, std::micro>(std::chrono::steady_clock::now() - act_start_time)
       .count();
@@ -3116,7 +3150,9 @@ controller_interface::return_type ControllerManager::update(
     deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list);
     if (!rt_buffer_.fallback_controllers_list.empty())
     {
-      activate_controllers(rt_controller_list, rt_buffer_.fallback_controllers_list);
+      activate_controllers(
+        rt_controller_list, rt_buffer_.fallback_controllers_list,
+        controller_manager_msgs::srv::SwitchController::Request::STRICT);
     }
     // To publish the activity of the failing controllers and the fallback controllers
     publish_activity();

controller_manager/test/test_controller_manager.cpp

@@ -2280,3 +2280,180 @@ TEST_F(
       test_chainable_controller->get_lifecycle_state().id());
   }
 }
+
+TEST_F(
+  TestControllerManagerChainableControllerFailedActivation,
+  test_chainable_controllers_failed_activation_stops_all_list)
+{
+  const std::string test_chainable_controller_2_name = "test_chainable_controller_2";
+
+  const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
+  controller_interface::InterfaceConfiguration cmd_itfs_cfg;
+  controller_interface::InterfaceConfiguration itfs_cfg;
+  cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  // controller 1
+  auto test_chainable_controller =
+    std::make_shared<test_chainable_controller::TestChainableController>();
+  cmd_itfs_cfg.names = {"joint1/position"};
+  itfs_cfg.names = {"joint2/velocity"};
+  test_chainable_controller->set_command_interface_configuration(cmd_itfs_cfg);
+  test_chainable_controller->set_state_interface_configuration(itfs_cfg);
+  test_chainable_controller->set_reference_interface_names({"modified_joint1/position"});
+  test_chainable_controller->set_exported_state_interface_names({"modified_joint2/velocity"});
+  test_chainable_controller->fail_on_activate = false;
+
+  auto test_chainable_controller_2 =
+    std::make_shared<test_chainable_controller::TestChainableController>();
+  cmd_itfs_cfg.names = {"joint2/velocity"};
+  itfs_cfg.names = {"joint2/velocity"};
+  test_chainable_controller_2->set_command_interface_configuration(cmd_itfs_cfg);
+  test_chainable_controller_2->set_state_interface_configuration(itfs_cfg);
+  test_chainable_controller_2->set_reference_interface_names({"modified_joint2/position"});
+  test_chainable_controller_2->set_exported_state_interface_names({"modified_joint2/velocity"});
+  test_chainable_controller_2->fail_on_activate = false;
+
+  auto test_controller = std::make_shared<test_controller::TestController>();
+  cmd_itfs_cfg.names = {
+    std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/modified_joint1/position"};
+  test_controller->set_command_interface_configuration(cmd_itfs_cfg);
+  test_controller->set_state_interface_configuration(itfs_cfg);
+
+  cm_->add_controller(
+    test_chainable_controller, test_chainable_controller::TEST_CONTROLLER_NAME,
+    test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+  cm_->add_controller(
+    test_chainable_controller_2, test_chainable_controller_2_name,
+    test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+  cm_->add_controller(
+    test_controller, test_controller::TEST_CONTROLLER_NAME,
+    test_controller::TEST_CONTROLLER_CLASS_NAME);
+
+  EXPECT_EQ(3u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(2, test_chainable_controller.use_count());
+  EXPECT_EQ(2, test_chainable_controller_2.use_count());
+  EXPECT_EQ(2, test_controller.use_count());
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_chainable_controller->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_chainable_controller_2->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_controller->get_lifecycle_state().id());
+
+  // configure controllers
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME));
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->configure_controller(test_chainable_controller_2_name));
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME));
+  }
+
+  std::vector<std::string> start_controllers = {
+    test_chainable_controller::TEST_CONTROLLER_NAME, test_chainable_controller_2_name,
+    test_controller::TEST_CONTROLLER_NAME};
+  std::vector<std::string> stop_controllers = {};
+  {
+    ControllerManagerRunner cm_runner(this);
+
+    auto switch_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0));
+    ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      test_chainable_controller->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      test_chainable_controller_2->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      test_controller->get_lifecycle_state().id());
+
+    // Now deactivate all the controllers
+    auto switch_future_2 = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      stop_controllers, start_controllers, strictness, true, rclcpp::Duration(0, 0));
+    ASSERT_EQ(std::future_status::ready, switch_future_2.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future_2.get());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_chainable_controller->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_chainable_controller_2->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_controller->get_lifecycle_state().id());
+  }
+
+  // Now, let's make the first controller fail on activation and see that all the controllers are
+  // set to inactive in strict mode
+  test_chainable_controller->fail_on_activate = true;
+  {
+    ControllerManagerRunner cm_runner(this);
+
+    auto switch_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0));
+    ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+    EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+      test_chainable_controller->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_chainable_controller_2->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_controller->get_lifecycle_state().id());
+  }
+
+  // Now, reconfigure and test with BEST_EFFORT strictness and see that the other two controllers
+  // are active
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME));
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_chainable_controller->get_lifecycle_state().id());
+
+    const auto best_effort_strictness =
+      controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
+    auto switch_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      start_controllers, stop_controllers, best_effort_strictness, true, rclcpp::Duration(0, 0));
+    ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+    EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+      test_chainable_controller->get_lifecycle_state().id());
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      test_chainable_controller_2->get_lifecycle_state().id());
+    // test controller needs to be inactive as it depends on the failed controller's interface
+    EXPECT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      test_controller->get_lifecycle_state().id());
+  }
+}

controller_manager/test/test_release_interfaces.cpp

@@ -176,7 +176,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
       abstract_test_controller2.c->get_lifecycle_state().id());
   }
 
-  {  // Test starting both controllers at the same time
+  {  // Test starting both controllers at the same time, with STRICT both should fail
     RCLCPP_INFO(
       cm_->get_logger(),
       "Starting both controllers at the same time (should notify about resource conflict)");
@@ -189,6 +189,27 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
       << "switch_controller should be blocking until next update cycle";
     ControllerManagerRunner cm_runner(this);
     EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      abstract_test_controller1.c->get_lifecycle_state().id());
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      abstract_test_controller2.c->get_lifecycle_state().id());
+  }
+
+  {  // Test starting both controllers at the same time, with BEST_EFFORT one should activate
+    RCLCPP_INFO(
+      cm_->get_logger(),
+      "Starting both controllers at the same time (should notify about resource conflict)");
+    std::vector<std::string> start_controllers = {controller_name1, controller_name2};
+    std::vector<std::string> stop_controllers = {};
+    auto switch_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      start_controllers, stop_controllers, BEST_EFFORT, true, rclcpp::Duration(0, 0));
+    ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
     ASSERT_EQ(
       lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
       abstract_test_controller1.c->get_lifecycle_state().id());

hardware_interface/include/hardware_interface/helpers.hpp

@@ -85,6 +85,20 @@ void add_item(std::vector<T> & vector, const T & item)
   }
 }
 
+/**
+ * @brief Add the items from one container to the other, if they are not already in it.
+ * @param vector The container to add the items to.
+ * @param items The container to add the items from.
+ */
+template <typename T>
+void add_items(std::vector<T> & vector, const std::vector<T> & items)
+{
+  for (const auto & item : items)
+  {
+    add_item(vector, item);
+  }
+}
+
 /**
  * @brief Remove the item from the container if it is in it.
  * @param container The container to remove the item from.