Merge branch 'master' into tpo-add_semantic_cmd_interface

controller_interface/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.22.0 (2024-12-20)
+-------------------
+* Fixed typo. Added s to state_interfaces\_ (`#1930 <https://github.com/ros-controls/ros2_control/issues/1930>`_)
+* [CI] Add clang job, setup concurrency, use rt_tools humble branch (`#1910 <https://github.com/ros-controls/ros2_control/issues/1910>`_)
+* Contributors: Christoph Fröhlich, louietouie
+
 4.21.0 (2024-12-06)
 -------------------
 * [Diagnostics] Add diagnostics of execution time and periodicity of the controllers and controller_manager (`#1871 <https://github.com/ros-controls/ros2_control/issues/1871>`_)

controller_interface/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_interface</name>
-  <version>4.21.0</version>
+  <version>4.22.0</version>
   <description>Description of controller_interface</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

controller_manager/CHANGELOG.rst

@@ -2,6 +2,19 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.22.0 (2024-12-20)
+-------------------
+* Async Hardware Components (`#1567 <https://github.com/ros-controls/ros2_control/issues/1567>`_)
+* Add controller node options args to be able to set controller specific node arguments (`#1713 <https://github.com/ros-controls/ros2_control/issues/1713>`_)
+* Use singleton approach to store and reuse the service clients (`#1949 <https://github.com/ros-controls/ros2_control/issues/1949>`_)
+* Increase the max and min periodicity tolerances to fix flaky tests (`#1937 <https://github.com/ros-controls/ros2_control/issues/1937>`_)
+* Fix the spawner to support full wildcard parameter entries (`#1933 <https://github.com/ros-controls/ros2_control/issues/1933>`_)
+* Suppress unnecessary warnings in clock received validation (`#1935 <https://github.com/ros-controls/ros2_control/issues/1935>`_)
+* Optimize the valid time check in the update loop (`#1923 <https://github.com/ros-controls/ros2_control/issues/1923>`_)
+* [CI] Add clang job, setup concurrency, use rt_tools humble branch (`#1910 <https://github.com/ros-controls/ros2_control/issues/1910>`_)
+* Update CPU affinity parameter to be able to set multiple CPUs (`#1915 <https://github.com/ros-controls/ros2_control/issues/1915>`_)
+* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Takashi Sato
+
 4.21.0 (2024-12-06)
 -------------------
 * Use the .hpp headers from realtime_tools package (`#1916 <https://github.com/ros-controls/ros2_control/issues/1916>`_)

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/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,

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/include/controller_manager/controller_manager.hpp

@@ -222,15 +222,6 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   unsigned int get_update_rate() const;
 
-  /// Deletes all async controllers and components.
-  /**
-   * Needed to join the threads immediately after the control loop is ended
-   * to avoid unnecessary iterations. Otherwise
-   * the threads will be joined only when the controller manager gets destroyed.
-   */
-  CONTROLLER_MANAGER_PUBLIC
-  void shutdown_async_controllers_and_components();
-
 protected:
   CONTROLLER_MANAGER_PUBLIC
   void init_services();

controller_manager/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_manager</name>
-  <version>4.21.0</version>
+  <version>4.22.0</version>
   <description>Description of controller_manager</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>
@@ -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);
 }
 
@@ -2681,11 +2692,6 @@ std::pair<std::string, std::string> ControllerManager::split_command_interface(
 
 unsigned int ControllerManager::get_update_rate() const { return update_rate_; }
 
-void ControllerManager::shutdown_async_controllers_and_components()
-{
-  resource_manager_->shutdown_async_components();
-}
-
 void ControllerManager::propagate_deactivation_of_chained_mode(
   const std::vector<ControllerSpec> & controllers)
 {
@@ -3453,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))
@@ -3475,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/src/ros2_control_node.cpp

@@ -147,8 +147,6 @@ int main(int argc, char ** argv)
           std::this_thread::sleep_until(next_iteration_time);
         }
       }
-
-      cm->shutdown_async_controllers_and_components();
     });
 
   executor->add_node(cm);

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

controller_manager/test/test_spawner_unspawner.cpp

@@ -664,6 +664,45 @@ TEST_F(TestLoadController, spawner_with_many_controllers)
   }
 }
 
+TEST_F(TestLoadController, test_spawner_parsed_controller_ros_args)
+{
+  ControllerManagerRunner cm_runner(this);
+  cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+  cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+  std::stringstream ss;
+
+  EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0);
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
+
+  // Now as the controller is active, we can call check for the service
+  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("set_bool_client");
+  auto set_bool_service = node->create_client<example_interfaces::srv::SetBool>("/set_bool");
+  ASSERT_FALSE(set_bool_service->wait_for_service(std::chrono::seconds(2)));
+  ASSERT_FALSE(set_bool_service->service_is_ready());
+  // Now check the service availability in the controller namespace
+  auto ctrl_1_set_bool_service =
+    node->create_client<example_interfaces::srv::SetBool>("/ctrl_1/set_bool");
+  ASSERT_TRUE(ctrl_1_set_bool_service->wait_for_service(std::chrono::seconds(2)));
+  ASSERT_TRUE(ctrl_1_set_bool_service->service_is_ready());
+
+  // Now test the remapping of the service name with the controller_ros_args
+  EXPECT_EQ(
+    call_spawner(
+      "ctrl_2 -c test_controller_manager --controller-ros-args '-r /ctrl_2/set_bool:=/set_bool'"),
+    0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
+
+  // Now as the controller is active, we can call check for the remapped service
+  ASSERT_TRUE(set_bool_service->wait_for_service(std::chrono::seconds(2)));
+  ASSERT_TRUE(set_bool_service->service_is_ready());
+  // Now check the service availability in the controller namespace
+  auto ctrl_2_set_bool_service =
+    node->create_client<example_interfaces::srv::SetBool>("/ctrl_2/set_bool");
+  ASSERT_FALSE(ctrl_2_set_bool_service->wait_for_service(std::chrono::seconds(2)));
+  ASSERT_FALSE(ctrl_2_set_bool_service->service_is_ready());
+}
+
 class TestLoadControllerWithoutRobotDescription
 : public ControllerManagerFixture<controller_manager::ControllerManager>
 {

controller_manager_msgs/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package controller_manager_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.22.0 (2024-12-20)
+-------------------
+
 4.21.0 (2024-12-06)
 -------------------
 

controller_manager_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>controller_manager_msgs</name>
-  <version>4.21.0</version>
+  <version>4.22.0</version>
   <description>Messages and services for the controller manager.</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

doc/release_notes.rst

@@ -78,6 +78,7 @@ controller_manager
 * The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 <https://github.com/ros-controls/ros2_control/pull/1639>`_).
 * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 <https://github.com/ros-controls/ros2_control/pull/1640>`_).
 * Added support for the wildcard entries for the controller configuration files (`#1724 <https://github.com/ros-controls/ros2_control/pull/1724>`_).
+* The spawner now supports the ``--controller-ros-args`` argument to pass the ROS 2 node arguments to the controller node to be able to remap topics, services and etc (`#1713 <https://github.com/ros-controls/ros2_control/pull/1713>`_).
 * The spawner now supports parsing multiple ``-p`` or ``--param-file`` arguments, this should help in loading multiple parameter files for a controller or for multiple controllers (`#1805 <https://github.com/ros-controls/ros2_control/pull/1805>`_).
 * ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 <https://github.com/ros-controls/ros2_control/pull/1790>`_).
 * ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 <https://github.com/ros-controls/ros2_control/pull/1810>`_).
@@ -158,6 +159,7 @@ hardware_interface
 * With (`#1683 <https://github.com/ros-controls/ros2_control/pull/1683>`_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 <https://github.com/ros-controls/ros2_control/pull/1240>`_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles.
 * With (`#1421 <https://github.com/ros-controls/ros2_control/pull/1421>`_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file.
 * With (`#1763 <https://github.com/ros-controls/ros2_control/pull/1763>`_) parsing for SDF published to ``robot_description`` topic is now also supported.
+* With (`#1567 <https://github.com/ros-controls/ros2_control/pull/1567>`_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components.
 
 joint_limits
 ************

hardware_interface/CHANGELOG.rst

@@ -2,6 +2,16 @@
 Changelog for package hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.22.0 (2024-12-20)
+-------------------
+* Propagate read/write rate to the HardwareInfo properly (`#1928 <https://github.com/ros-controls/ros2_control/issues/1928>`_)
+* Async Hardware Components (`#1567 <https://github.com/ros-controls/ros2_control/issues/1567>`_)
+* Add controller node options args to be able to set controller specific node arguments (`#1713 <https://github.com/ros-controls/ros2_control/issues/1713>`_)
+* Make get_name() return a const reference (`#1952 <https://github.com/ros-controls/ros2_control/issues/1952>`_)
+* Let sensors also export state interfaces of joints (`#1885 <https://github.com/ros-controls/ros2_control/issues/1885>`_)
+* [CI] Add clang job, setup concurrency, use rt_tools humble branch (`#1910 <https://github.com/ros-controls/ros2_control/issues/1910>`_)
+* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Takashi Sato
+
 4.21.0 (2024-12-06)
 -------------------
 * [Feature] Choose different read and write rate for the hardware components (`#1570 <https://github.com/ros-controls/ros2_control/issues/1570>`_)

hardware_interface/CMakeLists.txt

@@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp_lifecycle
   rcpputils
   rcutils
+  realtime_tools
   TinyXML2
   tinyxml2_vendor
   joint_limits