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"]