Fix the namespaced controller_manager spawner + added tests

controller_manager/CMakeLists.txt

@@ -185,6 +185,7 @@ if(BUILD_TESTING)
 
   ament_add_gmock(test_spawner_unspawner
     test/test_spawner_unspawner.cpp
+    TIMEOUT 120
   )
   target_link_libraries(test_spawner_unspawner
     controller_manager

controller_manager/controller_manager/spawner.py

@@ -136,17 +136,20 @@ 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):
+def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name):
     with open(parameter_file) as f:
+        namespaced_controller = (
+            controller_name if namespace == "/" else f"{namespace}/{controller_name}"
+        )
         parameters = yaml.safe_load(f)
-        if controller_name in parameters:
-            value = parameters[controller_name]
+        if namespaced_controller in parameters:
+            value = parameters[namespaced_controller]
             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}"
+                    f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}"
                 )
-            if parameter_name in parameters[controller_name]["ros__parameters"]:
-                return parameters[controller_name]["ros__parameters"][parameter_name]
+            if parameter_name in parameters[namespaced_controller]["ros__parameters"]:
+                return parameters[namespaced_controller]["ros__parameters"][parameter_name]
             else:
                 return None
 
@@ -170,7 +173,11 @@ def main(args=None):
         required=False,
     )
     parser.add_argument(
-        "-n", "--namespace", help="Namespace for the controller", default="", required=False
+        "-n",
+        "--namespace",
+        help="Namespace for the controller_manager and the controller(s) (deprecated)",
+        default=None,
+        required=False,
     )
     parser.add_argument(
         "--load-only",
@@ -217,7 +224,6 @@ def main(args=None):
     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_manager_timeout = args.controller_manager_timeout
 
@@ -226,9 +232,27 @@ def main(args=None):
 
     node = Node("spawner_" + controller_names[0])
 
+    if node.get_namespace() != "/" and args.namespace:
+        raise RuntimeError(
+            f"Setting namespace through both '--namespace {args.namespace}' arg and the ROS 2 standard way "
+            f"'--ros-args -r __ns:={node.get_namespace()}' is not allowed!"
+        )
+
+    if args.namespace:
+        warnings.filterwarnings("always")
+        warnings.warn(
+            "The '--namespace' argument is deprecated and will be removed in future releases."
+            " Use the ROS 2 standard way of setting the node namespacing using --ros-args -r __ns:=<namespace>",
+            DeprecationWarning,
+        )
+
+    spawner_namespace = args.namespace if args.namespace else node.get_namespace()
+
+    if not spawner_namespace.startswith("/"):
+        spawner_namespace = f"/{spawner_namespace}"
+
     if not controller_manager_name.startswith("/"):
-        spawner_namespace = node.get_namespace()
-        if spawner_namespace != "/":
+        if spawner_namespace and spawner_namespace != "/":
             controller_manager_name = f"{spawner_namespace}/{controller_manager_name}"
         else:
             controller_manager_name = f"/{controller_manager_name}"
@@ -244,11 +268,8 @@ def main(args=None):
 
         for controller_name in controller_names:
             fallback_controllers = args.fallback_controllers
-            prefixed_controller_name = controller_name
-            if controller_namespace:
-                prefixed_controller_name = controller_namespace + "/" + controller_name
 
-            if is_controller_loaded(node, controller_manager_name, prefixed_controller_name):
+            if is_controller_loaded(node, controller_manager_name, controller_name):
                 node.get_logger().warn(
                     bcolors.WARNING
                     + "Controller already loaded, skipping load_controller"
@@ -258,11 +279,13 @@ def main(args=None):
                 controller_type = (
                     None
                     if param_file is None
-                    else get_parameter_from_param_file(controller_name, param_file, "type")
+                    else get_parameter_from_param_file(
+                        controller_name, spawner_namespace, param_file, "type"
+                    )
                 )
                 if controller_type:
                     parameter = Parameter()
-                    parameter.name = prefixed_controller_name + ".type"
+                    parameter.name = controller_name + ".type"
                     parameter.value = get_parameter_value(string_value=controller_type)
 
                     response = call_set_parameters(
@@ -277,7 +300,7 @@ def main(args=None):
                             + controller_type
                             + '" for '
                             + bcolors.BOLD
-                            + prefixed_controller_name
+                            + controller_name
                             + bcolors.ENDC
                         )
                     else:
@@ -287,14 +310,14 @@ def main(args=None):
                             + controller_type
                             + '" for '
                             + bcolors.BOLD
-                            + prefixed_controller_name
+                            + controller_name
                             + bcolors.ENDC
                         )
                         return 1
 
                 if param_file:
                     parameter = Parameter()
-                    parameter.name = prefixed_controller_name + ".params_file"
+                    parameter.name = controller_name + ".params_file"
                     parameter.value = get_parameter_value(string_value=param_file)
 
                     response = call_set_parameters(
@@ -309,7 +332,7 @@ def main(args=None):
                             + param_file
                             + '" for '
                             + bcolors.BOLD
-                            + prefixed_controller_name
+                            + controller_name
                             + bcolors.ENDC
                         )
                     else:
@@ -319,19 +342,19 @@ def main(args=None):
                             + param_file
                             + '" for '
                             + bcolors.BOLD
-                            + prefixed_controller_name
+                            + controller_name
                             + bcolors.ENDC
                         )
                         return 1
 
                 if not fallback_controllers and param_file:
                     fallback_controllers = get_parameter_from_param_file(
-                        controller_name, param_file, "fallback_controllers"
+                        controller_name, spawner_namespace, param_file, "fallback_controllers"
                     )
 
                 if fallback_controllers:
                     parameter = Parameter()
-                    parameter.name = prefixed_controller_name + ".fallback_controllers"
+                    parameter.name = controller_name + ".fallback_controllers"
                     parameter.value = get_parameter_value(string_value=str(fallback_controllers))
 
                     response = call_set_parameters(
@@ -346,7 +369,7 @@ def main(args=None):
                             + ",".join(fallback_controllers)
                             + '"] for '
                             + bcolors.BOLD
-                            + prefixed_controller_name
+                            + controller_name
                             + bcolors.ENDC
                         )
                     else:
@@ -356,7 +379,7 @@ def main(args=None):
                             + ",".join(fallback_controllers)
                             + '"] for '
                             + bcolors.BOLD
-                            + prefixed_controller_name
+                            + controller_name
                             + bcolors.ENDC
                         )
                         return 1
@@ -367,16 +390,12 @@ def main(args=None):
                         bcolors.FAIL
                         + "Failed loading controller "
                         + bcolors.BOLD
-                        + prefixed_controller_name
+                        + controller_name
                         + bcolors.ENDC
                     )
                     return 1
                 node.get_logger().info(
-                    bcolors.OKBLUE
-                    + "Loaded "
-                    + bcolors.BOLD
-                    + prefixed_controller_name
-                    + bcolors.ENDC
+                    bcolors.OKBLUE + "Loaded " + bcolors.BOLD + controller_name + bcolors.ENDC
                 )
 
             if not args.load_only:
@@ -401,7 +420,7 @@ def main(args=None):
                         bcolors.OKGREEN
                         + "Configured and activated "
                         + bcolors.BOLD
-                        + prefixed_controller_name
+                        + controller_name
                         + bcolors.ENDC
                     )
 

controller_manager/test/controller_manager_test_common.hpp

@@ -61,14 +61,15 @@ class ControllerManagerFixture : public ::testing::Test
 {
 public:
   explicit ControllerManagerFixture(
-    const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf)
+    const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf,
+    const std::string & cm_namespace = "")
   : robot_description_(robot_description)
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
     cm_ = std::make_shared<CtrlMgr>(
       std::make_unique<hardware_interface::ResourceManager>(
         rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
-      executor_, TEST_CM_NAME);
+      executor_, TEST_CM_NAME, cm_namespace);
     // We want to be able to not pass robot description immediately
     if (!robot_description_.empty())
     {

controller_manager/test/test_controller_spawner_with_type.yaml

@@ -11,3 +11,17 @@ chainable_ctrl_with_parameters_and_type:
 ctrl_with_parameters_and_no_type:
   ros__parameters:
     joint_names: ["joint2"]
+
+/foo_namespace/ctrl_with_parameters_and_type:
+  ros__parameters:
+    type: "controller_manager/test_controller"
+    joint_names: ["joint1"]
+
+/foo_namespace/chainable_ctrl_with_parameters_and_type:
+  ros__parameters:
+    type: "controller_manager/test_chainable_controller"
+    joint_names: ["joint1"]
+
+/foo_namespace/ctrl_with_parameters_and_no_type:
+  ros__parameters:
+    joint_names: ["joint2"]