diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 585078c5fc..0121d0764d 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -107,6 +107,9 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); + HARDWARE_INTERFACE_PUBLIC + std::recursive_mutex & get_mutex(); + private: std::unique_ptr impl_; // Last read cycle time diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index ce4db654da..1da3b80b58 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -91,6 +91,9 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; } + HARDWARE_INTERFACE_PUBLIC + std::recursive_mutex & get_mutex(); + private: std::unique_ptr impl_; // Last read cycle time diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index fe8eec4982..2b6b438b87 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -107,6 +107,9 @@ class System final HARDWARE_INTERFACE_PUBLIC return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); + HARDWARE_INTERFACE_PUBLIC + std::recursive_mutex & get_mutex(); + private: std::unique_ptr impl_; // Last read cycle time diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 0600decf1e..d11357aef9 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -244,14 +244,6 @@ const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_c return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(actuators_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping read() call for actuator '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if ( impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -276,14 +268,6 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(actuators_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping write() call for actuator '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if ( impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -306,4 +290,5 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & return result; } +std::recursive_mutex & Actuator::get_mutex() { return actuators_mutex_; } } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 29792714c1..0047f5947d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1788,6 +1788,14 @@ HardwareReadWriteStatus ResourceManager::read( { for (auto & component : components) { + std::unique_lock lock(component.get_mutex(), std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + get_logger(), "Skipping read() call for the component '%s' since it is locked", + component.get_name().c_str()); + continue; + } auto ret_val = return_type::OK; try { @@ -1864,6 +1872,14 @@ HardwareReadWriteStatus ResourceManager::write( { for (auto & component : components) { + std::unique_lock lock(component.get_mutex(), std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + get_logger(), "Skipping write() call for the component '%s' since it is locked", + component.get_name().c_str()); + continue; + } auto ret_val = return_type::OK; try { diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 285b7d28e3..161e29bc9c 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -219,14 +219,6 @@ const rclcpp::Time & Sensor::get_last_read_time() const { return last_read_cycle return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(sensors_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping read() call for the sensor '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if ( impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -249,4 +241,5 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per return result; } +std::recursive_mutex & Sensor::get_mutex() { return sensors_mutex_; } } // namespace hardware_interface diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 75ae9454bc..d1912c1c69 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -240,14 +240,6 @@ const rclcpp::Time & System::get_last_write_time() const { return last_write_cyc return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(system_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping read() call for system '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if ( impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -272,14 +264,6 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - std::unique_lock lock(system_mutex_, std::try_to_lock); - if (!lock.owns_lock()) - { - RCLCPP_DEBUG( - impl_->get_logger(), "Skipping write() call for system '%s' since it is locked", - impl_->get_name().c_str()); - return return_type::OK; - } if ( impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -302,4 +286,5 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe return result; } +std::recursive_mutex & System::get_mutex() { return system_mutex_; } } // namespace hardware_interface