Merge branch 'jazzy' into mergify/bp/jazzy/pr-2452

Author: christophfroehlich

doc/migration.rst

@@ -100,6 +100,9 @@ hardware_interface
 * ``test_components`` was moved to its own package. Update the dependencies if you are using them. (`#1325 <https://github.com/ros-controls/ros2_control/pull/1325>`_)
 * 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.
 * A new ``get_optional`` that returns a ``std::optional`` was added to the ``CommandInterface`` and ``StateInterface``. This can be used to check if the value is available or not. (`#1976 <https://github.com/ros-controls/ros2_control/pull/1976>`_ and `#2061 <https://github.com/ros-controls/ros2_control/pull/2061>`_)
+* The ``thread_priority`` variable in the ``HardwareInfo`` struct has been deprecated in favor of newly
+  introduced ``async_params`` variable that has more options in the ``HardwareComponentParams`` struct.
+  The deprecated ``thread_priority`` variable will be removed in a future release. (`# 2477 <https://github.com/ros-controls/ros2_control/pull/2477>`_).
 
 Adaption of Command-/StateInterfaces
 ***************************************

doc/release_notes.rst

@@ -175,20 +175,19 @@ hardware_interface
 * The hardware components can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 <https://github.com/ros-controls/ros2_control/pull/1918>`_)
 * Added new ``get_optional`` method that returns ``std::optional`` of the templated type, and this can be used to check if the value is available or not (`#1976 <https://github.com/ros-controls/ros2_control/pull/1976>`_ and `#2061 <https://github.com/ros-controls/ros2_control/pull/2061>`_)
 * Added hardware components execution time and periodicity statistics diagnostics (`#2086 <https://github.com/ros-controls/ros2_control/pull/2086>`_)
+* Changes from `(PR #1688) <https://github.com/ros-controls/ros2_control/pull/1688>`_ for an overview of related changes and discussion refer to `(PR #1240) <https://github.com/ros-controls/ros2_control/pull/1240>`_:
+
+   * ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp <https://github.com/ros-controls/ros2_control/tree/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/hardware_info.hpp>`__).
+   * The memory for storing the value of a ``Command-/StateInterfaces`` is no longer allocated in the hardware but instead in the ``Command-/StateInterfaces`` itself.
+   * To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map<std::string, InterfaceDescription>``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the  ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` function by creating some custom ``Command-/StateInterfaces``.
+
+* The ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` methods will now only receive the start/stop interfaces that belong to the hardware component instead of everything (`#2120 <https://github.com/ros-controls/ros2_control/pull/2120>`_)
+* The asynchronous components now support two scheduling policies: ``synchronized`` and ``detached`` and other properties to configure them (`#2477 <https://github.com/ros-controls/ros2_control/pull/2477>`_).
 
 joint_limits
 ************
 * Add header to import limits from standard URDF definition (`#1298 <https://github.com/ros-controls/ros2_control/pull/1298>`_)
 
-Adaption of Command-/StateInterfaces
-***************************************
-Changes from `(PR #1688) <https://github.com/ros-controls/ros2_control/pull/1688>`_ for an overview of related changes and discussion refer to `(PR #1240) <https://github.com/ros-controls/ros2_control/pull/1240>`_.
-
-* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp <https://github.com/ros-controls/ros2_control/tree/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/hardware_info.hpp>`__).
-* The memory for storing the value of a ``Command-/StateInterfaces`` is no longer allocated in the hardware but instead in the ``Command-/StateInterfaces`` itself.
-* To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map<std::string, InterfaceDescription>``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the  ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` function by creating some custom ``Command-/StateInterfaces``.
-* The ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` methods will now only receive the start/stop interfaces that belong to the hardware component instead of everything (`#2120 <https://github.com/ros-controls/ros2_control/pull/2120>`_)
-
 ros2controlcli
 **************
 * Spawner colours were added to ``list_controllers`` depending upon active or inactive (`#1409 <https://github.com/ros-controls/ros2_control/pull/1409>`_)

hardware_interface/doc/asynchronous_components.rst

@@ -13,7 +13,15 @@ Parameters
 The following parameters can be set in the ``ros2_control`` hardware configuration to run the hardware component asynchronously:
 
 * ``is_async``: (optional) If set to ``true``, the hardware component will run asynchronously. Default is ``false``.
+
+Under the ``ros2_control`` tag, a ``properties`` tag can be added to specify the following parameters of the asynchronous hardware component:
+
 * ``thread_priority``: (optional) The priority of the thread that runs the hardware component. The priority is an integer value between 0 and 99. The default value is 50.
+* ``affinity``: (optional) The CPU affinity of the thread that runs the hardware component. The affinity is a list of CPU core IDs. The default value is an empty list, which means that the thread can run on any CPU core.
+* ``scheduling_policy``: (optional) The scheduling policy of the thread that runs the hardware component. The scheduling policy can be one of the following values:
+  * ``synchronized`` (default): The thread will run with the synchronized with the main controller_manager thread. The controller_manager is responsible for triggering the read and write calls of the hardware component.
+  * ``detached``: The thread will run independently of the main controller_manager thread. The hardware component will manage its own timing for triggering the read and write calls.
+* ``print_warnings``: (optional) If set to ``true``, a warning will be printed if the thread is not able to meet its timing requirements. Default is ``true``.
 
 .. note::
   The thread priority is only used when the hardware component is run asynchronously.
@@ -59,7 +67,10 @@ For a RRBot with multimodal gripper and external sensor:
       <state_interface name="digital_input2"/>
     </gpio>
   </ros2_control>
-  <ros2_control name="MultimodalGripper" type="actuator" is_async="true" thread_priority="30">
+  <ros2_control name="MultimodalGripper" type="actuator" is_async="true">
+    <properties>
+      <async affinity="[2,4]" scheduling_policy="synchronized" print_warnings="true" thread_priority="30"/>
+    </properties>
     <hardware>
       <plugin>ros2_control_demo_hardware/MultimodalGripper</plugin>
     </hardware>

hardware_interface/include/hardware_interface/hardware_component_interface.hpp

@@ -140,13 +140,25 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
     logger_ = logger_copy.get_child(
       "hardware_component." + params.hardware_info.type + "." + params.hardware_info.name);
     info_ = params.hardware_info;
-    if (info_.is_async)
+    if (params.hardware_info.is_async)
     {
-      RCLCPP_INFO_STREAM(
-        get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority);
+      realtime_tools::AsyncFunctionHandlerParams async_thread_params;
+      async_thread_params.thread_priority = info_.async_params.thread_priority;
+      async_thread_params.scheduling_policy =
+        realtime_tools::AsyncSchedulingPolicy(info_.async_params.scheduling_policy);
+      async_thread_params.cpu_affinity_cores = info_.async_params.cpu_affinity_cores;
+      async_thread_params.clock = params.clock;
+      async_thread_params.logger = params.logger;
+      async_thread_params.exec_rate = params.hardware_info.rw_rate;
+      async_thread_params.print_warnings = info_.async_params.print_warnings;
+      RCLCPP_INFO(
+        get_logger(), "Starting async handler with scheduler priority: %d and policy : %s",
+        info_.async_params.thread_priority,
+        async_thread_params.scheduling_policy.to_string().c_str());
       async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
+      const bool is_sensor_type = (info_.type == "sensor");
       async_handler_->init(
-        [this](const rclcpp::Time & time, const rclcpp::Duration & period)
+        [this, is_sensor_type](const rclcpp::Time & time, const rclcpp::Duration & period)
         {
           const auto read_start_time = std::chrono::steady_clock::now();
           const auto ret_read = read(time, period);
@@ -159,7 +171,9 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
           {
             return ret_read;
           }
-          if (info_.type != "sensor")
+          if (
+            !is_sensor_type &&
+            this->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
           {
             const auto write_start_time = std::chrono::steady_clock::now();
             const auto ret_write = write(time, period);
@@ -173,7 +187,7 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
           }
           return return_type::OK;
         },
-        info_.thread_priority);
+        async_thread_params);
       async_handler_->start_thread();
     }
 
@@ -493,8 +507,8 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
       status.successful = result.first;
       if (!status.successful)
       {
-        RCLCPP_WARN(
-          get_logger(),
+        RCLCPP_WARN_EXPRESSION(
+          get_logger(), info_.async_params.print_warnings,
           "Trigger read/write called while the previous async trigger is still in progress for "
           "hardware interface : '%s'. Failed to trigger read/write cycle!",
           info_.name.c_str());
@@ -714,6 +728,19 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
    */
   const HardwareInfo & get_hardware_info() const { return info_; }
 
+  /// Pause any asynchronous operations.
+  /**
+   * This method is called to pause any ongoing asynchronous operations, such as read/write cycles.
+   * It is typically used during lifecycle transitions or when the hardware needs to be paused.
+   */
+  void pause_async_operations()
+  {
+    if (async_handler_)
+    {
+      async_handler_->pause_execution();
+    }
+  }
+
   /// Prepare for the activation of the hardware.
   /**
    * This method is called before the hardware is activated by the resource manager.

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -229,6 +229,18 @@ struct InterfaceDescription
   HandleDataType get_data_type() const { return HandleDataType(interface_info.data_type); }
 };
 
+struct HardwareAsyncParams
+{
+  /// Thread priority for the async worker thread
+  int thread_priority = 50;
+  /// Scheduling policy for the async worker thread
+  std::string scheduling_policy = "synchronized";
+  /// CPU affinity cores for the async worker thread
+  std::vector<int> cpu_affinity_cores = {};
+  /// Whether to print warnings when the async thread doesn't meet its deadline
+  bool print_warnings = true;
+};
+
 /// This structure stores information about hardware defined in a robot's URDF.
 struct HardwareInfo
 {
@@ -243,7 +255,9 @@ struct HardwareInfo
   /// Component is async
   bool is_async;
   /// Async thread priority
-  int thread_priority;
+  [[deprecated("Use async_params instead.")]] int thread_priority;
+  /// Async Parameters
+  HardwareAsyncParams async_params;
   /// Name of the pluginlib plugin of the hardware that will be loaded.
   std::string hardware_plugin_name;
   /// (Optional) Key-value pairs for hardware parameters.

hardware_interface/include/hardware_interface/lexical_casts.hpp

@@ -32,6 +32,19 @@ namespace hardware_interface
 */
 double stod(const std::string & s);
 
+/**
+ * \brief Convert a string to lower case.
+ * \param string The input string.
+ * \return The lower case version of the input string.
+ */
+std::string to_lower_case(const std::string & string);
+
+/**
+ * \brief Parse a boolean value from a string.
+ * \param bool_string The input string, can be "true", "false" (case insensitive)
+ * \return The parsed boolean value.
+ * \throws std::invalid_argument if the string is not a valid boolean representation.
+ */
 bool parse_bool(const std::string & bool_string);
 
 template <typename T>

hardware_interface/src/component_parser.cpp

@@ -55,6 +55,8 @@ constexpr const auto kStateInterfaceTag = "state_interface";
 constexpr const auto kMinTag = "min";
 constexpr const auto kMaxTag = "max";
 constexpr const auto kLimitsTag = "limits";
+constexpr const auto kPropertiesTag = "properties";
+constexpr const auto kAsyncTag = "async";
 constexpr const auto kEnableAttribute = "enable";
 constexpr const auto kInitialValueTag = "initial_value";
 constexpr const auto kMimicAttribute = "mimic";
@@ -68,6 +70,9 @@ constexpr const auto kOffsetAttribute = "offset";
 constexpr const auto kReadWriteRateAttribute = "rw_rate";
 constexpr const auto kIsAsyncAttribute = "is_async";
 constexpr const auto kThreadPriorityAttribute = "thread_priority";
+constexpr const auto kAffinityCoresAttribute = "affinity";
+constexpr const auto kSchedulingPolicyAttribute = "scheduling_policy";
+constexpr const auto kPrintWarningsAttribute = "print_warnings";
 
 }  // namespace
 
@@ -672,6 +677,7 @@ HardwareInfo parse_resource_from_xml(
   hardware.is_async = parse_is_async_attribute(ros2_control_it);
   hardware.thread_priority = hardware.is_async ? parse_thread_priority_attribute(ros2_control_it)
                                                : std::numeric_limits<int>::max();
+  hardware.async_params.thread_priority = hardware.thread_priority;
 
   // Parse everything under ros2_control tag
   hardware.hardware_plugin_name = "";
@@ -698,6 +704,42 @@ HardwareInfo parse_resource_from_xml(
         hardware.hardware_parameters = parse_parameters_from_xml(params_it);
       }
     }
+    else if (std::string(kPropertiesTag) == ros2_control_child_it->Name())
+    {
+      const auto * async_it = ros2_control_child_it->FirstChildElement(kAsyncTag);
+      if (async_it)
+      {
+        // Async properties are defined
+        try
+        {
+          if (async_it->FindAttribute(kAffinityCoresAttribute))
+          {
+            hardware.async_params.cpu_affinity_cores =
+              parse_array<int>(get_attribute_value(async_it, kAffinityCoresAttribute, kAsyncTag));
+          }
+          if (async_it->FindAttribute(kSchedulingPolicyAttribute))
+          {
+            hardware.async_params.scheduling_policy =
+              to_lower_case(get_attribute_value(async_it, kSchedulingPolicyAttribute, kAsyncTag));
+          }
+          if (async_it->FindAttribute(kThreadPriorityAttribute))
+          {
+            hardware.async_params.thread_priority = parse_thread_priority_attribute(async_it);
+            hardware.thread_priority = hardware.async_params.thread_priority;
+          }
+          if (async_it->FindAttribute(kPrintWarningsAttribute))
+          {
+            hardware.async_params.print_warnings =
+              parse_bool(get_attribute_value(async_it, kPrintWarningsAttribute, kAsyncTag));
+          }
+        }
+        catch (const std::exception & e)
+        {
+          throw std::runtime_error(
+            fmt::format(FMT_COMPILE("Error parsing {} tag: {}"), kAsyncTag, e.what()));
+        }
+      }
+    }
     else if (std::string(kJointTag) == ros2_control_child_it->Name())
     {
       hardware.joints.push_back(parse_component_from_xml(ros2_control_child_it));

hardware_interface/src/hardware_component.cpp

@@ -97,6 +97,7 @@ const rclcpp_lifecycle::State & HardwareComponent::configure()
   std::unique_lock<std::recursive_mutex> lock(component_mutex_);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
   {
+    impl_->pause_async_operations();
     switch (impl_->on_configure(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
@@ -124,6 +125,7 @@ const rclcpp_lifecycle::State & HardwareComponent::cleanup()
   impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
+    impl_->pause_async_operations();
     switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
@@ -149,6 +151,7 @@ const rclcpp_lifecycle::State & HardwareComponent::shutdown()
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
   {
+    impl_->pause_async_operations();
     switch (impl_->on_shutdown(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
@@ -177,6 +180,7 @@ const rclcpp_lifecycle::State & HardwareComponent::activate()
   }
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
+    impl_->pause_async_operations();
     impl_->prepare_for_activation();
     switch (impl_->on_activate(impl_->get_lifecycle_state()))
     {
@@ -205,6 +209,7 @@ const rclcpp_lifecycle::State & HardwareComponent::deactivate()
   impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
+    impl_->pause_async_operations();
     switch (impl_->on_deactivate(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
@@ -233,6 +238,7 @@ const rclcpp_lifecycle::State & HardwareComponent::error()
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
   {
+    impl_->pause_async_operations();
     switch (impl_->on_error(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS: