From 857724db239bc9028c790fa1d699d0667043eca8 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 26 Jan 2021 12:58:34 +0100 Subject: [PATCH] Add rosparam verb load Signed-off-by: Victor Lopez --- ros2param/ros2param/api/__init__.py | 43 ++++++++++++++++ ros2param/ros2param/verb/load.py | 77 +++++++++++++++++++++++++++++ ros2param/setup.py | 1 + 3 files changed, 121 insertions(+) create mode 100644 ros2param/ros2param/verb/load.py diff --git a/ros2param/ros2param/api/__init__.py b/ros2param/ros2param/api/__init__.py index cd8a4b29a..f8963da82 100644 --- a/ros2param/ros2param/api/__init__.py +++ b/ros2param/ros2param/api/__init__.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from rcl_interfaces.msg import Parameter from rcl_interfaces.msg import ParameterType from rcl_interfaces.msg import ParameterValue from rcl_interfaces.srv import DescribeParameters @@ -19,8 +20,10 @@ from rcl_interfaces.srv import ListParameters from rcl_interfaces.srv import SetParameters import rclpy +from rclpy.parameter import PARAMETER_SEPARATOR_STRING from ros2cli.node.direct import DirectNode import yaml +import sys def get_value(*, parameter_value): @@ -92,6 +95,46 @@ def get_parameter_value(*, string_value): return value +def parse_parameter_dict(*, namespace, parameter_dict): + parameters = [] + for param_name, param_value in parameter_dict.items(): + full_param_name = namespace + param_name + # Unroll nested parameters + if type(param_value) == dict: + parameters += parse_parameter_dict( + namespace=full_param_name + PARAMETER_SEPARATOR_STRING, + parameter_dict=param_value) + else: + parameter = Parameter() + parameter.name = full_param_name + parameter.value = get_parameter_value(string_value=str(param_value)) + parameters.append(parameter) + return parameters + + +def load_parameter_dict(*, node, node_name, parameter_dict): + + parameters = parse_parameter_dict(namespace="", parameter_dict=parameter_dict) + response = call_set_parameters( + node=node, node_name=node_name, parameters=parameters) + + # output response + assert len(response.results) == len(parameters) + for i in range(0, len(response.results)): + result = response.results[i] + param_name = parameters[i].name + if result.successful: + msg = 'Set parameter {} successful'.format(param_name) + if result.reason: + msg += ': ' + result.reason + print(msg) + else: + msg = 'Set parameter {} failed'.format(param_name) + if result.reason: + msg += ': ' + result.reason + print(msg, file=sys.stderr) + + def call_describe_parameters(*, node, node_name, parameter_names=None): # create client client = node.create_client( diff --git a/ros2param/ros2param/verb/load.py b/ros2param/ros2param/verb/load.py new file mode 100644 index 000000000..f858af391 --- /dev/null +++ b/ros2param/ros2param/verb/load.py @@ -0,0 +1,77 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ros2cli.node.direct import DirectNode +from ros2cli.node.strategy import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2node.api import get_absolute_node_name +from ros2node.api import get_node_names +from ros2node.api import NodeNameCompleter +from ros2param.api import load_parameter_dict +from ros2param.verb import VerbExtension + +import yaml + + +class LoadVerb(VerbExtension): + """Load parameter file for a node.""" + + def add_arguments(self, parser, cli_name): # noqa: D102 + add_arguments(parser) + arg = parser.add_argument( + 'node_name', help='Name of the ROS node') + arg.completer = NodeNameCompleter( + include_hidden_nodes_key='include_hidden_nodes') + parser.add_argument( + '--include-hidden-nodes', action='store_true', + help='Consider hidden nodes as well') + arg = parser.add_argument( + 'parameter_file', help='Parameter file') + parser.add_argument( + '--use-wildcard', action='store_true', + help='Load parameters in the \'/**\' namespace into the node') + + def main(self, *, args): # noqa: D102 + with NodeStrategy(args) as node: + node_names = get_node_names( + node=node, include_hidden_nodes=args.include_hidden_nodes) + + node_name = get_absolute_node_name(args.node_name) + if node_name not in {n.full_name for n in node_names}: + return 'Node not found' + # Remove leading slash + node_namespace = node_name[1:] + + with DirectNode(args) as node: + with open(args.parameter_file, "r") as f: + param_file = yaml.safe_load(f) + param_namespaces = [] + if args.use_wildcard and "/**" in param_file: + param_namespaces.append("/**") + if node_namespace in param_file: + param_namespaces.append(node_namespace) + + if param_namespaces == []: + raise RuntimeError("Param file doesn't contain parameters for {}, " + " only for namespaces: {}" .format(node_namespace, + param_file.keys())) + + for ns in param_namespaces: + value = param_file[ns] + if type(value) != dict or "ros__parameters" not in value: + raise RuntimeError("Invalid structure of parameter file in namespace {}" + "expected same format as provided by ros2 param dump" + .format(ns)) + load_parameter_dict(node=node, node_name=node_name, + parameter_dict=value["ros__parameters"]) diff --git a/ros2param/setup.py b/ros2param/setup.py index e59e3a301..55a982059 100644 --- a/ros2param/setup.py +++ b/ros2param/setup.py @@ -46,6 +46,7 @@ 'get = ros2param.verb.get:GetVerb', 'list = ros2param.verb.list:ListVerb', 'set = ros2param.verb.set:SetVerb', + 'load = ros2param.verb.load:LoadVerb', ], } )