Merge branch 'master' into add-simple-joint-limiter

.github/workflows/humble-debian-build.yml

@@ -0,0 +1,30 @@
+name: Debian Humble Build
+on:
+  workflow_dispatch:
+  pull_request:
+    branches:
+      - humble
+  schedule:
+    # Run every day to detect flakiness and broken dependencies
+    - cron: '03 1 * * *'
+
+
+jobs:
+  humble_debian:
+    name: Humble debian build
+    runs-on: ubuntu-latest
+    env:
+      ROS_DISTRO: humble
+    container: ghcr.io/ros-controls/ros:humble-debian
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          path: src/ros2_control
+      - name: Build and test
+        shell: bash
+        run: |
+          source /opt/ros2_ws/install/setup.bash
+          vcs import src < src/ros2_control/ros2_control.${{ env.ROS_DISTRO }}.repos
+          colcon build --packages-skip rqt_controller_manager
+          colcon test --packages-skip rqt_controller_manager control_msgs controller_manager_msgs
+          colcon test-result --verbose

.github/workflows/humble-rhel-binary-build.yml

@@ -1,9 +1,6 @@
-name: Humble RHEL Binary Build
+name: RHEL Humble Binary Build
 on:
   workflow_dispatch:
-  push:
-    branches:
-      - humble
   pull_request:
     branches:
       - humble
@@ -22,10 +19,13 @@ jobs:
       - uses: actions/checkout@v4
         with:
           path: src/ros2_control
-      - run: |
+      - name: Install dependencies
+        run: |
           rosdep update
           rosdep install -iyr --from-path src/ros2_control || true
+      - name: Build and test
+        run: |
           source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash
           colcon build --packages-skip rqt_controller_manager
           colcon test --packages-skip rqt_controller_manager ros2controlcli
-          colcon test-result
+          colcon test-result --verbose

.github/workflows/iron-debian-build.yml

@@ -0,0 +1,30 @@
+name: Debian Iron Build
+on:
+  workflow_dispatch:
+  pull_request:
+    branches:
+      - iron
+  schedule:
+    # Run every day to detect flakiness and broken dependencies
+    - cron: '03 1 * * *'
+
+
+jobs:
+  iron_debian:
+    name: Iron debian build
+    runs-on: ubuntu-latest
+    env:
+      ROS_DISTRO: iron
+    container: ghcr.io/ros-controls/ros:iron-debian
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          path: src/ros2_control
+      - name: Build and test
+        shell: bash
+        run: |
+          source /opt/ros2_ws/install/setup.bash
+          vcs import src < src/ros2_control/ros2_control.${{ env.ROS_DISTRO }}.repos
+          colcon build --packages-skip rqt_controller_manager
+          colcon test --packages-skip rqt_controller_manager control_msgs controller_manager_msgs
+          colcon test-result --verbose

.github/workflows/iron-rhel-binary-build.yml

@@ -1,9 +1,6 @@
-name: Iron RHEL Binary Build
+name: RHEL Iron Binary Build
 on:
   workflow_dispatch:
-  push:
-    branches:
-      - iron
   pull_request:
     branches:
       - iron
@@ -23,10 +20,13 @@ jobs:
       - uses: actions/checkout@v4
         with:
           path: src/ros2_control
-      - run: |
+      - name: Install dependencies
+        run: |
           rosdep update
           rosdep install -iyr --from-path src/ros2_control || true
+      - name: Build and test
+        run: |
           source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash
           colcon build --packages-skip rqt_controller_manager
           colcon test --packages-skip rqt_controller_manager ros2controlcli
-          colcon test-result
+          colcon test-result --verbose

.github/workflows/rolling-debian-build.yml

@@ -0,0 +1,30 @@
+name: Debian Rolling Build
+on:
+  workflow_dispatch:
+  pull_request:
+    branches:
+      - master
+  schedule:
+    # Run every day to detect flakiness and broken dependencies
+    - cron: '03 1 * * *'
+
+
+jobs:
+  rolling_debian:
+    name: Rolling debian build
+    runs-on: ubuntu-latest
+    env:
+      ROS_DISTRO: rolling
+    container: ghcr.io/ros-controls/ros:rolling-debian
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          path: src/ros2_control
+      - name: Build and test
+        shell: bash
+        run: |
+          source /opt/ros2_ws/install/setup.bash
+          vcs import src < src/ros2_control/ros2_control.${{ env.ROS_DISTRO }}.repos
+          colcon build --packages-skip rqt_controller_manager
+          colcon test --packages-skip rqt_controller_manager
+          colcon test-result --verbose

.github/workflows/rolling-rhel-binary-build.yml

@@ -1,9 +1,6 @@
-name: Rolling RHEL Binary Build
+name: RHEL Rolling Binary Build
 on:
   workflow_dispatch:
-  push:
-    branches:
-      - master
   pull_request:
     branches:
       - master
@@ -23,10 +20,13 @@ jobs:
       - uses: actions/checkout@v4
         with:
           path: src/ros2_control
-      - run: |
+      - name: Install dependencies
+        run: |
           rosdep update
           rosdep install -iyr --from-path src/ros2_control || true
+      - name: Build and test
+        run: |
           source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash
           colcon build --packages-skip rqt_controller_manager
           colcon test --packages-skip rqt_controller_manager ros2controlcli
-          colcon test-result
+          colcon test-result --verbose

controller_manager/controller_manager/spawner.py

@@ -19,8 +19,6 @@
 import sys
 import time
 import warnings
-import io
-from contextlib import redirect_stdout, redirect_stderr
 
 from controller_manager import (
     configure_controller,
@@ -37,7 +35,6 @@
 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
 
@@ -266,6 +263,38 @@ def main(args=None):
                         )
                         return 1
 
+                if param_file:
+                    parameter = Parameter()
+                    parameter.name = prefixed_controller_name + ".params_file"
+                    parameter.value = get_parameter_value(string_value=param_file)
+
+                    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 params file to "'
+                            + param_file
+                            + '" for '
+                            + bcolors.BOLD
+                            + prefixed_controller_name
+                            + bcolors.ENDC
+                        )
+                    else:
+                        node.get_logger().fatal(
+                            bcolors.FAIL
+                            + 'Could not set controller params file to "'
+                            + param_file
+                            + '" 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(
@@ -284,39 +313,6 @@ def main(args=None):
                     + bcolors.ENDC
                 )
 
-            if param_file:
-                # load_parameter_file writes to stdout/stderr. Here we capture that and use node logging instead
-                with redirect_stdout(io.StringIO()) as f_stdout, redirect_stderr(
-                    io.StringIO()
-                ) as f_stderr:
-                    load_parameter_file(
-                        node=node,
-                        node_name=prefixed_controller_name,
-                        parameter_file=param_file,
-                        use_wildcard=True,
-                    )
-                if f_stdout.getvalue():
-                    node.get_logger().info(bcolors.OKCYAN + f_stdout.getvalue() + bcolors.ENDC)
-                if f_stderr.getvalue():
-                    node.get_logger().error(bcolors.FAIL + f_stderr.getvalue() + bcolors.ENDC)
-                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:
                 ret = configure_controller(node, controller_manager_name, controller_name)
                 if not ret.ok:

controller_manager/include/controller_manager/controller_manager.hpp

@@ -414,6 +414,16 @@ class ControllerManager : public rclcpp::Node
     const std::vector<controller_manager::ControllerSpec> & controllers);
 
   void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+  /**
+   * @brief determine_controller_node_options - A method that retrieves the controller defined node
+   * options and adapts them, based on if there is a params file to be loaded or the use_sim_time
+   * needs to be set
+   * @param controller - controller info
+   * @return The node options that will be set to the controller LifeCycleNode
+   */
+  rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const;
+
   diagnostic_updater::Updater diagnostics_updater_;
 
   std::shared_ptr<rclcpp::Executor> executor_;

controller_manager/src/controller_manager.cpp

@@ -574,6 +574,23 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
   controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(
     0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
 
+  // We have to fetch the parameters_file at the time of loading the controller, because this way we
+  // can load them at the creation of the LifeCycleNode and this helps in using the features such as
+  // read_only params, dynamic maps lists etc
+  // Now check if the parameters_file parameter exist
+  const std::string param_name = controller_name + ".params_file";
+  std::string parameters_file;
+
+  // Check if parameter has been declared
+  if (!has_parameter(param_name))
+  {
+    declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING);
+  }
+  if (get_parameter(param_name, parameters_file) && !parameters_file.empty())
+  {
+    controller_spec.info.parameters_file = parameters_file;
+  }
+
   return add_controller_impl(controller_spec);
 }
 
@@ -1300,28 +1317,18 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
     return nullptr;
   }
 
+  const rclcpp::NodeOptions controller_node_options = determine_controller_node_options(controller);
   if (
     controller.c->init(
-      controller.info.name, robot_description_, get_update_rate(), get_namespace()) ==
-    controller_interface::return_type::ERROR)
+      controller.info.name, robot_description_, get_update_rate(), get_namespace(),
+      controller_node_options) == controller_interface::return_type::ERROR)
   {
     to.clear();
     RCLCPP_ERROR(
       get_logger(), "Could not initialize the controller named '%s'", controller.info.name.c_str());
     return nullptr;
   }
 
-  // ensure controller's `use_sim_time` parameter matches controller_manager's
-  const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time");
-  if (use_sim_time.as_bool())
-  {
-    RCLCPP_INFO(
-      get_logger(),
-      "Setting use_sim_time=True for %s to match controller manager "
-      "(see ros2_control#325 for details)",
-      controller.info.name.c_str());
-    controller.c->get_node()->set_parameter(use_sim_time);
-  }
   executor_->add_node(controller.c->get_node()->get_node_base_interface());
   to.emplace_back(controller);
 
@@ -2593,4 +2600,39 @@ void ControllerManager::controller_activity_diagnostic_callback(
   }
 }
 
+rclcpp::NodeOptions ControllerManager::determine_controller_node_options(
+  const ControllerSpec & controller) const
+{
+  auto check_for_element = [](const auto & list, const auto & element)
+  { return std::find(list.begin(), list.end(), element) != list.end(); };
+
+  rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions().enable_logger_service(true);
+  std::vector<std::string> node_options_arguments = controller_node_options.arguments();
+  const std::string ros_args_arg = "--ros-args";
+  if (controller.info.parameters_file.has_value())
+  {
+    if (!check_for_element(node_options_arguments, ros_args_arg))
+    {
+      node_options_arguments.push_back(ros_args_arg);
+    }
+    node_options_arguments.push_back("--params-file");
+    node_options_arguments.push_back(controller.info.parameters_file.value());
+  }
+
+  // ensure controller's `use_sim_time` parameter matches controller_manager's
+  const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time");
+  if (use_sim_time.as_bool())
+  {
+    if (!check_for_element(node_options_arguments, ros_args_arg))
+    {
+      node_options_arguments.push_back(ros_args_arg);
+    }
+    node_options_arguments.push_back("-p");
+    node_options_arguments.push_back("use_sim_time:=true");
+  }
+
+  controller_node_options = controller_node_options.arguments(node_options_arguments);
+  return controller_node_options;
+}
+
 }  // namespace controller_manager