Do not use CLI calls but direct API for setting parameters. (ros-cont…

…rols#910) * Do not use CLI calls but direct API for setting parameters. * Add explicity dependency to `rcl_interfaces`.
henrygerardmoore · Jan 21, 2023 · fffb02e · fffb02e
1 parent 98212d4
commit fffb02e
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 7 deletions.
diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py
@@ -16,7 +16,6 @@
 import argparse
 import errno
 import os
-import subprocess
 import sys
 import time
 import warnings
@@ -25,9 +24,13 @@
     load_controller, switch_controllers, unload_controller
 
 import rclpy
+from rcl_interfaces.msg import Parameter
 from rclpy.duration import Duration
 from rclpy.node import Node
+from rclpy.parameter import get_parameter_value
 from rclpy.signals import SignalHandlerOptions
+from ros2param.api import call_set_parameters
+from ros2param.api import load_parameter_file
 
 # from https://stackoverflow.com/a/287944
 
@@ -184,19 +187,35 @@ def main(args=None):
             node.get_logger().warn('Controller already loaded, skipping load_controller')
         else:
             if controller_type:
-                ret = subprocess.run(['ros2', 'param', 'set', controller_manager_name,
-                                      prefixed_controller_name + '.type', controller_type])
+                parameter = Parameter()
+                Parameter.name = prefixed_controller_name + '.type'
+                parameter.value = get_parameter_value(string_value=controller_type)
+
+                response = call_set_parameters(
+                  node=node, node_name=controller_manager_name, parameters=[parameter])
+                assert len(response.results) == 1
+                result = response.results[0]
+                if result.successful:
+                    node.get_logger().info(bcolors.OKCYAN + 'Set controller type to "' + controller_type + '" for ' + bcolors.BOLD + prefixed_controller_name + bcolors.ENDC)
+                else:
+                    node.get_logger().fatal(bcolors.FAIL + 'Could not set controller type to "' + controller_type + '" for ' + bcolors.BOLD + prefixed_controller_name + bcolors.ENDC)
+                    return 1
+
             ret = load_controller(node, controller_manager_name, controller_name)
             if not ret.ok:
                 node.get_logger().fatal(bcolors.FAIL + 'Failed loading controller ' + bcolors.BOLD + prefixed_controller_name + bcolors.ENDC)
                 return 1
             node.get_logger().info(bcolors.OKBLUE + 'Loaded ' + bcolors.BOLD + prefixed_controller_name + bcolors.ENDC)
 
         if param_file:
-            ret = subprocess.run(['ros2', 'param', 'load', prefixed_controller_name, param_file])
-            if ret.returncode != 0:
-                # Error message printed by ros2 param
-                return ret.returncode
+            load_parameter_file(node=node, node_name=prefixed_controller_name, parameter_file=param_file,
+                                use_wildcard=True)
+            node.get_logger().info(bcolors.OKCYAN + 'Loaded parameters file "' + param_file + '" for ' + bcolors.BOLD + prefixed_controller_name + bcolors.ENDC)
+            # TODO(destogl): use return value when upstream return value is merged
+            # ret =
+            # if ret.returncode != 0:
+            #     Error message printed by ros2 param
+            #     return ret.returncode
             node.get_logger().info('Loaded ' + param_file + ' into ' + prefixed_controller_name)
 
         if not args.load_only:

diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml
@@ -11,6 +11,7 @@
   <license>Apache License 2.0</license>
   <author email="victor.lopez@pal-robotics.com">Victor Lopez</author>
 
+  <depend>rcl_interfaces</depend>
   <depend>rclpy</depend>
   <depend>ros2cli</depend>
   <depend>ros2node</depend>