[RM] Decouple read/write cycles of each component with mutex to not b…

…lock other components (ros-controls#1646)

hardware_interface/include/hardware_interface/actuator.hpp

@@ -42,7 +42,8 @@ class Actuator final
   HARDWARE_INTERFACE_PUBLIC
   explicit Actuator(std::unique_ptr<ActuatorInterface> 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<ActuatorInterface> impl_;
+  mutable std::recursive_mutex actuators_mutex_;
 };
 
 }  // namespace hardware_interface

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -228,7 +228,6 @@ 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.
@@ -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_;
 

hardware_interface/include/hardware_interface/sensor.hpp

@@ -42,7 +42,8 @@ class Sensor final
   HARDWARE_INTERFACE_PUBLIC
   explicit Sensor(std::unique_ptr<SensorInterface> 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<SensorInterface> impl_;
+  mutable std::recursive_mutex sensors_mutex_;
 };
 
 }  // namespace hardware_interface

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -167,7 +167,6 @@ 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.
@@ -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_;
 

hardware_interface/include/hardware_interface/system.hpp

@@ -42,7 +42,8 @@ class System final
   HARDWARE_INTERFACE_PUBLIC
   explicit System(std::unique_ptr<SystemInterface> 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<SystemInterface> impl_;
+  mutable std::recursive_mutex system_mutex_;
 };
 
 }  // namespace hardware_interface

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_;
 

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<ActuatorInterface> impl) : impl_(std::move(impl)) {}
 
+Actuator::Actuator(Actuator && other) noexcept
+{
+  std::lock_guard<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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)

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<std::recursive_mutex> 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<std::recursive_mutex> guard(resources_lock_);
   read_write_status.ok = true;
   read_write_status.failed_hardware_names.clear();
 

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<SensorInterface> impl) : impl_(std::move(impl)) {}
 
+Sensor::Sensor(Sensor && other) noexcept
+{
+  std::lock_guard<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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)