Parse thread scheduler priority parsing to the async handlers

ros-controls · Oct 27, 2024 · 68c62fd · 68c62fd
1 parent 99ce092
commit 68c62fd
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 3 deletions.
diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp
@@ -114,6 +114,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
     info_ = hardware_info;
     if (info_.is_async)
     {
+      RCLCPP_INFO_STREAM(
+        get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority);
       async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
       async_handler_->init(
         [this](const rclcpp::Time & time, const rclcpp::Duration & period)
@@ -130,7 +132,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
             next_trigger_ = TriggerType::READ;
             return ret;
           }
-        });
+        },
+        info_.thread_priority);
       async_handler_->start_thread();
     }
     return on_init(hardware_info);

diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp
@@ -114,9 +114,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     info_ = hardware_info;
     if (info_.is_async)
     {
+      RCLCPP_INFO_STREAM(
+        get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority);
       read_async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
       read_async_handler_->init(
-        std::bind(&SensorInterface::read, this, std::placeholders::_1, std::placeholders::_2));
+        std::bind(&SensorInterface::read, this, std::placeholders::_1, std::placeholders::_2),
+        info_.thread_priority);
       read_async_handler_->start_thread();
     }
     return on_init(hardware_info);

diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp
@@ -117,6 +117,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     info_ = hardware_info;
     if (info_.is_async)
     {
+      RCLCPP_INFO_STREAM(
+        get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority);
       async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
       async_handler_->init(
         [this](const rclcpp::Time & time, const rclcpp::Duration & period)
@@ -133,7 +135,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
             next_trigger_ = TriggerType::READ;
             return ret;
           }
-        });
+        },
+        info_.thread_priority);
       async_handler_->start_thread();
     }
     return on_init(hardware_info);