From f540edb6ce15cd6dfdaef760280e92207fe5192c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Jul 2024 23:31:43 +0200 Subject: [PATCH 1/4] initial fix for HW components concurrency issue --- .../include/hardware_interface/actuator.hpp | 4 ++- .../hardware_interface/actuator_interface.hpp | 4 +-- .../include/hardware_interface/sensor.hpp | 4 ++- .../hardware_interface/sensor_interface.hpp | 4 +-- hardware_interface/src/actuator.cpp | 30 +++++++++++++++++++ hardware_interface/src/sensor.cpp | 22 ++++++++++++++ 6 files changed, 62 insertions(+), 6 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 3534d9f587..bb91b62057 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -42,7 +42,8 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC explicit Actuator(std::unique_ptr impl); - Actuator(Actuator && other) = default; + HARDWARE_INTERFACE_PUBLIC + explicit Actuator(Actuator && other) noexcept; ~Actuator() = default; @@ -102,6 +103,7 @@ class Actuator final private: std::unique_ptr impl_; + mutable std::recursive_mutex actuators_mutex_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 1228d88bcd..e0993fcb2a 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -228,12 +228,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } -protected: /// Get the logger of the ActuatorInterface. /** * \return logger of the ActuatorInterface. */ - rclcpp::Logger get_logger() const { return actuator_logger_; } + rclcpp::Logger get_logger() { return actuator_logger_; } /// Get the clock of the ActuatorInterface. /** @@ -247,6 +246,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ const HardwareInfo & get_hardware_info() const { return info_; } +protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 4126910d44..98197bd0e9 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -42,7 +42,8 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC explicit Sensor(std::unique_ptr impl); - Sensor(Sensor && other) = default; + HARDWARE_INTERFACE_PUBLIC + explicit Sensor(Sensor && other) noexcept; ~Sensor() = default; @@ -89,6 +90,7 @@ class Sensor final private: std::unique_ptr impl_; + mutable std::recursive_mutex sensors_mutex_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 6fde3f8891..cd6e0ce808 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -167,12 +167,11 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } -protected: /// Get the logger of the SensorInterface. /** * \return logger of the SensorInterface. */ - rclcpp::Logger get_logger() const { return sensor_logger_; } + rclcpp::Logger get_logger() { return sensor_logger_; } /// Get the clock of the SensorInterface. /** @@ -186,6 +185,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ const HardwareInfo & get_hardware_info() const { return info_; } +protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 66419402f8..52cfc681a1 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -35,10 +36,17 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Actuator::Actuator(std::unique_ptr impl) : impl_(std::move(impl)) {} +Actuator::Actuator(Actuator && other) noexcept +{ + std::lock_guard lock(other.actuators_mutex_); + impl_ = std::move(other.impl_); +} + const rclcpp_lifecycle::State & Actuator::initialize( const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(actuator_info, logger, clock_interface)) @@ -60,6 +68,7 @@ const rclcpp_lifecycle::State & Actuator::initialize( const rclcpp_lifecycle::State & Actuator::configure() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_configure(impl_->get_state())) @@ -83,6 +92,7 @@ const rclcpp_lifecycle::State & Actuator::configure() const rclcpp_lifecycle::State & Actuator::cleanup() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_cleanup(impl_->get_state())) @@ -103,6 +113,7 @@ const rclcpp_lifecycle::State & Actuator::cleanup() const rclcpp_lifecycle::State & Actuator::shutdown() { + std::unique_lock lock(actuators_mutex_); if ( impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -124,6 +135,7 @@ const rclcpp_lifecycle::State & Actuator::shutdown() const rclcpp_lifecycle::State & Actuator::activate() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_state())) @@ -146,6 +158,7 @@ const rclcpp_lifecycle::State & Actuator::activate() const rclcpp_lifecycle::State & Actuator::deactivate() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { switch (impl_->on_deactivate(impl_->get_state())) @@ -168,6 +181,7 @@ const rclcpp_lifecycle::State & Actuator::deactivate() const rclcpp_lifecycle::State & Actuator::error() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->on_error(impl_->get_state())) @@ -221,6 +235,14 @@ const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_ 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_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -243,6 +265,14 @@ 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_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 9b7f1f69d6..9f1d4b73e1 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -34,10 +35,17 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Sensor::Sensor(std::unique_ptr impl) : impl_(std::move(impl)) {} +Sensor::Sensor(Sensor && other) noexcept +{ + std::lock_guard lock(other.sensors_mutex_); + impl_ = std::move(other.impl_); +} + const rclcpp_lifecycle::State & Sensor::initialize( const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(sensor_info, logger, clock_interface)) @@ -59,6 +67,7 @@ const rclcpp_lifecycle::State & Sensor::initialize( const rclcpp_lifecycle::State & Sensor::configure() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_configure(impl_->get_state())) @@ -82,6 +91,7 @@ const rclcpp_lifecycle::State & Sensor::configure() const rclcpp_lifecycle::State & Sensor::cleanup() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_cleanup(impl_->get_state())) @@ -102,6 +112,7 @@ const rclcpp_lifecycle::State & Sensor::cleanup() const rclcpp_lifecycle::State & Sensor::shutdown() { + std::unique_lock lock(sensors_mutex_); if ( impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -123,6 +134,7 @@ const rclcpp_lifecycle::State & Sensor::shutdown() const rclcpp_lifecycle::State & Sensor::activate() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_state())) @@ -145,6 +157,7 @@ const rclcpp_lifecycle::State & Sensor::activate() const rclcpp_lifecycle::State & Sensor::deactivate() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { switch (impl_->on_deactivate(impl_->get_state())) @@ -167,6 +180,7 @@ const rclcpp_lifecycle::State & Sensor::deactivate() const rclcpp_lifecycle::State & Sensor::error() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->on_error(impl_->get_state())) @@ -199,6 +213,14 @@ const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_st 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_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) From 02d08eb0caffcde9592862496d17f95b7ade08c4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 30 Jul 2024 18:22:59 +0200 Subject: [PATCH 2/4] Add the concurrency fix to the System hardware component --- .../include/hardware_interface/system.hpp | 4 ++- .../hardware_interface/system_interface.hpp | 3 +- hardware_interface/src/system.cpp | 29 +++++++++++++++++++ 3 files changed, 34 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 9308872ef8..1cc29a085d 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -42,7 +42,8 @@ class System final HARDWARE_INTERFACE_PUBLIC explicit System(std::unique_ptr impl); - System(System && other) = default; + HARDWARE_INTERFACE_PUBLIC + explicit System(System && other) noexcept; ~System() = default; @@ -102,6 +103,7 @@ class System final private: std::unique_ptr impl_; + mutable std::recursive_mutex system_mutex_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 06358f205e..54f1af33a7 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -25,6 +25,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -229,7 +230,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } -protected: /// Get the logger of the SystemInterface. /** * \return logger of the SystemInterface. @@ -248,6 +248,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ const HardwareInfo & get_hardware_info() const { return info_; } +protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index ba327d8ab2..82473563b6 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -34,10 +34,17 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface System::System(std::unique_ptr impl) : impl_(std::move(impl)) {} +System::System(System && other) noexcept +{ + std::lock_guard lock(other.system_mutex_); + impl_ = std::move(other.impl_); +} + const rclcpp_lifecycle::State & System::initialize( const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(system_info, logger, clock_interface)) @@ -59,6 +66,7 @@ const rclcpp_lifecycle::State & System::initialize( const rclcpp_lifecycle::State & System::configure() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_configure(impl_->get_state())) @@ -82,6 +90,7 @@ const rclcpp_lifecycle::State & System::configure() const rclcpp_lifecycle::State & System::cleanup() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_cleanup(impl_->get_state())) @@ -102,6 +111,7 @@ const rclcpp_lifecycle::State & System::cleanup() const rclcpp_lifecycle::State & System::shutdown() { + std::unique_lock lock(system_mutex_); if ( impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -123,6 +133,7 @@ const rclcpp_lifecycle::State & System::shutdown() const rclcpp_lifecycle::State & System::activate() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_state())) @@ -145,6 +156,7 @@ const rclcpp_lifecycle::State & System::activate() const rclcpp_lifecycle::State & System::deactivate() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { switch (impl_->on_deactivate(impl_->get_state())) @@ -167,6 +179,7 @@ const rclcpp_lifecycle::State & System::deactivate() const rclcpp_lifecycle::State & System::error() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->on_error(impl_->get_state())) @@ -218,6 +231,14 @@ const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_st 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_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -240,6 +261,14 @@ 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_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) From 50fa9962c0965667b15263e18242f4aeca3617a2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 30 Jul 2024 18:28:24 +0200 Subject: [PATCH 3/4] remove the blocking resource locks --- hardware_interface/src/resource_manager.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5b13e5d331..53fcad94d2 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1770,7 +1770,6 @@ void ResourceManager::shutdown_async_components() HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) { - std::lock_guard guard(resources_lock_); read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); @@ -1831,7 +1830,6 @@ HardwareReadWriteStatus ResourceManager::read( HardwareReadWriteStatus ResourceManager::write( const rclcpp::Time & time, const rclcpp::Duration & period) { - std::lock_guard guard(resources_lock_); read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); From e10b2f6d55aec1d00ee9ab5f94ba879ecfb0a152 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 30 Jul 2024 18:43:36 +0200 Subject: [PATCH 4/4] readd missing const in get_logger method of actuator and sensor --- .../include/hardware_interface/actuator_interface.hpp | 2 +- .../include/hardware_interface/sensor_interface.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index e0993fcb2a..a74d2a4964 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -232,7 +232,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /** * \return logger of the ActuatorInterface. */ - rclcpp::Logger get_logger() { return actuator_logger_; } + rclcpp::Logger get_logger() const { return actuator_logger_; } /// Get the clock of the ActuatorInterface. /** diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index cd6e0ce808..0c7f43e7fc 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -171,7 +171,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return logger of the SensorInterface. */ - rclcpp::Logger get_logger() { return sensor_logger_; } + rclcpp::Logger get_logger() const { return sensor_logger_; } /// Get the clock of the SensorInterface. /**