Merge branch 'master' into sensor-joints

.pre-commit-config.yaml

@@ -63,7 +63,7 @@ repos:
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v19.1.3
+    rev: v19.1.4
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -133,7 +133,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.29.4
+    rev: 0.30.0
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -71,6 +71,25 @@ struct ControllerUpdateStats
   unsigned int total_triggers;
   unsigned int failed_triggers;
 };
+
+/**
+ * Struct to store the status of the controller update method.
+ * The status contains information if the update was triggered successfully, the result of the
+ * update method and the execution duration of the update method. The status is used to provide
+ * feedback to the controller_manager.
+ * @var successful: true if the update was triggered successfully, false if not.
+ * @var result: return_type::OK if update is successfully, otherwise return_type::ERROR.
+ * @var execution_time: duration of the execution of the update method.
+ * @var period: period of the update method.
+ */
+struct ControllerUpdateStatus
+{
+  bool successful = true;
+  return_type result = return_type::OK;
+  std::optional<std::chrono::nanoseconds> execution_time = std::nullopt;
+  std::optional<rclcpp::Duration> period = std::nullopt;
+};
+
 /**
  * Base interface class  for an controller. The interface may not be used to implement a controller.
  * The class provides definitions for `ControllerInterface` and `ChainableControllerInterface`
@@ -175,13 +194,11 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    *
    * \param[in] time The time at the start of this control loop iteration
    * \param[in] period The measured time taken by the last control loop iteration
-   * \returns A pair with the first element being a boolean indicating if the async callback method
-   * was triggered and the second element being the last return value of the async function. For
-   * more details check the AsyncFunctionHandler implementation in `realtime_tools` package.
+   * \returns ControllerUpdateStatus. The status contains information if the update was triggered
+   * successfully, the result of the update method and the execution duration of the update method.
    */
   CONTROLLER_INTERFACE_PUBLIC
-  std::pair<bool, return_type> trigger_update(
-    const rclcpp::Time & time, const rclcpp::Duration & period);
+  ControllerUpdateStatus trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period);
 
   CONTROLLER_INTERFACE_PUBLIC
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node();

controller_interface/src/controller_interface_base.cpp

@@ -159,12 +159,14 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c
   return node_->get_current_state();
 }
 
-std::pair<bool, return_type> ControllerInterfaceBase::trigger_update(
+ControllerUpdateStatus ControllerInterfaceBase::trigger_update(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+  ControllerUpdateStatus status;
   trigger_stats_.total_triggers++;
   if (is_async())
   {
+    const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time();
     const auto result = async_handler_->trigger_async_callback(time, period);
     if (!result.first)
     {
@@ -174,12 +176,28 @@ std::pair<bool, return_type> ControllerInterfaceBase::trigger_update(
         "The controller missed %u update cycles out of %u total triggers.",
         trigger_stats_.failed_triggers, trigger_stats_.total_triggers);
     }
-    return result;
+    status.successful = result.first;
+    status.result = result.second;
+    const auto execution_time = async_handler_->get_last_execution_time();
+    if (execution_time.count() > 0)
+    {
+      status.execution_time = execution_time;
+    }
+    if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED)
+    {
+      status.period = time - last_trigger_time;
+    }
   }
   else
   {
-    return std::make_pair(true, update(time, period));
+    const auto start_time = std::chrono::steady_clock::now();
+    status.successful = true;
+    status.result = update(time, period);
+    status.execution_time = std::chrono::duration_cast<std::chrono::nanoseconds>(
+      std::chrono::steady_clock::now() - start_time);
+    status.period = period;
   }
+  return status;
 }
 
 std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::get_node()

controller_manager/CMakeLists.txt

@@ -16,6 +16,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp
   realtime_tools
   std_msgs
+  libstatistics_collector
+  generate_parameter_library
 )
 
 find_package(ament_cmake REQUIRED)
@@ -27,6 +29,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
+generate_parameter_library(controller_manager_parameters
+  src/controller_manager_parameters.yaml
+)
+
 add_library(controller_manager SHARED
   src/controller_manager.cpp
 )
@@ -35,6 +41,9 @@ target_include_directories(controller_manager PUBLIC
   $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/controller_manager>
 )
+target_link_libraries(controller_manager PUBLIC
+  controller_manager_parameters
+)
 ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 # Causes the visibility macros to use dllexport rather than dllimport,
@@ -210,6 +219,9 @@ if(BUILD_TESTING)
   install(FILES test/test_controller_spawner_with_type.yaml
   DESTINATION test)
 
+  install(FILES test/test_controller_overriding_parameters.yaml
+    DESTINATION test)
+
   ament_add_gmock(test_hardware_management_srvs
     test/test_hardware_management_srvs.cpp
   )
@@ -233,7 +245,7 @@ install(
   DESTINATION include/controller_manager
 )
 install(
-  TARGETS controller_manager
+  TARGETS controller_manager controller_manager_parameters
   EXPORT export_controller_manager
   RUNTIME DESTINATION bin
   LIBRARY DESTINATION lib

controller_manager/controller_manager/__init__.py

@@ -23,9 +23,9 @@
     set_hardware_component_state,
     switch_controllers,
     unload_controller,
-    get_parameter_from_param_file,
+    get_parameter_from_param_files,
     set_controller_parameters,
-    set_controller_parameters_from_param_file,
+    set_controller_parameters_from_param_files,
     bcolors,
 )
 
@@ -40,8 +40,8 @@
     "set_hardware_component_state",
     "switch_controllers",
     "unload_controller",
-    "get_parameter_from_param_file",
+    "get_parameter_from_param_files",
     "set_controller_parameters",
-    "set_controller_parameters_from_param_file",
+    "set_controller_parameters_from_param_files",
     "bcolors",
 ]

controller_manager/controller_manager/controller_manager_services.py

@@ -284,57 +284,90 @@ def unload_controller(
     )
 
 
-def get_parameter_from_param_file(
-    node, controller_name, namespace, parameter_file, parameter_name
+def get_params_files_with_controller_parameters(
+    node, controller_name: str, namespace: str, parameter_files: list
 ):
-    with open(parameter_file) as f:
-        namespaced_controller = (
-            f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}"
-        )
-        WILDCARD_KEY = "/**"
-        ROS_PARAMS_KEY = "ros__parameters"
-        parameters = yaml.safe_load(f)
-        controller_param_dict = None
-        # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name'
-        for key in [
-            controller_name,
-            namespaced_controller,
-            f"{WILDCARD_KEY}/{controller_name}",
-            f"{WILDCARD_KEY}{namespaced_controller}",
-        ]:
-            if key in parameters:
-                if key == controller_name and namespace != "/":
-                    node.get_logger().fatal(
-                        f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}"
+    controller_parameter_files = []
+    for parameter_file in parameter_files:
+        if parameter_file in controller_parameter_files:
+            continue
+        with open(parameter_file) as f:
+            namespaced_controller = (
+                f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}"
+            )
+            WILDCARD_KEY = "/**"
+            parameters = yaml.safe_load(f)
+            # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name'
+            for key in [
+                controller_name,
+                namespaced_controller,
+                f"{WILDCARD_KEY}/{controller_name}",
+                f"{WILDCARD_KEY}{namespaced_controller}",
+            ]:
+                if key in parameters:
+                    if key == controller_name and namespace != "/":
+                        node.get_logger().fatal(
+                            f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}"
+                        )
+                        break
+                    controller_parameter_files.append(parameter_file)
+
+                if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]:
+                    controller_parameter_files.append(parameter_file)
+    return controller_parameter_files
+
+
+def get_parameter_from_param_files(
+    node, controller_name: str, namespace: str, parameter_files: list, parameter_name: str
+):
+    for parameter_file in parameter_files:
+        with open(parameter_file) as f:
+            namespaced_controller = (
+                f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}"
+            )
+            WILDCARD_KEY = "/**"
+            ROS_PARAMS_KEY = "ros__parameters"
+            parameters = yaml.safe_load(f)
+            controller_param_dict = None
+            # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name'
+            for key in [
+                controller_name,
+                namespaced_controller,
+                f"{WILDCARD_KEY}/{controller_name}",
+                f"{WILDCARD_KEY}{namespaced_controller}",
+            ]:
+                if key in parameters:
+                    if key == controller_name and namespace != "/":
+                        node.get_logger().fatal(
+                            f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}"
+                        )
+                        break
+                    controller_param_dict = parameters[key]
+
+                if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]:
+                    controller_param_dict = parameters[WILDCARD_KEY][key]
+
+                if controller_param_dict and (
+                    not isinstance(controller_param_dict, dict)
+                    or ROS_PARAMS_KEY not in controller_param_dict
+                ):
+                    raise RuntimeError(
+                        f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}"
                     )
+                if (
+                    controller_param_dict
+                    and ROS_PARAMS_KEY in controller_param_dict
+                    and parameter_name in controller_param_dict[ROS_PARAMS_KEY]
+                ):
                     break
-                controller_param_dict = parameters[key]
-
-            if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]:
-                controller_param_dict = parameters[WILDCARD_KEY][key]
-
-            if controller_param_dict and (
-                not isinstance(controller_param_dict, dict)
-                or ROS_PARAMS_KEY not in controller_param_dict
-            ):
-                raise RuntimeError(
-                    f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}"
-                )
-            if (
-                controller_param_dict
-                and ROS_PARAMS_KEY in controller_param_dict
-                and parameter_name in controller_param_dict[ROS_PARAMS_KEY]
-            ):
-                break
-
-        if controller_param_dict is None:
-            node.get_logger().fatal(
-                f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter file : {parameter_file}{bcolors.ENDC}"
-            )
-        if parameter_name in controller_param_dict[ROS_PARAMS_KEY]:
-            return controller_param_dict[ROS_PARAMS_KEY][parameter_name]
 
-        return None
+            if controller_param_dict and parameter_name in controller_param_dict[ROS_PARAMS_KEY]:
+                return controller_param_dict[ROS_PARAMS_KEY][parameter_name]
+    if controller_param_dict is None:
+        node.get_logger().fatal(
+            f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter files : {parameter_files}{bcolors.ENDC}"
+        )
+    return None
 
 
 def set_controller_parameters(
@@ -378,26 +411,36 @@ def set_controller_parameters(
     return True
 
 
-def set_controller_parameters_from_param_file(
-    node, controller_manager_name, controller_name, parameter_file, namespace=None
+def set_controller_parameters_from_param_files(
+    node, controller_manager_name: str, controller_name: str, parameter_files: list, namespace=None
 ):
-    if parameter_file:
-        spawner_namespace = namespace if namespace else node.get_namespace()
+    spawner_namespace = namespace if namespace else node.get_namespace()
+    controller_parameter_files = get_params_files_with_controller_parameters(
+        node, controller_name, spawner_namespace, parameter_files
+    )
+    if controller_parameter_files:
         set_controller_parameters(
-            node, controller_manager_name, controller_name, "params_file", parameter_file
+            node,
+            controller_manager_name,
+            controller_name,
+            "params_file",
+            controller_parameter_files,
         )
 
-        controller_type = get_parameter_from_param_file(
-            node, controller_name, spawner_namespace, parameter_file, "type"
+        controller_type = get_parameter_from_param_files(
+            node, controller_name, spawner_namespace, controller_parameter_files, "type"
         )
-        if controller_type:
-            if not set_controller_parameters(
-                node, controller_manager_name, controller_name, "type", controller_type
-            ):
-                return False
+        if controller_type and not set_controller_parameters(
+            node, controller_manager_name, controller_name, "type", controller_type
+        ):
+            return False
 
-        fallback_controllers = get_parameter_from_param_file(
-            node, controller_name, spawner_namespace, parameter_file, "fallback_controllers"
+        fallback_controllers = get_parameter_from_param_files(
+            node,
+            controller_name,
+            spawner_namespace,
+            controller_parameter_files,
+            "fallback_controllers",
         )
         if fallback_controllers:
             if not set_controller_parameters(