Add detach async policy for rate critical frameworks (#2477)

Author: saikishor

doc/migration.rst

@@ -54,3 +54,7 @@ hardware_interface
   have been updated from passing a ``const HardwareInfo &`` to passing a ``const
   HardwareComponentParams &`` (`# 2323 <https://github.com/ros-controls/ros2_control/pull/2323>`_).
   The old signatures are deprecated and will be removed in a future release.
+
+* 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>`_).

doc/release_notes.rst

@@ -17,6 +17,7 @@ hardware_interface
 * The hardware interface is now treated similarly as ERROR, when a hardware component returns ERROR on the read cycle (`#2334 <https://github.com/ros-controls/ros2_control/pull/2334>`_).
 * The controllers are now deactivated when a hardware component returns DEACTIVATE on the write cycle. The parameter ``deactivate_controllers_on_hardware_self_deactivate`` is added to control this behavior temporarily. It is recommended to set this parameter to true in order to avoid controllers to use inactive hardware components and to avoid any unexpected behavior. This feature parameter will be removed in future releases and will be defaulted to true (`#2334 <https://github.com/ros-controls/ros2_control/pull/2334>`_ & `#2501 <https://github.com/ros-controls/ros2_control/pull/2501>`_).
 * The controllers are not allowed to be activated when the hardware component is in INACTIVE state. The parameter ``allow_controller_activation_with_inactive_hardware`` is added to control this behavior temporarily. It is recommended to set this parameter to false in order to avoid controllers to use inactive hardware components and to avoid any unexpected behavior. This feature parameter will be removed in future releases and will be defaulted to false (`#2347 <https://github.com/ros-controls/ros2_control/pull/2347>`_).
+* 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>`_).
 
 ros2controlcli
 **************

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

@@ -131,13 +131,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);
@@ -150,7 +162,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);
@@ -164,7 +178,7 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
           }
           return return_type::OK;
         },
-        info_.thread_priority);
+        async_thread_params);
       async_handler_->start_thread();
     }
 
@@ -485,8 +499,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());
@@ -739,6 +753,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

@@ -247,6 +247,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
 {
@@ -261,7 +273,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:

hardware_interface/src/lexical_casts.cpp

@@ -62,12 +62,19 @@ double stod(const std::string & s)
   throw std::invalid_argument("Failed converting string to real number");
 }
 
+std::string to_lower_case(const std::string & string)
+{
+  std::string lower_case_string = string;
+  std::transform(
+    lower_case_string.begin(), lower_case_string.end(), lower_case_string.begin(),
+    [](unsigned char c) { return std::tolower(c); });
+  return lower_case_string;
+}
+
 bool parse_bool(const std::string & bool_string)
 {
   // Copy input to temp and make lowercase
-  std::string temp = bool_string;
-  std::transform(
-    temp.begin(), temp.end(), temp.begin(), [](unsigned char c) { return std::tolower(c); });
+  std::string temp = to_lower_case(bool_string);
 
   if (temp == "true")
   {

hardware_interface/test/test_component_parser.cpp

@@ -1431,6 +1431,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components)
   ASSERT_THAT(hardware_info.joints, SizeIs(1));
   ASSERT_TRUE(hardware_info.is_async);
   ASSERT_EQ(hardware_info.thread_priority, 30);
+  ASSERT_EQ(hardware_info.async_params.thread_priority, 30);
+  ASSERT_EQ(hardware_info.async_params.scheduling_policy, "detached");
+  ASSERT_FALSE(hardware_info.async_params.print_warnings);
+  ASSERT_EQ(3u, hardware_info.async_params.cpu_affinity_cores.size());
+  ASSERT_THAT(
+    hardware_info.async_params.cpu_affinity_cores,
+    testing::ContainerEq(std::vector<int>({2, 4, 6})));
 
   EXPECT_EQ(hardware_info.joints[0].name, "joint1");
   EXPECT_EQ(hardware_info.joints[0].type, "joint");
@@ -1444,6 +1451,10 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components)
   ASSERT_THAT(hardware_info.sensors, SizeIs(1));
   ASSERT_TRUE(hardware_info.is_async);
   ASSERT_EQ(hardware_info.thread_priority, 50);
+  ASSERT_EQ(hardware_info.async_params.thread_priority, 50);
+  ASSERT_EQ(hardware_info.async_params.scheduling_policy, "synchronized");
+  ASSERT_TRUE(hardware_info.async_params.print_warnings);
+  ASSERT_TRUE(hardware_info.async_params.cpu_affinity_cores.empty());
 
   EXPECT_EQ(hardware_info.sensors[0].name, "sensor1");
   EXPECT_EQ(hardware_info.sensors[0].type, "sensor");
@@ -1468,6 +1479,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components)
   EXPECT_EQ(hardware_info.gpios[0].type, "gpio");
   ASSERT_TRUE(hardware_info.is_async);
   ASSERT_EQ(hardware_info.thread_priority, 70);
+  ASSERT_EQ(hardware_info.async_params.thread_priority, 70);
+  ASSERT_EQ(hardware_info.async_params.scheduling_policy, "synchronized");
+  ASSERT_EQ(1u, hardware_info.async_params.cpu_affinity_cores.size());
+  ASSERT_THAT(
+    hardware_info.async_params.cpu_affinity_cores, testing::ContainerEq(std::vector<int>({1})));
 }
 
 TEST_F(TestComponentParser, successfully_parse_parameter_empty)