Parse thread scheduler priority parsing to the async handlers

ros-controls · Aug 26, 2024 · 091ebad · 091ebad
1 parent 94e2a8e
commit 091ebad
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
@@ -110,6 +110,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)
@@ -126,7 +128,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
@@ -110,9 +110,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
@@ -112,6 +112,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)
@@ -128,7 +130,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);