Add rosparam verb load (ros2#590)

* Add rosparam verb load

Signed-off-by: Victor Lopez <[email protected]>

* Move load_parameter_file implementation to api, add test_verb_load

Signed-off-by: Victor Lopez <[email protected]>

* Apply fixes for linter

Signed-off-by: Victor Lopez <[email protected]>

* Remove TODO comment

Signed-off-by: Victor Lopez <[email protected]>

* Fix linter errors

Signed-off-by: Audrow Nash <[email protected]>

Co-authored-by: Audrow Nash <[email protected]>

ros2param/ros2param/api/__init__.py

@@ -12,14 +12,19 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import sys
+
+from rcl_interfaces.msg import Parameter
 from rcl_interfaces.msg import ParameterType
 from rcl_interfaces.msg import ParameterValue
 from rcl_interfaces.srv import DescribeParameters
 from rcl_interfaces.srv import GetParameters
 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
 
 
@@ -92,6 +97,65 @@ 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 load_parameter_file(*, node, node_name, parameter_file):
+    # Remove leading slash and namespaces
+    internal_node_name = node_name.split('/')[-1]
+    with open(parameter_file, 'r') as f:
+        param_file = yaml.safe_load(f)
+        if internal_node_name not in param_file:
+            raise RuntimeError('Param file does not contain parameters for {}, '
+                               ' only for namespaces: {}' .format(internal_node_name,
+                                                                  param_file.keys()))
+
+        value = param_file[internal_node_name]
+        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(internal_node_name))
+        load_parameter_dict(node=node, node_name=node_name,
+                            parameter_dict=value['ros__parameters'])
+
+
 def call_describe_parameters(*, node, node_name, parameter_names=None):
     # create client
     client = node.create_client(

ros2param/ros2param/verb/load.py

@@ -0,0 +1,50 @@
+# 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_file
+from ros2param.verb import VerbExtension
+
+
+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')
+
+    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'
+
+        with DirectNode(args) as node:
+            load_parameter_file(node=node, node_name=node_name, parameter_file=args.parameter_file)

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',
         ],
     }
 )

ros2param/test/fixtures/parameter_node.py

@@ -11,7 +11,6 @@
 # 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.
-import sys
 
 import rclpy
 from rclpy.parameter import PARAMETER_SEPARATOR_STRING
@@ -38,9 +37,6 @@ def main(args=None):
         rclpy.spin(node)
     except KeyboardInterrupt:
         print('parameter node stopped cleanly')
-    except BaseException:
-        print('exception in parameter node:', file=sys.stderr)
-        raise
     finally:
         node.destroy_node()
         rclpy.shutdown()