diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index dc2647993f..fb3078fa38 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -444,43 +444,62 @@ def set_controller_parameters( def set_controller_parameters_from_param_files( - node, controller_manager_name: str, controller_name: str, parameter_files: list, namespace=None + node, controller_manager_name: str, controller_name: str, parameter_files: list, param_file_remote: bool, controller_to_type: dict[str, str], namespace=None ): spawner_namespace = namespace if namespace else node.get_namespace() - controller_parameter_files = get_params_files_with_controller_parameters( - node, controller_name, spawner_namespace, parameter_files - ) - if controller_parameter_files: - set_controller_parameters( + + if param_file_remote: + print(f"setting params_file = {parameter_files}") + if not set_controller_parameters( node, controller_manager_name, controller_name, "params_file", - controller_parameter_files, - ) - - controller_type = get_parameter_from_param_files( - node, controller_name, spawner_namespace, controller_parameter_files, "type" - ) + parameter_files, + ): + return False + + controller_type = controller_to_type.get(controller_name) + print(f"setting type = {controller_type}") if controller_type and not set_controller_parameters( node, controller_manager_name, controller_name, "type", controller_type ): return False - - fallback_controllers = get_parameter_from_param_files( - node, - controller_name, - spawner_namespace, - controller_parameter_files, - "fallback_controllers", + else: + controller_parameter_files = get_params_files_with_controller_parameters( + node, controller_name, spawner_namespace, parameter_files ) - if fallback_controllers: - if not set_controller_parameters( + if controller_parameter_files: + set_controller_parameters( node, controller_manager_name, controller_name, - "fallback_controllers", - fallback_controllers, + "params_file", + controller_parameter_files, + ) + + controller_type = get_parameter_from_param_files( + node, controller_name, spawner_namespace, controller_parameter_files, "type" + ) + if controller_type and not set_controller_parameters( + node, controller_manager_name, controller_name, "type", controller_type ): return False + + fallback_controllers = get_parameter_from_param_files( + node, + controller_name, + spawner_namespace, + controller_parameter_files, + "fallback_controllers", + ) + if fallback_controllers: + if not set_controller_parameters( + node, + controller_manager_name, + controller_name, + "fallback_controllers", + fallback_controllers, + ): + return False return True diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 4e020fdd89..e9f3a87c3a 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -72,10 +72,36 @@ def is_controller_loaded( return any(c.name == controller_name for c in controllers) +def parse_type_from_controllers(controller_names: list[str]) -> dict[str, str]: + controller_to_type = dict() + for name in controller_names: + # We expect controller:some/type + # -> split[0]=controller AND split[1]=some/type + split = name.split(":") + if len(split) != 2 or not split[0] or not split[1]: + raise ValueError( + f"Invalid format '{name}'. Expected format is 'controller_name:some/controller_type' if '--param-file-remote-only' flag is used." + ) + controller = split[0] + controller_type = split[1] + + if controller in controller_to_type: + raise ValueError( + f"Controller names must be unique. Got multiple occurrences of {controller}" + ) + else: + controller_to_type[controller] = controller_type + return controller_to_type + + def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() - parser.add_argument("controller_names", help="List of controllers", nargs="+") + parser.add_argument( + "controller_names", + help="List of controllers. In combination with '--param-file-remote-only' flag pass type of controller as 'controller:/controller/type'", + nargs="+", + ) parser.add_argument( "-c", "--controller-manager", @@ -93,6 +119,20 @@ def main(args=None): action="append", required=False, ) + parser.add_argument( + "--param-file-remote-only", + help="Set this to load the param file only remotely. Param file is not needed to be present locally only remotely.", + default=False, + action="store_true", + required=False, + ) + parser.add_argument( + "-n", + "--namespace", + help="DEPRECATED Namespace for the controller_manager and the controller(s)", + default=None, + required=False, + ) parser.add_argument( "--load-only", help="Only load the controller and leave unconfigured.", @@ -155,6 +195,7 @@ def main(args=None): controller_names = args.controller_names controller_manager_name = args.controller_manager param_files = args.param_file + param_file_remote = args.param_file_remote_only controller_manager_timeout = args.controller_manager_timeout service_call_timeout = args.service_call_timeout switch_timeout = args.switch_timeout @@ -162,12 +203,51 @@ def main(args=None): unload_controllers_upon_exit = False node = None - if param_files: + if param_files and not param_file_remote: for param_file in param_files: if not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) logger = rclpy.logging.get_logger("ros2_control_controller_spawner_" + controller_names[0]) + # If we have remote flag given we want to parse the controller_names from + # controller:controller/type to a dict[controller] = controller/type + # the controller_names are then overwritten with only the controller names + controller_to_type = {} + if param_file_remote: + controller_to_type = parse_type_from_controllers(controller_names) + if not controller_to_type: + raise ValueError( + "Invalid format for controller_name.Expected format is 'controller_name:some/controller_type' if '--param-file-remote-only' flag is used." + ) + controller_names = list(controller_to_type.keys()) + + 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:=", + 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("/"): + if spawner_namespace and spawner_namespace != "/": + controller_manager_name = f"{spawner_namespace}/{controller_manager_name}" + else: + controller_manager_name = f"/{controller_manager_name}" + try: spawner_node_name = "spawner_" + controller_names[0] lock = FileLock("/tmp/ros2-control-controller-spawner.lock") @@ -233,12 +313,15 @@ def main(args=None): [arg for args in controller_ros_args for arg in args.split()], ): return 1 + if param_files: if not set_controller_parameters_from_param_files( node, controller_manager_name, controller_name, param_files, + param_file_remote, + controller_to_type, spawner_namespace, ): return 1