Parse thread scheduler priority parsing to the async handlers

ros-controls · Aug 6, 2024 · a162832 · a162832
1 parent 05d89f0
commit a162832
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
@@ -111,6 +111,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)
@@ -127,7 +129,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);