update the logic to use only single async_handler for both read and w…

…rite cycles
ros-controls · Sep 17, 2024 · 8653f1e · 8653f1e
1 parent f6ffb30
commit 8653f1e
Show file tree

Hide file tree

Showing 3 changed files with 62 additions and 79 deletions.
diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp
@@ -15,6 +15,7 @@
 #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
 #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
 
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -109,14 +110,24 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  info_ = hardware_info;
  if (info_.is_async)
  {
- read_async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
- write_async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
- read_async_handler_->init(
- std::bind(&ActuatorInterface::read, this, std::placeholders::_1, std::placeholders::_2));
- read_async_handler_->start_thread();
- write_async_handler_->init(
- std::bind(&ActuatorInterface::write, this, std::placeholders::_1, std::placeholders::_2));
- write_async_handler_->start_thread();
+ async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
+ async_handler_->init(
+ [this](const rclcpp::Time & time, const rclcpp::Duration & period)
+ {
+ if (next_trigger_ == TriggerType::READ)
+ {
+ const auto ret = read(time, period);
+ next_trigger_ = TriggerType::WRITE;
+ return ret;
+ }
+ else
+ {
+ const auto ret = write(time, period);
+ next_trigger_ = TriggerType::READ;
+ return ret;
+ }
+ });
+ async_handler_->start_thread();
  }
  return on_init(hardware_info);
  };
@@ -213,32 +224,22 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  if (next_trigger_ == TriggerType::WRITE)
  {
  RCLCPP_WARN(
- rclcpp::get_logger("ActuatorInterface"),
+ get_logger(),
  "Trigger read called while write async handler call is still pending for hardware "
  "interface : '%s'. Skipping read cycle and will wait for a write cycle!",
  info_.name.c_str());
  return return_type::OK;
  }
- if (write_async_handler_->is_trigger_cycle_in_progress())
- {
- RCLCPP_WARN(
- rclcpp::get_logger("ActuatorInterface"),
- "Trigger read called while write async handler is still in progress for hardware "
- "interface : '%s'. Skipping read cycle!",
- info_.name.c_str());
- return return_type::OK;
- }
- std::tie(trigger_status, result) = read_async_handler_->trigger_async_callback(time, period);
+ std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
  if (!trigger_status)
  {
  RCLCPP_WARN(
- rclcpp::get_logger("ActuatorInterface"),
- "Trigger read called while read async handler is still in progress for hardware "
+ get_logger(),
+ "Trigger read called while write async trigger is still in progress for hardware "
  "interface : '%s'. Failed to trigger read cycle!",
  info_.name.c_str());
  return return_type::OK;
  }
- next_trigger_ = TriggerType::WRITE;
  }
  else
  {
@@ -278,32 +279,22 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  if (next_trigger_ == TriggerType::READ)
  {
  RCLCPP_WARN(
- rclcpp::get_logger("ActuatorInterface"),
+ get_logger(),
  "Trigger write called while read async handler call is still pending for hardware "
  "interface : '%s'. Skipping write cycle and will wait for a read cycle!",
  info_.name.c_str());
  return return_type::OK;
  }
- if (read_async_handler_->is_trigger_cycle_in_progress())
- {
- RCLCPP_WARN(
- rclcpp::get_logger("ActuatorInterface"),
- "Trigger write called while read async handler is still in progress for hardware "
- "interface : '%s'. Skipping write cycle!",
- info_.name.c_str());
- return return_type::OK;
- }
- std::tie(trigger_status, result) = write_async_handler_->trigger_async_callback(time, period);
+ std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
  if (!trigger_status)
  {
  RCLCPP_WARN(
- rclcpp::get_logger("ActuatorInterface"),
- "Trigger write called while write async handler is still in progress for hardware "
+ get_logger(),
+ "Trigger write called while read async trigger is still in progress for hardware "
  "interface : '%s'. Failed to trigger write cycle!",
  info_.name.c_str());
  return return_type::OK;
  }
- next_trigger_ = TriggerType::READ;
  }
  else
  {
@@ -371,8 +362,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
 protected:
  HardwareInfo info_;
  rclcpp_lifecycle::State lifecycle_state_;
- std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> read_async_handler_;
- std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> write_async_handler_;
+ std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
 
 private:
  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
@@ -381,7 +371,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  {
  READ,
  WRITE
- } next_trigger_ = TriggerType::READ;
+ };
+ std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
 };
 
 } // namespace hardware_interface

diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp
@@ -15,6 +15,7 @@
 #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_
 #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_
 
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -159,7 +160,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  if (!trigger_status)
  {
  RCLCPP_WARN(
- rclcpp::get_logger("SensorInterface"),
+ get_logger(),
  "Trigger read called while read async handler is still in progress for hardware "
  "interface : '%s'. Failed to trigger read cycle!",
  info_.name.c_str());

diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp
@@ -15,6 +15,7 @@
 #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_
 #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_
 
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -111,14 +112,24 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  info_ = hardware_info;
  if (info_.is_async)
  {
- read_async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
- write_async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
- read_async_handler_->init(
- std::bind(&SystemInterface::read, this, std::placeholders::_1, std::placeholders::_2));
- read_async_handler_->start_thread();
- write_async_handler_->init(
- std::bind(&SystemInterface::write, this, std::placeholders::_1, std::placeholders::_2));
- write_async_handler_->start_thread();
+ async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
+ async_handler_->init(
+ [this](const rclcpp::Time & time, const rclcpp::Duration & period)
+ {
+ if (next_trigger_ == TriggerType::READ)
+ {
+ const auto ret = read(time, period);
+ next_trigger_ = TriggerType::WRITE;
+ return ret;
+ }
+ else
+ {
+ const auto ret = write(time, period);
+ next_trigger_ = TriggerType::READ;
+ return ret;
+ }
+ });
+ async_handler_->start_thread();
  }
  return on_init(hardware_info);
  };
@@ -215,32 +226,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  if (next_trigger_ == TriggerType::WRITE)
  {
  RCLCPP_WARN(
- rclcpp::get_logger("SystemInterface"),
+ get_logger(),
  "Trigger read called while write async handler call is still pending for hardware "
  "interface : '%s'. Skipping read cycle and will wait for a write cycle!",
  info_.name.c_str());
  return return_type::OK;
  }
- if (write_async_handler_->is_trigger_cycle_in_progress())
- {
- RCLCPP_WARN(
- rclcpp::get_logger("SystemInterface"),
- "Trigger read called while write async handler is still in progress for hardware "
- "interface : '%s'. Skipping read cycle!",
- info_.name.c_str());
- return return_type::OK;
- }
- std::tie(trigger_status, result) = read_async_handler_->trigger_async_callback(time, period);
+ std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
  if (!trigger_status)
  {
  RCLCPP_WARN(
- rclcpp::get_logger("SystemInterface"),
- "Trigger read called while read async handler is still in progress for hardware "
+ get_logger(),
+ "Trigger read called while write async trigger is still in progress for hardware "
  "interface : '%s'. Failed to trigger read cycle!",
  info_.name.c_str());
  return return_type::OK;
  }
- next_trigger_ = TriggerType::WRITE;
  }
  else
  {
@@ -280,32 +281,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  if (next_trigger_ == TriggerType::READ)
  {
  RCLCPP_WARN(
- rclcpp::get_logger("SystemInterface"),
+ get_logger(),
  "Trigger write called while read async handler call is still pending for hardware "
  "interface : '%s'. Skipping write cycle and will wait for a read cycle!",
  info_.name.c_str());
  return return_type::OK;
  }
- if (read_async_handler_->is_trigger_cycle_in_progress())
- {
- RCLCPP_WARN(
- rclcpp::get_logger("SystemInterface"),
- "Trigger write called while read async handler is still in progress for hardware "
- "interface : '%s'. Skipping write cycle!",
- info_.name.c_str());
- return return_type::OK;
- }
- std::tie(trigger_status, result) = write_async_handler_->trigger_async_callback(time, period);
+ std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
  if (!trigger_status)
  {
  RCLCPP_WARN(
- rclcpp::get_logger("SystemInterface"),
- "Trigger write called while write async handler is still in progress for hardware "
+ get_logger(),
+ "Trigger write called while read async trigger is still in progress for hardware "
  "interface : '%s'. Failed to trigger write cycle!",
  info_.name.c_str());
  return return_type::OK;
  }
- next_trigger_ = TriggerType::READ;
  }
  else
  {
@@ -373,8 +364,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
 protected:
  HardwareInfo info_;
  rclcpp_lifecycle::State lifecycle_state_;
- std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> read_async_handler_;
- std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> write_async_handler_;
+ std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
 
 private:
  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
@@ -383,7 +373,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  {
  READ,
  WRITE
- } next_trigger_ = TriggerType::READ;
+ };
+ std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
 };
 
 } // namespace hardware_interface