Merge branch 'master' into add_parameters_to_interface_info

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -174,11 +174,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   {
 // \note The versions conditioning is added here to support the source-compatibility with Humble
 #if RCLCPP_VERSION_MAJOR >= 21
-    return rclcpp::NodeOptions().enable_logger_service(true);
+    return rclcpp::NodeOptions().enable_logger_service(true).use_global_arguments(false);
 #else
     return rclcpp::NodeOptions()
       .allow_undeclared_parameters(true)
-      .automatically_declare_parameters_from_overrides(true);
+      .automatically_declare_parameters_from_overrides(true)
+      .use_global_arguments(false);
 #endif
   }
 

controller_interface/test/test_controller_interface.cpp

@@ -16,6 +16,8 @@
 
 #include <gmock/gmock.h>
 #include <memory>
+#include <string>
+#include <vector>
 
 #include "rclcpp/executor_options.hpp"
 #include "rclcpp/executors/multi_threaded_executor.hpp"
@@ -57,13 +59,16 @@ TEST(TestableControllerInterface, init)
 TEST(TestableControllerInterface, setting_update_rate_in_configure)
 {
   // mocks the declaration of overrides parameters in a yaml file
-  char const * const argv[] = {"", "--ros-args", "-p", "update_rate:=2812"};
-  int argc = arrlen(argv);
-  rclcpp::init(argc, argv);
+  rclcpp::init(0, nullptr);
 
   TestableControllerInterface controller;
   // initialize, create node
-  const auto node_options = controller.define_custom_node_options();
+  auto node_options = controller.define_custom_node_options();
+  std::vector<std::string> node_options_arguments = node_options.arguments();
+  node_options_arguments.push_back("--ros-args");
+  node_options_arguments.push_back("-p");
+  node_options_arguments.push_back("update_rate:=2812");
+  node_options = node_options.arguments(node_options_arguments);
   ASSERT_EQ(
     controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options),
     controller_interface::return_type::OK);

controller_manager/controller_manager/__init__.py

@@ -23,6 +23,10 @@
     set_hardware_component_state,
     switch_controllers,
     unload_controller,
+    get_parameter_from_param_file,
+    set_controller_parameters,
+    set_controller_parameters_from_param_file,
+    bcolors,
 )
 
 __all__ = [
@@ -36,4 +40,8 @@
     "set_hardware_component_state",
     "switch_controllers",
     "unload_controller",
+    "get_parameter_from_param_file",
+    "set_controller_parameters",
+    "set_controller_parameters_from_param_file",
+    "bcolors",
 ]

controller_manager/controller_manager/controller_manager_services.py

@@ -26,13 +26,68 @@
 )
 
 import rclpy
+import yaml
+from rcl_interfaces.msg import Parameter
+
+# @note: The versions conditioning is added here to support the source-compatibility with Humble
+# The `get_parameter_value` function is moved to `rclpy.parameter` module from `ros2param.api` module from version 3.6.0
+try:
+    from rclpy.parameter import get_parameter_value
+except ImportError:
+    from ros2param.api import get_parameter_value
+from ros2param.api import call_set_parameters
+
+
+# from https://stackoverflow.com/a/287944
+class bcolors:
+    MAGENTA = "\033[95m"
+    OKBLUE = "\033[94m"
+    OKCYAN = "\033[96m"
+    OKGREEN = "\033[92m"
+    WARNING = "\033[93m"
+    FAIL = "\033[91m"
+    ENDC = "\033[0m"
+    BOLD = "\033[1m"
+    UNDERLINE = "\033[4m"
 
 
 class ServiceNotFoundError(Exception):
     pass
 
 
-def service_caller(node, service_name, service_type, request, service_timeout=0.0):
+def service_caller(
+    node,
+    service_name,
+    service_type,
+    request,
+    service_timeout=0.0,
+    call_timeout=10.0,
+    max_attempts=3,
+):
+    """
+    Abstraction of a service call.
+
+    Has an optional timeout to find the service, receive the answer to a call
+    and a mechanism to retry a call of no response is received.
+
+    @param node Node object to be associated with
+    @type rclpy.node.Node
+    @param service_name Service URL
+    @type str
+    @param request The request to be sent
+    @type service request type
+    @param service_timeout Timeout (in seconds) to wait until the service is available. 0 means
+    waiting forever, retrying every 10 seconds.
+    @type float
+    @param call_timeout Timeout (in seconds) for getting a response
+    @type float
+    @param max_attempts Number of attempts until a valid response is received. With some
+    middlewares it can happen, that the service response doesn't reach the client leaving it in
+    a waiting state forever.
+    @type int
+    @return The service response
+
+    """
     cli = node.create_client(service_type, service_name)
 
     while not cli.service_is_ready():
@@ -44,12 +99,20 @@ def service_caller(node, service_name, service_type, request, service_timeout=0.
             node.get_logger().warn(f"Could not contact service {service_name}")
 
     node.get_logger().debug(f"requester: making request: {request}\n")
-    future = cli.call_async(request)
-    rclpy.spin_until_future_complete(node, future)
-    if future.result() is not None:
-        return future.result()
-    else:
-        raise RuntimeError(f"Exception while calling service: {future.exception()}")
+    future = None
+    for attempt in range(max_attempts):
+        future = cli.call_async(request)
+        rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout)
+        if future.result() is None:
+            node.get_logger().warning(
+                f"Failed getting a result from calling {service_name} in "
+                f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)"
+            )
+        else:
+            return future.result()
+    raise RuntimeError(
+        f"Could not successfully call service {service_name} after {max_attempts} attempts."
+    )
 
 
 def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
@@ -180,3 +243,97 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti
         request,
         service_timeout,
     )
+
+
+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 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 : {namespaced_controller}"
+                )
+            if parameter_name in parameters[namespaced_controller]["ros__parameters"]:
+                return parameters[namespaced_controller]["ros__parameters"][parameter_name]
+            else:
+                return None
+        else:
+            return None
+
+
+def set_controller_parameters(
+    node, controller_manager_name, controller_name, parameter_name, parameter_value
+):
+    parameter = Parameter()
+    parameter.name = controller_name + "." + parameter_name
+    parameter_string = str(parameter_value)
+    parameter.value = get_parameter_value(string_value=parameter_string)
+
+    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
+            + 'Setting controller param "'
+            + parameter_name
+            + '" to "'
+            + parameter_string
+            + '" for '
+            + bcolors.BOLD
+            + controller_name
+            + bcolors.ENDC
+        )
+    else:
+        node.get_logger().fatal(
+            bcolors.FAIL
+            + 'Could not set controller param "'
+            + parameter_name
+            + '" to "'
+            + parameter_string
+            + '" for '
+            + bcolors.BOLD
+            + controller_name
+            + bcolors.ENDC
+        )
+        return False
+    return True
+
+
+def set_controller_parameters_from_param_file(
+    node, controller_manager_name, controller_name, parameter_file, namespace=None
+):
+    if parameter_file:
+        spawner_namespace = namespace if namespace else node.get_namespace()
+        set_controller_parameters(
+            node, controller_manager_name, controller_name, "param_file", parameter_file
+        )
+
+        controller_type = get_parameter_from_param_file(
+            controller_name, spawner_namespace, parameter_file, "type"
+        )
+        if controller_type:
+            if not set_controller_parameters(
+                node, controller_manager_name, controller_name, "type", controller_type
+            ):
+                return False
+
+        fallback_controllers = get_parameter_from_param_file(
+            controller_name, spawner_namespace, parameter_file, "fallback_controllers"
+        )
+        if fallback_controllers:
+            if not set_controller_parameters(
+                node,
+                controller_manager_name,
+                controller_name,
+                "fallback_controllers",
+                fallback_controllers,
+            ):
+                return False
+    return True

controller_manager/controller_manager/hardware_spawner.py

@@ -19,6 +19,7 @@
 from controller_manager import (
     list_hardware_components,
     set_hardware_component_state,
+    bcolors,
 )
 from controller_manager.controller_manager_services import ServiceNotFoundError
 
@@ -28,19 +29,6 @@
 from rclpy.signals import SignalHandlerOptions
 
 
-# from https://stackoverflow.com/a/287944
-class bcolors:
-    HEADER = "\033[95m"
-    OKBLUE = "\033[94m"
-    OKCYAN = "\033[96m"
-    OKGREEN = "\033[92m"
-    WARNING = "\033[93m"
-    FAIL = "\033[91m"
-    ENDC = "\033[0m"
-    BOLD = "\033[1m"
-    UNDERLINE = "\033[4m"
-
-
 def first_match(iterable, predicate):
     return next((n for n in iterable if predicate(n)), None)