Fix spawner behaviour on failing controller activation or deactivation (

#1941)

---------

Co-authored-by: Christoph Fröhlich <[email protected]>
(cherry picked from commit 038a05f)

# Conflicts:
#	controller_manager/CMakeLists.txt
#	controller_manager/src/controller_manager.cpp
#	controller_manager/test/test_controller/test_controller.cpp

controller_manager/CMakeLists.txt

@@ -179,6 +179,7 @@ if(BUILD_TESTING)
     test/test_hardware_spawner.cpp
     TIMEOUT 120
   )
+<<<<<<< HEAD
   target_include_directories(test_hardware_spawner PRIVATE include)
   target_link_libraries(test_hardware_spawner ${PROJECT_NAME})
   ament_target_dependencies(test_hardware_spawner ros2_control_test_assets)
@@ -194,6 +195,21 @@ if(BUILD_TESTING)
 
   install(FILES test/test_controller_spawner_wildcard_entries.yaml
     DESTINATION test)
+=======
+  target_link_libraries(test_hardware_spawner
+    controller_manager
+    test_controller
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
+  install(FILES
+          test/test_controller_spawner_with_type.yaml
+          test/test_controller_overriding_parameters.yaml
+          test/test_controller_spawner_with_fallback_controllers.yaml
+          test/test_controller_spawner_wildcard_entries.yaml
+          test/test_controller_spawner_with_interfaces.yaml
+          DESTINATION test)
+>>>>>>> 038a05f (Fix spawner behaviour on failing controller activation or deactivation (#1941))
 
   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;
       }
@@ -1528,7 +1558,15 @@ void ControllerManager::activate_controllers()
       catch (const std::exception & e)
       {
         RCLCPP_ERROR(
+<<<<<<< HEAD
           get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
+=======
+          get_logger(),
+          "Caught exception of type : %s while claiming the command interfaces. Can't activate "
+          "controller '%s': %s",
+          typeid(e).name(), controller_name.c_str(), e.what());
+        command_loans.clear();
+>>>>>>> 038a05f (Fix spawner behaviour on failing controller activation or deactivation (#1941))
         assignment_successful = false;
         break;
       }
@@ -1579,11 +1617,26 @@ void ControllerManager::activate_controllers()
     if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
     {
       RCLCPP_ERROR(
+<<<<<<< HEAD
         get_logger(),
         "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).",
         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);
+=======
+        get_logger(), "Caught exception of type : %s while activating the controller '%s': %s",
+        typeid(e).name(), controller_name.c_str(), e.what());
+      controller->release_interfaces();
+      continue;
+    }
+    catch (...)
+    {
+      RCLCPP_ERROR(
+        get_logger(), "Caught unknown exception while activating the controller '%s'",
+        controller_name.c_str());
+      controller->release_interfaces();
+      continue;
+>>>>>>> 038a05f (Fix spawner behaviour on failing controller activation or deactivation (#1941))
     }
 
     // if it is a chainable controller, make the reference interfaces available on activation

controller_manager/test/test_controller/test_controller.cpp

@@ -64,10 +64,34 @@ controller_interface::return_type TestController::update(
 
   for (size_t i = 0; i < command_interfaces_.size(); ++i)
   {
+<<<<<<< HEAD
     RCLCPP_INFO(
       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]);
+=======
+    command_interfaces_[0].set_value(set_first_command_interface_value_to);
+    // reset to be easier to test
+    set_first_command_interface_value_to = std::numeric_limits<double>::quiet_NaN();
+  }
+  else
+  {
+    for (size_t i = 0; i < command_interfaces_.size(); ++i)
+    {
+      if (!std::isfinite(external_commands_for_testing_[i]))
+      {
+        RCLCPP_ERROR(
+          get_node()->get_logger(),
+          "External command value for command interface '%s' is not finite",
+          command_interfaces_[i].get_name().c_str());
+        return controller_interface::return_type::ERROR;
+      }
+      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]);
+    }
+>>>>>>> 038a05f (Fix spawner behaviour on failing controller activation or deactivation (#1941))
   }
 
   return controller_interface::return_type::OK;
@@ -77,6 +101,51 @@ CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; }
 
 CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
 {
+<<<<<<< HEAD
+=======
+  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;
+  }
+
+  const std::string service_name = get_node()->get_name() + std::string("/set_bool");
+  service_ = get_node()->create_service<example_interfaces::srv::SetBool>(
+    service_name,
+    [this](
+      const std::shared_ptr<example_interfaces::srv::SetBool::Request> request,
+      std::shared_ptr<example_interfaces::srv::SetBool::Response> response)
+    {
+      RCLCPP_INFO_STREAM(
+        get_node()->get_logger(), "Setting response to " << std::boolalpha << request->data);
+      response->success = request->data;
+    });
+
+>>>>>>> 038a05f (Fix spawner behaviour on failing controller activation or deactivation (#1941))
   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()[0];
+  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_lifecycle_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()[1];
+  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_lifecycle_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()[0];
+  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_lifecycle_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