Merge branch 'master' into async_hardware_components

.github/workflows/humble-semi-binary-downstream-build.yml

@@ -0,0 +1,49 @@
+name: Humble Downstream Build
+# description: 'Build & test downstream packages from source.'
+# author: Christoph Froehlich <[email protected]>
+
+on:
+  workflow_dispatch:
+  pull_request:
+    branches:
+      - humble
+    paths:
+      - '**.hpp'
+      - '**.h'
+      - '**.cpp'
+      - '**.py'
+      - '**.yaml'
+      - '.github/workflows/humble-semi-binary-downstream-build.yml'
+      - '**/package.xml'
+      - '**/CMakeLists.txt'
+      - 'ros_controls.humble.repos'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true
+
+jobs:
+  build-downstream:
+    uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
+    with:
+      ros_distro: humble
+      ros_repo: testing
+      ref_for_scheduled_build: humble
+      upstream_workspace: ros2_control.humble.repos
+      # we don't test this repository, we just build it
+      not_test_build: true
+      # we test the downstream packages, which are part of our organization
+      downstream_workspace: ros_controls.humble.repos
+      not_test_downstream: false
+  build-downstream-3rd-party:
+    uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
+    with:
+      ros_distro: humble
+      ros_repo: testing
+      ref_for_scheduled_build: humble
+      upstream_workspace: ros2_control.humble.repos
+      # we don't test this repository, we just build it
+      not_test_build: true
+      # we don't test the downstream packages, which are outside of our organization
+      downstream_workspace: downstream.humble.repos
+      not_test_downstream: true

.github/workflows/jazzy-semi-binary-downstream-build.yml

@@ -0,0 +1,49 @@
+name: Jazzy Downstream Build
+# description: 'Build & test downstream packages from source.'
+# author: Christoph Froehlich <[email protected]>
+
+on:
+  workflow_dispatch:
+  pull_request:
+    branches:
+      - master
+    paths:
+      - '**.hpp'
+      - '**.h'
+      - '**.cpp'
+      - '**.py'
+      - '**.yaml'
+      - '.github/workflows/jazzy-semi-binary-downstream-build.yml'
+      - '**/package.xml'
+      - '**/CMakeLists.txt'
+      - 'ros_controls.jazzy.repos'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true
+
+jobs:
+  build-downstream:
+    uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
+    with:
+      ros_distro: jazzy
+      ros_repo: testing
+      ref_for_scheduled_build: master
+      upstream_workspace: ros2_control.jazzy.repos
+      # we don't test this repository, we just build it
+      not_test_build: true
+      # we test the downstream packages, which are part of our organization
+      downstream_workspace: ros_controls.jazzy.repos
+      not_test_downstream: false
+  build-downstream-3rd-party:
+    uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
+    with:
+      ros_distro: jazzy
+      ros_repo: testing
+      ref_for_scheduled_build: master
+      upstream_workspace: ros2_control.jazzy.repos
+      # we don't test this repository, we just build it
+      not_test_build: true
+      # we don't test the downstream packages, which are outside of our organization
+      downstream_workspace: downstream.jazzy.repos
+      not_test_downstream: true

.github/workflows/rolling-semi-binary-downstream-build.yml

@@ -0,0 +1,57 @@
+name: Rolling Downstream Build
+# description: 'Build & test downstream packages from source.'
+# author: Christoph Froehlich <[email protected]>
+
+on:
+  workflow_dispatch:
+  pull_request:
+    branches:
+      - master
+    paths:
+      - '**.hpp'
+      - '**.h'
+      - '**.cpp'
+      - '**.py'
+      - '**.yaml'
+      - '.github/workflows/rolling-semi-binary-downstream-build.yml'
+      - '**/package.xml'
+      - '**/CMakeLists.txt'
+      - 'ros_controls.rolling.repos'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true
+
+jobs:
+  build-downstream:
+    uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
+    strategy:
+      fail-fast: false
+      matrix:
+        ROS_DISTRO: [rolling]
+    with:
+      ros_distro: ${{ matrix.ROS_DISTRO }}
+      ros_repo: testing
+      ref_for_scheduled_build: master
+      upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos
+      # we don't test this repository, we just build it
+      not_test_build: true
+      # we test the downstream packages, which are part of our organization
+      downstream_workspace: ros_controls.${{ matrix.ROS_DISTRO }}.repos
+      not_test_downstream: false
+  build-downstream-3rd-party:
+    uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
+    strategy:
+      fail-fast: false
+      matrix:
+        ROS_DISTRO: [rolling]
+    with:
+      ros_distro: ${{ matrix.ROS_DISTRO }}
+      ros_repo: testing
+      ref_for_scheduled_build: master
+      upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos
+      # we don't test this repository, we just build it
+      not_test_build: true
+      # we don't test the downstream packages, which are outside of our organization
+      downstream_workspace: downstream.${{ matrix.ROS_DISTRO }}.repos
+      not_test_downstream: true

controller_manager/CMakeLists.txt

@@ -58,10 +58,12 @@ target_link_libraries(ros2_control_node PRIVATE
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)
+  find_package(example_interfaces REQUIRED)
 
   # Plugin Libraries that are built and installed for use in testing
   add_library(test_controller SHARED test/test_controller/test_controller.cpp)
   target_link_libraries(test_controller PUBLIC controller_manager)
+  ament_target_dependencies(test_controller PUBLIC example_interfaces)
   target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
   pluginlib_export_plugin_description_file(controller_interface test/test_controller/test_controller.xml)
   install(
@@ -210,6 +212,7 @@ if(BUILD_TESTING)
   )
   target_link_libraries(test_hardware_spawner
     controller_manager
+    test_controller
     ros2_control_test_assets::ros2_control_test_assets
   )
 

controller_manager/controller_manager/controller_manager_services.py

@@ -55,6 +55,35 @@ class ServiceNotFoundError(Exception):
     pass
 
 
+class SingletonServiceCaller:
+    """
+    Singleton class to call services of controller manager.
+
+    This class is used to create a service client for a given service name.
+    If the service client already exists, it returns the existing client.
+    It is used to avoid creating multiple service clients for the same service name.
+
+    It needs Node object, service type and fully qualified service name to create a service client.
+
+    """
+
+    _clients = {}
+
+    def __new__(cls, node, service_type, fully_qualified_service_name):
+        if (node, fully_qualified_service_name) not in cls._clients:
+            cls._clients[(node, fully_qualified_service_name)] = node.create_client(
+                service_type, fully_qualified_service_name
+            )
+            node.get_logger().debug(
+                f"{bcolors.MAGENTA}Creating a new service client : {fully_qualified_service_name} with node : {node.get_name()}{bcolors.ENDC}"
+            )
+
+        node.get_logger().debug(
+            f"{bcolors.OKBLUE}Returning the existing service client : {fully_qualified_service_name} for node : {node.get_name()}{bcolors.ENDC}"
+        )
+        return cls._clients[(node, fully_qualified_service_name)]
+
+
 def service_caller(
     node,
     service_name,
@@ -92,7 +121,7 @@ def service_caller(
     fully_qualified_service_name = (
         f"{namespace}/{service_name}" if not service_name.startswith("/") else service_name
     )
-    cli = node.create_client(service_type, fully_qualified_service_name)
+    cli = SingletonServiceCaller(node, service_type, fully_qualified_service_name)
 
     while not cli.service_is_ready():
         node.get_logger().info(

controller_manager/controller_manager/spawner.py

@@ -26,6 +26,7 @@
     load_controller,
     switch_controllers,
     unload_controller,
+    set_controller_parameters,
     set_controller_parameters_from_param_files,
     bcolors,
 )
@@ -145,6 +146,12 @@ def main(args=None):
         action="store_true",
         required=False,
     )
+    parser.add_argument(
+        "--controller-ros-args",
+        help="The --ros-args to be passed to the controller node for remapping topics etc",
+        default=None,
+        required=False,
+    )
 
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
@@ -203,6 +210,15 @@ def main(args=None):
                     + bcolors.ENDC
                 )
             else:
+                if controller_ros_args := args.controller_ros_args:
+                    if not set_controller_parameters(
+                        node,
+                        controller_manager_name,
+                        controller_name,
+                        "node_options_args",
+                        controller_ros_args.split(),
+                    ):
+                        return 1
                 if param_files:
                     if not set_controller_parameters_from_param_files(
                         node,
@@ -342,6 +358,7 @@ def main(args=None):
         node.get_logger().fatal(str(err))
         return 1
     finally:
+        node.destroy_node()
         rclpy.shutdown()
 
 

controller_manager/doc/userdoc.rst

@@ -142,7 +142,7 @@ There are two scripts to interact with controller manager from launch files:
 
     $ ros2 run controller_manager spawner -h
     usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT]
-                  [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT]
+                  [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT] [--controller-ros-args CONTROLLER_ROS_ARGS]
                   controller_names [controller_names ...]
 
     positional arguments:
@@ -167,6 +167,8 @@ There are two scripts to interact with controller manager from launch files:
                             Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused
                             simulations at startup
       --activate-as-group   Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether
+      --controller-ros-args CONTROLLER_ROS_ARGS
+                            The --ros-args to be passed to the controller node for remapping topics etc
 
 
 The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files:

controller_manager/package.xml

@@ -36,6 +36,7 @@
   <test_depend>python3-coverage</test_depend>
   <test_depend>hardware_interface_testing</test_depend>
   <test_depend>ros2_control_test_assets</test_depend>
+  <test_depend>example_interfaces</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

controller_manager/src/controller_manager.cpp

@@ -616,6 +616,17 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
     controller_spec.info.fallback_controllers_names = fallback_controllers;
   }
 
+  const std::string node_options_args_param = controller_name + ".node_options_args";
+  std::vector<std::string> node_options_args;
+  if (!has_parameter(node_options_args_param))
+  {
+    declare_parameter(node_options_args_param, rclcpp::ParameterType::PARAMETER_STRING_ARRAY);
+  }
+  if (get_parameter(node_options_args_param, node_options_args) && !node_options_args.empty())
+  {
+    controller_spec.info.node_options_args = node_options_args;
+  }
+
   return add_controller_impl(controller_spec);
 }
 
@@ -3448,6 +3459,18 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options(
     node_options_arguments.push_back(arg);
   }
 
+  // Add deprecation notice if the arguments are from the controller_manager node
+  if (
+    check_for_element(node_options_arguments, RCL_REMAP_FLAG) ||
+    check_for_element(node_options_arguments, RCL_SHORT_REMAP_FLAG))
+  {
+    RCLCPP_WARN(
+      get_logger(),
+      "The use of remapping arguments to the controller_manager node is deprecated. Please use the "
+      "'--controller-ros-args' argument of the spawner to pass remapping arguments to the "
+      "controller node.");
+  }
+
   for (const auto & parameters_file : controller.info.parameters_files)
   {
     if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG))
@@ -3470,6 +3493,18 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options(
     node_options_arguments.push_back("use_sim_time:=true");
   }
 
+  // Add options parsed through the spawner
+  if (
+    !controller.info.node_options_args.empty() &&
+    !check_for_element(controller.info.node_options_args, RCL_ROS_ARGS_FLAG))
+  {
+    node_options_arguments.push_back(RCL_ROS_ARGS_FLAG);
+  }
+  for (const auto & arg : controller.info.node_options_args)
+  {
+    node_options_arguments.push_back(arg);
+  }
+
   std::string arguments;
   arguments.reserve(1000);
   for (const auto & arg : node_options_arguments)

controller_manager/test/test_controller/test_controller.cpp

@@ -101,6 +101,17 @@ CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; }
 
 CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  const std::string service_name = get_node()->get_name() + std::string("/set_bool");
+  service_ = get_node()->create_service<example_interfaces::srv::SetBool>(
+    service_name,
+    [this](
+      const std::shared_ptr<example_interfaces::srv::SetBool::Request> request,
+      std::shared_ptr<example_interfaces::srv::SetBool::Response> response)
+    {
+      RCLCPP_INFO_STREAM(
+        get_node()->get_logger(), "Setting response to " << std::boolalpha << request->data);
+      response->success = request->data;
+    });
   return CallbackReturn::SUCCESS;
 }
 

controller_manager/test/test_controller/test_controller.hpp

@@ -15,11 +15,14 @@
 #ifndef TEST_CONTROLLER__TEST_CONTROLLER_HPP_
 #define TEST_CONTROLLER__TEST_CONTROLLER_HPP_
 
+#include <memory>
 #include <string>
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
 #include "controller_manager/visibility_control.h"
+#include "example_interfaces/srv/set_bool.hpp"
+#include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
 namespace test_controller
@@ -68,10 +71,9 @@ class TestController : public controller_interface::ControllerInterface
   CONTROLLER_MANAGER_PUBLIC
   std::vector<double> get_state_interface_data() const;
 
-  const std::string & getRobotDescription() const;
-
   void set_external_commands_for_testing(const std::vector<double> & commands);
 
+  rclcpp::Service<example_interfaces::srv::SetBool>::SharedPtr service_;
   unsigned int internal_counter = 0;
   bool simulate_cleanup_failure = false;
   // Variable where we store when cleanup was called, pointer because the controller