Fix spawner behaviour on failing controller activation or deactivation (#1941) (#1968)

---------

Co-authored-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>

Author: mergify[bot]

controller_manager/CMakeLists.txt

@@ -183,17 +183,13 @@ if(BUILD_TESTING)
   target_link_libraries(test_hardware_spawner ${PROJECT_NAME})
   ament_target_dependencies(test_hardware_spawner ros2_control_test_assets)
 
-  install(FILES test/test_controller_spawner_with_type.yaml
-    DESTINATION test)
-
-  install(FILES test/test_controller_spawner_with_basic_controllers.yaml
-    DESTINATION test)
-
-  install(FILES test/test_controller_overriding_parameters.yaml
-    DESTINATION test)
-
-  install(FILES test/test_controller_spawner_wildcard_entries.yaml
-    DESTINATION test)
+  install(FILES
+          test/test_controller_spawner_with_type.yaml
+          test/test_controller_spawner_with_basic_controllers.yaml
+          test/test_controller_overriding_parameters.yaml
+          test/test_controller_spawner_wildcard_entries.yaml
+          test/test_controller_spawner_with_interfaces.yaml
+          DESTINATION test)
 
   ament_add_gmock(
     test_hardware_management_srvs

controller_manager/controller_manager/spawner.py

@@ -265,7 +265,7 @@ def main(args=None):
                     )
                     if not ret.ok:
                         node.get_logger().error(
-                            bcolors.FAIL + "Failed to activate controller" + bcolors.ENDC
+                            f"{bcolors.FAIL}Failed to activate controller : {controller_name}{bcolors.ENDC}"
                         )
                         return 1
 
@@ -290,7 +290,7 @@ def main(args=None):
             )
             if not ret.ok:
                 node.get_logger().error(
-                    bcolors.FAIL + "Failed to activate the parsed controllers list" + bcolors.ENDC
+                    f"{bcolors.FAIL}Failed to activate the parsed controllers list : {controller_names}{bcolors.ENDC}"
                 )
                 return 1
 

controller_manager/src/controller_manager.cpp

@@ -1264,6 +1264,7 @@ controller_interface::return_type ControllerManager::switch_controller(
   to = controllers;
 
   // update the claimed interface controller info
+  auto switch_result = controller_interface::return_type::OK;
   for (auto & controller : to)
   {
     if (is_controller_active(controller.c))
@@ -1284,6 +1285,32 @@ controller_interface::return_type ControllerManager::switch_controller(
     {
       controller.info.claimed_interfaces.clear();
     }
+    if (
+      std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) !=
+      activate_request_.end())
+    {
+      if (!is_controller_active(controller.c))
+      {
+        RCLCPP_ERROR(
+          get_logger(), "Could not activate controller : '%s'", controller.info.name.c_str());
+        switch_result = controller_interface::return_type::ERROR;
+      }
+    }
+    /// @note The following is the case of the real controllers that are deactivated and doesn't
+    /// include the chained controllers that are deactivated and activated
+    if (
+      std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name) !=
+        deactivate_request_.end() &&
+      std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) ==
+        activate_request_.end())
+    {
+      if (is_controller_active(controller.c))
+      {
+        RCLCPP_ERROR(
+          get_logger(), "Could not deactivate controller : '%s'", controller.info.name.c_str());
+        switch_result = controller_interface::return_type::ERROR;
+      }
+    }
   }
 
   // switch lists
@@ -1293,8 +1320,10 @@ controller_interface::return_type ControllerManager::switch_controller(
 
   clear_requests();
 
-  RCLCPP_DEBUG(get_logger(), "Successfully switched controllers");
-  return controller_interface::return_type::OK;
+  RCLCPP_DEBUG_EXPRESSION(
+    get_logger(), switch_result == controller_interface::return_type::OK,
+    "Successfully switched controllers");
+  return switch_result;
 }
 
 controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_controller_impl(
@@ -1518,6 +1547,7 @@ void ControllerManager::activate_controllers()
           get_logger(),
           "Resource conflict for controller '%s'. Command interface '%s' is already claimed.",
           controller_name.c_str(), command_interface.c_str());
+        command_loans.clear();
         assignment_successful = false;
         break;
       }
@@ -1529,6 +1559,7 @@ void ControllerManager::activate_controllers()
       {
         RCLCPP_ERROR(
           get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
+        command_loans.clear();
         assignment_successful = false;
         break;
       }

controller_manager/test/test_controller/test_controller.cpp

@@ -64,7 +64,7 @@ controller_interface::return_type TestController::update(
 
   for (size_t i = 0; i < command_interfaces_.size(); ++i)
   {
-    RCLCPP_INFO(
+    RCLCPP_DEBUG(
       get_node()->get_logger(), "Setting value of command interface '%s' to %f",
       command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]);
     command_interfaces_[i].set_value(external_commands_for_testing_[i]);
@@ -77,6 +77,36 @@ CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; }
 
 CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  auto ctrl_node = get_node();
+  if (!ctrl_node->has_parameter("command_interfaces"))
+  {
+    ctrl_node->declare_parameter("command_interfaces", std::vector<std::string>({}));
+  }
+  if (!ctrl_node->has_parameter("state_interfaces"))
+  {
+    ctrl_node->declare_parameter("state_interfaces", std::vector<std::string>({}));
+  }
+  const std::vector<std::string> command_interfaces =
+    ctrl_node->get_parameter("command_interfaces").as_string_array();
+  const std::vector<std::string> state_interfaces =
+    ctrl_node->get_parameter("state_interfaces").as_string_array();
+  if (!command_interfaces.empty() || !state_interfaces.empty())
+  {
+    cmd_iface_cfg_.names.clear();
+    state_iface_cfg_.names.clear();
+    for (const auto & cmd_itf : command_interfaces)
+    {
+      cmd_iface_cfg_.names.push_back(cmd_itf);
+    }
+    cmd_iface_cfg_.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+    external_commands_for_testing_.resize(command_interfaces.size(), 0.0);
+    for (const auto & state_itf : state_interfaces)
+    {
+      state_iface_cfg_.names.push_back(state_itf);
+    }
+    state_iface_cfg_.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  }
+
   return CallbackReturn::SUCCESS;
 }
 

controller_manager/test/test_controller_spawner_with_interfaces.yaml

@@ -0,0 +1,25 @@
+ctrl_with_joint2_command_interface:
+  ros__parameters:
+    type: "controller_manager/test_controller"
+    command_interfaces:
+      - "joint2/velocity"
+
+ctrl_with_joint1_command_interface:
+  ros__parameters:
+    type: "controller_manager/test_controller"
+    command_interfaces:
+      - "joint1/position"
+
+ctrl_with_joint1_and_joint2_command_interfaces:
+  ros__parameters:
+    type: "controller_manager/test_controller"
+    command_interfaces:
+      - "joint1/position"
+      - "joint2/velocity"
+
+ctrl_with_state_interfaces:
+  ros__parameters:
+    type: "controller_manager/test_controller"
+    state_interfaces:
+      - "joint1/position"
+      - "joint2/position"

controller_manager/test/test_release_interfaces.cpp

@@ -86,7 +86,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
     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::OK, switch_future.get());
+    EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
     ASSERT_EQ(
       lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
       abstract_test_controller1.c->get_state().id());
@@ -190,7 +190,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
     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::OK, switch_future.get());
+    EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
     ASSERT_EQ(
       lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
       abstract_test_controller1.c->get_state().id());

controller_manager/test/test_spawner_unspawner.cpp

@@ -411,6 +411,106 @@ TEST_F(TestLoadController, spawner_test_with_wildcard_entries_with_no_ctrl_name)
   verify_ctrl_parameter(wildcard_ctrl_3.c->get_node(), true);
 }
 
+TEST_F(TestLoadController, spawner_test_failed_activation_of_controllers)
+{
+  const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+                                     "/test/test_controller_spawner_with_interfaces.yaml";
+
+  ControllerManagerRunner cm_runner(this);
+  // Provide controller type via the parsed file
+  EXPECT_EQ(
+    call_spawner(
+      "ctrl_with_joint1_command_interface ctrl_with_joint2_command_interface -c "
+      "test_controller_manager "
+      "--controller-manager-timeout 1.0 "
+      "-p " +
+      test_file_path),
+    0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
+
+  auto ctrl_with_joint2_command_interface = cm_->get_loaded_controllers()[1];
+  ASSERT_EQ(ctrl_with_joint2_command_interface.info.name, "ctrl_with_joint2_command_interface");
+  ASSERT_EQ(
+    ctrl_with_joint2_command_interface.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+  EXPECT_EQ(
+    ctrl_with_joint2_command_interface.c->get_state().id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+  ASSERT_EQ(
+    ctrl_with_joint2_command_interface.c->command_interface_configuration().names.size(), 1ul);
+  ASSERT_THAT(
+    ctrl_with_joint2_command_interface.c->command_interface_configuration().names,
+    std::vector<std::string>({"joint2/velocity"}));
+
+  auto ctrl_with_joint1_command_interface = cm_->get_loaded_controllers()[0];
+  ASSERT_EQ(ctrl_with_joint1_command_interface.info.name, "ctrl_with_joint1_command_interface");
+  ASSERT_EQ(
+    ctrl_with_joint1_command_interface.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+  EXPECT_EQ(
+    ctrl_with_joint1_command_interface.c->get_state().id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+  ASSERT_EQ(
+    ctrl_with_joint1_command_interface.c->command_interface_configuration().names.size(), 1ul);
+  ASSERT_THAT(
+    ctrl_with_joint1_command_interface.c->command_interface_configuration().names,
+    std::vector<std::string>({"joint1/position"}));
+
+  EXPECT_EQ(
+    call_spawner(
+      "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager "
+      "--controller-manager-timeout 1.0 "
+      "-p " +
+      test_file_path),
+    256)
+    << "Should fail as the ctrl_with_joint1_command_interface and "
+       "ctrl_with_joint2_command_interface are active";
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
+
+  auto ctrl_with_joint1_and_joint2_command_interfaces = cm_->get_loaded_controllers()[2];
+  ASSERT_EQ(
+    ctrl_with_joint1_and_joint2_command_interfaces.info.name,
+    "ctrl_with_joint1_and_joint2_command_interfaces");
+  ASSERT_EQ(
+    ctrl_with_joint1_and_joint2_command_interfaces.info.type,
+    test_controller::TEST_CONTROLLER_CLASS_NAME);
+  ASSERT_EQ(
+    ctrl_with_joint1_and_joint2_command_interfaces.c->get_state().id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+  ASSERT_EQ(
+    ctrl_with_joint1_and_joint2_command_interfaces.c->command_interface_configuration()
+      .names.size(),
+    2ul);
+  ASSERT_THAT(
+    ctrl_with_joint1_and_joint2_command_interfaces.c->command_interface_configuration().names,
+    std::vector<std::string>({"joint1/position", "joint2/velocity"}));
+
+  EXPECT_EQ(call_unspawner("ctrl_with_joint1_command_interface -c test_controller_manager"), 0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
+  EXPECT_EQ(
+    call_spawner(
+      "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager "
+      "--controller-manager-timeout 1.0 "
+      "-p " +
+      test_file_path),
+    256)
+    << "Should fail as the ctrl_with_joint2_command_interface is still active";
+
+  EXPECT_EQ(call_unspawner("ctrl_with_joint2_command_interface -c test_controller_manager"), 0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
+  EXPECT_EQ(
+    call_spawner(
+      "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager "
+      "--controller-manager-timeout 1.0 "
+      "-p " +
+      test_file_path),
+    0)
+    << "Should pass as the ctrl_with_joint1_command_interface and "
+       "ctrl_with_joint2_command_interface are inactive";
+}
+
 TEST_F(TestLoadController, unload_on_kill)
 {
   // Launch spawner with unload on kill