diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 767fee3a7f..337c2b5f43 100644 --- 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 #include #include @@ -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>(); - write_async_handler_ = std::make_unique>(); - 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>(); + 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> read_async_handler_; - std::unique_ptr> write_async_handler_; + std::unique_ptr> 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 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 index 69d9b9063e..a31fca2222 100644 --- 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 #include #include @@ -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 index cd86cc1a7c..8f444447a2 100644 --- 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 #include #include @@ -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>(); - write_async_handler_ = std::make_unique>(); - 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>(); + 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> read_async_handler_; - std::unique_ptr> write_async_handler_; + std::unique_ptr> 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 next_trigger_ = TriggerType::READ; }; } // namespace hardware_interface