Merge branch 'master' into async_controllers

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -191,11 +191,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,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, "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