Merge branch 'master' into feat/interface/remapping

codecov.yml

@@ -14,8 +14,6 @@ fixes:
 comment:
   layout: "diff, flags, files"
   behavior: default
-ignore:
-  - "**/test"
 flags:
   unittests:
     paths:

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,28 +26,96 @@
 )
 
 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
 
-def service_caller(node, service_name, service_type, request, service_timeout=10.0):
+
+# 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,
+    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)
 
-    if not cli.service_is_ready():
-        node.get_logger().debug(
-            f"waiting {service_timeout} seconds for service {service_name} to become available..."
-        )
-        if not cli.wait_for_service(service_timeout):
-            raise RuntimeError(f"Could not contact service {service_name}")
+    while not cli.service_is_ready():
+        node.get_logger().info(f"waiting for service {service_name} to become available...")
+        if service_timeout:
+            if not cli.wait_for_service(service_timeout):
+                raise ServiceNotFoundError(f"Could not contact service {service_name}")
+        elif not cli.wait_for_service(10.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=10.0):
+def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
     request = ConfigureController.Request()
     request.name = controller_name
     return service_caller(
@@ -59,7 +127,7 @@ def configure_controller(node, controller_manager_name, controller_name, service
     )
 
 
-def list_controllers(node, controller_manager_name, service_timeout=10.0):
+def list_controllers(node, controller_manager_name, service_timeout=0.0):
     request = ListControllers.Request()
     return service_caller(
         node,
@@ -70,7 +138,7 @@ def list_controllers(node, controller_manager_name, service_timeout=10.0):
     )
 
 
-def list_controller_types(node, controller_manager_name, service_timeout=10.0):
+def list_controller_types(node, controller_manager_name, service_timeout=0.0):
     request = ListControllerTypes.Request()
     return service_caller(
         node,
@@ -81,7 +149,7 @@ def list_controller_types(node, controller_manager_name, service_timeout=10.0):
     )
 
 
-def list_hardware_components(node, controller_manager_name, service_timeout=10.0):
+def list_hardware_components(node, controller_manager_name, service_timeout=0.0):
     request = ListHardwareComponents.Request()
     return service_caller(
         node,
@@ -92,7 +160,7 @@ def list_hardware_components(node, controller_manager_name, service_timeout=10.0
     )
 
 
-def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0):
+def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0):
     request = ListHardwareInterfaces.Request()
     return service_caller(
         node,
@@ -103,7 +171,7 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0
     )
 
 
-def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0):
+def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
     request = LoadController.Request()
     request.name = controller_name
     return service_caller(
@@ -115,7 +183,7 @@ def load_controller(node, controller_manager_name, controller_name, service_time
     )
 
 
-def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0):
+def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0):
     request = ReloadControllerLibraries.Request()
     request.force_kill = force_kill
     return service_caller(
@@ -128,7 +196,7 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi
 
 
 def set_hardware_component_state(
-    node, controller_manager_name, component_name, lifecyle_state, service_timeout=10.0
+    node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0
 ):
     request = SetHardwareComponentState.Request()
     request.name = component_name
@@ -165,7 +233,7 @@ def switch_controllers(
     )
 
 
-def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0):
+def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
     request = UnloadController.Request()
     request.name = controller_name
     return service_caller(
@@ -175,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, "params_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