Merge branch 'master' into fix/exclusive_hw_interface_switching

controller_manager/CMakeLists.txt

@@ -205,6 +205,12 @@ if(BUILD_TESTING)
     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)
+
   ament_add_gmock(test_hardware_management_srvs
     test/test_hardware_management_srvs.cpp
   )

controller_manager/controller_manager/launch_utils.py

@@ -12,6 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import warnings
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration, PythonExpression
@@ -61,6 +62,12 @@ def generate_load_controller_launch_description(
     ]
 
     if controller_type:
+        warnings.warn(
+            "The 'controller_type' argument is deprecated and will be removed in future releases."
+            " Declare the controller type parameter in the param file instead.",
+            DeprecationWarning,
+            stacklevel=2,
+        )
         spawner_arguments += ["--controller-type", controller_type]
 
     if controller_params_file:

controller_manager/controller_manager/spawner.py

@@ -19,6 +19,7 @@
 import sys
 import time
 import warnings
+import yaml
 
 from controller_manager import (
     configure_controller,
@@ -135,6 +136,21 @@ def is_controller_loaded(node, controller_manager, controller_name):
     return any(c.name == controller_name for c in controllers)
 
 
+def get_parameter_from_param_file(controller_name, parameter_file, parameter_name):
+    with open(parameter_file) as f:
+        parameters = yaml.safe_load(f)
+        if controller_name in parameters:
+            value = parameters[controller_name]
+            if not isinstance(value, dict) or "ros__parameters" not in value:
+                raise RuntimeError(
+                    f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {controller_name}"
+                )
+            if parameter_name in parameters[controller_name]["ros__parameters"]:
+                return parameters[controller_name]["ros__parameters"][parameter_name]
+            else:
+                return None
+
+
 def main(args=None):
     rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
     parser = argparse.ArgumentParser()
@@ -170,7 +186,7 @@ def main(args=None):
     parser.add_argument(
         "-t",
         "--controller-type",
-        help="If not provided it should exist in the controller manager namespace",
+        help="If not provided it should exist in the controller manager namespace (deprecated)",
         default=None,
         required=False,
     )
@@ -194,16 +210,31 @@ 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)
     controller_names = args.controller_names
     controller_manager_name = args.controller_manager
     controller_namespace = args.namespace
     param_file = args.param_file
-    controller_type = args.controller_type
     controller_manager_timeout = args.controller_manager_timeout
 
+    if args.controller_type:
+        warnings.filterwarnings("always")
+        warnings.warn(
+            "The '--controller-type' argument is deprecated and will be removed in future releases."
+            " Declare the controller type parameter in the param file instead.",
+            DeprecationWarning,
+        )
+
     if param_file and not os.path.isfile(param_file):
         raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
 
@@ -226,6 +257,8 @@ 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:
                 prefixed_controller_name = controller_namespace + "/" + controller_name
@@ -237,6 +270,10 @@ def main(args=None):
                     + bcolors.ENDC
                 )
             else:
+                if not controller_type and param_file:
+                    controller_type = get_parameter_from_param_file(
+                        controller_name, param_file, "type"
+                    )
                 if controller_type:
                     parameter = Parameter()
                     parameter.name = prefixed_controller_name + ".type"
@@ -301,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_controller_spawner_with_type.yaml

@@ -0,0 +1,13 @@
+ctrl_with_parameters_and_type:
+  ros__parameters:
+    type: "controller_manager/test_controller"
+    joint_names: ["joint0"]
+
+chainable_ctrl_with_parameters_and_type:
+  ros__parameters:
+    type: "controller_manager/test_chainable_controller"
+    joint_names: ["joint1"]
+
+ctrl_with_parameters_and_no_type:
+  ros__parameters:
+    joint_names: ["joint2"]

controller_manager/test/test_spawner_unspawner.cpp

@@ -23,6 +23,7 @@
 #include "controller_manager/controller_manager.hpp"
 #include "controller_manager_test_common.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "test_chainable_controller/test_chainable_controller.hpp"
 #include "test_controller/test_controller.hpp"
 
 using ::testing::_;
@@ -259,6 +260,56 @@ TEST_F(TestLoadController, spawner_test_type_in_arg)
   ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
 }
 
+TEST_F(TestLoadController, spawner_test_type_in_params_file)
+{
+  const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+                                     "/test/test_controller_spawner_with_type.yaml";
+
+  // Provide controller type via the parsed file
+  EXPECT_EQ(
+    call_spawner(
+      "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
+      "test_controller_manager -p " +
+      test_file_path),
+    0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
+
+  auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0];
+  ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type");
+  ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+  ASSERT_EQ(
+    ctrl_with_parameters_and_type.c->get_state().id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+  auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
+  ASSERT_EQ(
+    chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type");
+  ASSERT_EQ(
+    chain_ctrl_with_parameters_and_type.info.type,
+    test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+  ASSERT_EQ(
+    chain_ctrl_with_parameters_and_type.c->get_state().id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+  EXPECT_EQ(
+    call_spawner(
+      "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path),
+    256);
+  // Will still be same as the current call will fail
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
+
+  auto ctrl_1 = cm_->get_loaded_controllers()[0];
+  ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type");
+  ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+  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, "chainable_ctrl_with_parameters_and_type");
+  ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+  ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+}
+
 TEST_F(TestLoadController, unload_on_kill)
 {
   // Launch spawner with unload on kill
@@ -273,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/CMakeLists.txt

@@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rcutils
   TinyXML2
   tinyxml2_vendor
+  joint_limits
   urdf
 )
 

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

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -19,6 +19,8 @@
 #include <unordered_map>
 #include <vector>
 
+#include "joint_limits/joint_limits.hpp"
+
 namespace hardware_interface
 {
 /**
@@ -42,6 +44,8 @@ struct InterfaceInfo
   std::string data_type;
   /// (Optional) If the handle is an array, the size of the array. Used by GPIOs.
   int size;
+  /// (Optional) enable or disable the limits for the command interfaces
+  bool enable_limits;
 };
 
 /// @brief This structure stores information about a joint that is mimicking another joint
@@ -163,6 +167,10 @@ struct HardwareInfo
    * The XML contents prior to parsing
    */
   std::string original_xml;
+  /**
+   * The URDF parsed limits of the hardware components joint command interfaces
+   */
+  std::unordered_map<std::string, joint_limits::JointLimits> limits;
 };
 
 }  // namespace hardware_interface