Add fallback controllers list to the ControllerInfo (#1503)

controller_manager/CMakeLists.txt

@@ -189,6 +189,10 @@ if(BUILD_TESTING)
     test_controller
     ros2_control_test_assets::ros2_control_test_assets
   )
+
+  install(FILES test/test_controller_spawner_with_fallback_controllers.yaml
+    DESTINATION test)
+
   install(FILES test/test_controller_spawner_with_type.yaml
   DESTINATION test)
 

controller_manager/controller_manager/spawner.py

@@ -210,6 +210,14 @@ def main(args=None):
         action="store_true",
         required=False,
     )
+    parser.add_argument(
+        "--fallback_controllers",
+        help="Fallback controllers list are activated as a fallback strategy when the"
+        " spawned controllers fail. When the argument is provided, it takes precedence over"
+        " the fallback_controllers list in the param file",
+        default=None,
+        nargs="+",
+    )
 
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
@@ -249,6 +257,7 @@ def main(args=None):
             return 1
 
         for controller_name in controller_names:
+            fallback_controllers = args.fallback_controllers
             controller_type = args.controller_type
             prefixed_controller_name = controller_name
             if controller_namespace:
@@ -329,6 +338,43 @@ def main(args=None):
                         )
                         return 1
 
+                if not fallback_controllers and param_file:
+                    fallback_controllers = get_parameter_from_param_file(
+                        controller_name, param_file, "fallback_controllers"
+                    )
+
+                if fallback_controllers:
+                    parameter = Parameter()
+                    parameter.name = prefixed_controller_name + ".fallback_controllers"
+                    parameter.value = get_parameter_value(string_value=str(fallback_controllers))
+
+                    response = call_set_parameters(
+                        node=node, node_name=controller_manager_name, parameters=[parameter]
+                    )
+                    assert len(response.results) == 1
+                    result = response.results[0]
+                    if result.successful:
+                        node.get_logger().info(
+                            bcolors.OKCYAN
+                            + 'Setting fallback_controllers to ["'
+                            + ",".join(fallback_controllers)
+                            + '"] for '
+                            + bcolors.BOLD
+                            + prefixed_controller_name
+                            + bcolors.ENDC
+                        )
+                    else:
+                        node.get_logger().fatal(
+                            bcolors.FAIL
+                            + 'Could not set fallback_controllers to ["'
+                            + ",".join(fallback_controllers)
+                            + '"] for '
+                            + bcolors.BOLD
+                            + prefixed_controller_name
+                            + bcolors.ENDC
+                        )
+                        return 1
+
                 ret = load_controller(node, controller_manager_name, controller_name)
                 if not ret.ok:
                     node.get_logger().fatal(

controller_manager/src/controller_manager.cpp

@@ -482,6 +482,17 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
     controller_spec.info.parameters_file = parameters_file;
   }
 
+  const std::string fallback_ctrl_param = controller_name + ".fallback_controllers";
+  std::vector<std::string> fallback_controllers;
+  if (!has_parameter(fallback_ctrl_param))
+  {
+    declare_parameter(fallback_ctrl_param, rclcpp::ParameterType::PARAMETER_STRING_ARRAY);
+  }
+  if (get_parameter(fallback_ctrl_param, fallback_controllers) && !fallback_controllers.empty())
+  {
+    controller_spec.info.fallback_controllers_names = fallback_controllers;
+  }
+
   return add_controller_impl(controller_spec);
 }
 

controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml

@@ -0,0 +1,13 @@
+ctrl_1:
+  ros__parameters:
+    joint_names: ["joint1"]
+
+ctrl_2:
+  ros__parameters:
+    joint_names: ["joint2"]
+    fallback_controllers: ["ctrl_6", "ctrl_7", "ctrl_8"]
+
+ctrl_3:
+  ros__parameters:
+    joint_names: ["joint3"]
+    fallback_controllers: ["ctrl_9"]

controller_manager/test/test_spawner_unspawner.cpp

@@ -324,3 +324,58 @@ TEST_F(TestLoadController, unload_on_kill)
 
   ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul);
 }
+
+TEST_F(TestLoadController, spawner_test_fallback_controllers)
+{
+  const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+                                     "/test/test_controller_spawner_with_fallback_controllers.yaml";
+
+  cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+  cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+  cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+
+  ControllerManagerRunner cm_runner(this);
+  EXPECT_EQ(
+    call_spawner(
+      "ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 "
+      "-p " +
+      test_file_path),
+    0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
+  {
+    auto ctrl_1 = cm_->get_loaded_controllers()[0];
+    ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
+    ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_THAT(
+      ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
+    ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  }
+
+  // Try to spawn now the controller with fallback controllers inside the yaml
+  EXPECT_EQ(
+    call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
+  {
+    auto ctrl_1 = cm_->get_loaded_controllers()[0];
+    ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
+    ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_THAT(
+      ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
+    ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+    auto ctrl_2 = cm_->get_loaded_controllers()[1];
+    ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
+    ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_THAT(
+      ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8"));
+    ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+    auto ctrl_3 = cm_->get_loaded_controllers()[2];
+    ASSERT_EQ(ctrl_3.info.name, "ctrl_3");
+    ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9"));
+    ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  }
+}

hardware_interface/include/hardware_interface/controller_info.hpp

@@ -38,6 +38,9 @@ struct ControllerInfo
 
   /// List of claimed interfaces by the controller.
   std::vector<std::string> claimed_interfaces;
+
+  /// List of fallback controller names to be activated if this controller fails.
+  std::vector<std::string> fallback_controllers_names;
 };
 
 }  // namespace hardware_interface