Fix the blocking calls of lifecycle_state in the real-time loop

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -29,6 +29,7 @@
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
 
+#include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/version.h"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
@@ -200,8 +201,23 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
 
   std::shared_ptr<const rclcpp_lifecycle::LifecycleNode> get_node() const;
 
+  /**
+   * Get the current lifecycle state of the controller node.
+   * \note Accessing members of the returned rclcpp_lifecycle::State is not real-time safe and
+   * should not be called in the control loop.
+   * \note This method is thread safe.
+   * \returns lifecycle state of the controller node.
+   */
   const rclcpp_lifecycle::State & get_lifecycle_state() const;
 
+  /**
+   * Get the lifecycle id of the controller node that is cached internally
+   * to avoid calls to get_lifecycle_state() in the real-time control loop.
+   * \note This method is real-time safe and thread safe and can be called in the control loop.
+   * \returns lifecycle id of the controller node.
+   */
+  uint8_t get_lifecycle_id() const;
+
   unsigned int get_update_rate() const;
 
   bool is_async() const;
@@ -380,6 +396,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   controller_interface::ControllerInterfaceParams ctrl_itf_params_;
   std::atomic_bool skip_async_triggers_ = false;
   ControllerUpdateStats trigger_stats_;
+  mutable std::atomic<uint8_t> lifecycle_id_ = lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
 
 protected:
   pal_statistics::RegistrationsRAII stats_registrations_;

controller_interface/src/chainable_controller_interface.cpp

@@ -200,7 +200,7 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
 {
   bool result = false;
 
-  if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  if (get_lifecycle_id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     result = on_set_chained_mode(chained_mode);
 

controller_interface/src/controller_interface_base.cpp

@@ -62,6 +62,7 @@ return_type ControllerInterfaceBase::init(
     ctrl_itf_params_.controller_name, ctrl_itf_params_.node_namespace,
     ctrl_itf_params_.node_options,
     false);  // disable LifecycleNode service interfaces
+  lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release);
 
   if (ctrl_itf_params_.controller_manager_update_rate == 0 && ctrl_itf_params_.update_rate != 0)
   {
@@ -92,22 +93,12 @@ return_type ControllerInterfaceBase::init(
     return return_type::ERROR;
   }
 
-  switch (on_init())
-  {
-    case LifecycleNodeInterface::CallbackReturn::SUCCESS:
-      break;
-    case LifecycleNodeInterface::CallbackReturn::ERROR:
-    case LifecycleNodeInterface::CallbackReturn::FAILURE:
-      RCLCPP_DEBUG(
-        get_node()->get_logger(),
-        "Calling shutdown transition of controller node '%s' due to init failure.",
-        get_node()->get_name());
-      node_->shutdown();
-      return return_type::ERROR;
-  }
-
   node_->register_on_configure(
-    std::bind(&ControllerInterfaceBase::on_configure, this, std::placeholders::_1));
+    [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
+    {
+      lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release);
+      return on_configure(previous_state);
+    });
 
   node_->register_on_cleanup(
     [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
@@ -116,33 +107,37 @@ return_type ControllerInterfaceBase::init(
       // it in `on_configure` and `on_deactivate` - see the docs for details
       enable_introspection(false);
       this->stop_async_handler_thread();
+      lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release);
       return on_cleanup(previous_state);
     });
 
   node_->register_on_activate(
     [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
     {
-      skip_async_triggers_.store(false);
+      skip_async_triggers_.store(false, std::memory_order_release);
       enable_introspection(true);
       if (is_async() && async_handler_ && async_handler_->is_running())
       {
         // This is needed if it is disabled due to a thrown exception in the async callback thread
         async_handler_->reset_variables();
       }
+      lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release);
       return on_activate(previous_state);
     });
 
   node_->register_on_deactivate(
     [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
     {
       enable_introspection(false);
+      lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release);
       return on_deactivate(previous_state);
     });
 
   node_->register_on_shutdown(
     [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
     {
       this->stop_async_handler_thread();
+      lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release);
       auto transition_state_status = on_shutdown(previous_state);
       this->release_interfaces();
       return transition_state_status;
@@ -152,16 +147,32 @@ return_type ControllerInterfaceBase::init(
     [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
     {
       this->stop_async_handler_thread();
+      lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release);
       auto transition_state_status = on_error(previous_state);
       this->release_interfaces();
       return transition_state_status;
     });
 
+  switch (on_init())
+  {
+    case LifecycleNodeInterface::CallbackReturn::SUCCESS:
+      break;
+    case LifecycleNodeInterface::CallbackReturn::ERROR:
+    case LifecycleNodeInterface::CallbackReturn::FAILURE:
+      RCLCPP_ERROR(
+        get_node()->get_logger(),
+        "Calling shutdown transition of controller node '%s' due to init failure.",
+        get_node()->get_name());
+      node_->shutdown();
+      return return_type::ERROR;
+  }
+
   return return_type::OK;
 }
 
 const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
 {
+  lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release);
   // TODO(destogl): this should actually happen in "on_configure" but I am not sure how to get
   // overrides correctly in combination with std::bind. The goal is to have the following calls:
   // 1. CM: controller.get_node()->configure()
@@ -253,7 +264,9 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
   REGISTER_ROS2_CONTROL_INTROSPECTION("failed_triggers", &trigger_stats_.failed_triggers);
   trigger_stats_.reset();
 
-  return get_node()->configure();
+  const auto & return_value = get_node()->configure();
+  lifecycle_id_.store(return_value.id(), std::memory_order_release);
+  return return_value;
 }
 
 void ControllerInterfaceBase::assign_interfaces(
@@ -279,6 +292,24 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c
   return node_->get_current_state();
 }
 
+uint8_t ControllerInterfaceBase::get_lifecycle_id() const
+{
+  const auto id = lifecycle_id_.load(std::memory_order_acquire);
+  if (
+    id == lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING ||
+    id == lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING ||
+    id == lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP ||
+    id == lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING ||
+    id == lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN ||
+    id == lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING)
+  {
+    const auto new_id = this->get_lifecycle_state().id();
+    lifecycle_id_.store(new_id, std::memory_order_release);
+    return new_id;
+  }
+  return id;
+}
+
 ControllerUpdateStatus ControllerInterfaceBase::trigger_update(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
@@ -379,7 +410,7 @@ void ControllerInterfaceBase::wait_for_trigger_update_to_finish()
 
 void ControllerInterfaceBase::prepare_for_deactivation()
 {
-  skip_async_triggers_.store(true);
+  skip_async_triggers_.store(true, std::memory_order_release);
   this->wait_for_trigger_update_to_finish();
 }
 

controller_interface/test/test_controller_interface.cpp

@@ -233,6 +233,7 @@ TEST(TestableControllerInterfaceInitError, init_with_error)
 
   ASSERT_EQ(
     controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
+  ASSERT_EQ(controller.get_lifecycle_id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
   rclcpp::shutdown();
 }
 
@@ -255,6 +256,7 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure)
 
   ASSERT_EQ(
     controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
+  ASSERT_EQ(controller.get_lifecycle_id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
   rclcpp::shutdown();
 }
 

controller_manager/doc/userdoc.rst

@@ -59,6 +59,9 @@ Alternatives to the standard kernel include
 Though installing a realtime-kernel will definitely get the best results when it comes to low
 jitter, using a lowlatency kernel can improve things a lot with being really easy to install.
 
+.. note::
+  Avoid using the get_lifecycle_state() method in the real-time control loop of the controllers and the hardware components as it is not real-time safe.
+
 Publishers
 -----------
 

controller_manager/src/controller_manager.cpp

@@ -65,14 +65,12 @@ static const rmw_qos_profile_t qos_services = {
 inline bool is_controller_unconfigured(
   const controller_interface::ControllerInterfaceBase & controller)
 {
-  return controller.get_lifecycle_state().id() ==
-         lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED;
+  return controller.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED;
 }
 
 inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller)
 {
-  return controller.get_lifecycle_state().id() ==
-         lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
+  return controller.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
 }
 
 inline bool is_controller_inactive(
@@ -83,7 +81,7 @@ inline bool is_controller_inactive(
 
 inline bool is_controller_active(const controller_interface::ControllerInterfaceBase & controller)
 {
-  return controller.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
+  return controller.get_lifecycle_id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
 }
 
 inline bool is_controller_active(
@@ -1251,7 +1249,7 @@ controller_interface::return_type ControllerManager::configure_controller(
   }
   auto controller = found_it->c;
 
-  auto state = controller->get_lifecycle_state();
+  const auto & state = controller->get_lifecycle_state();
   if (
     state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ||
     state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
@@ -1262,7 +1260,6 @@ controller_interface::return_type ControllerManager::configure_controller(
     return controller_interface::return_type::ERROR;
   }
 
-  auto new_state = controller->get_lifecycle_state();
   if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
     RCLCPP_DEBUG(
@@ -1277,7 +1274,7 @@ controller_interface::return_type ControllerManager::configure_controller(
 
   try
   {
-    new_state = controller->configure();
+    const auto & new_state = controller->configure();
     if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
     {
       RCLCPP_ERROR(
@@ -3071,8 +3068,6 @@ controller_interface::return_type ControllerManager::update(
         loaded_controller.info.name.c_str());
       continue;
     }
-    // TODO(v-lopez) we could cache this information
-    // https://github.com/ros-controls/ros2_control/issues/153
     if (is_controller_active(*loaded_controller.c))
     {
       if (

controller_manager/test/test_chainable_controller/test_chainable_controller.cpp

@@ -20,6 +20,19 @@
 
 #include "lifecycle_msgs/msg/state.hpp"
 
+namespace
+{
+void verify_internal_lifecycle_id(uint8_t expected_id, uint8_t actual_id)
+{
+  if (expected_id != actual_id)
+  {
+    throw std::runtime_error(
+      "Internal lifecycle ID does not match the expected lifecycle ID. Expected: " +
+      std::to_string(expected_id) + ", Actual: " + std::to_string(actual_id));
+  }
+}
+}  // namespace
+
 namespace test_chainable_controller
 {
 TestChainableController::TestChainableController()
@@ -32,6 +45,7 @@ TestChainableController::TestChainableController()
 controller_interface::InterfaceConfiguration
 TestChainableController::command_interface_configuration() const
 {
+  verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
   if (
     get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
     get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
@@ -48,6 +62,7 @@ TestChainableController::command_interface_configuration() const
 controller_interface::InterfaceConfiguration
 TestChainableController::state_interface_configuration() const
 {
+  verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
   if (
     get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
     get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
@@ -71,6 +86,7 @@ TestChainableController::state_interface_configuration() const
 controller_interface::return_type TestChainableController::update_reference_from_subscribers(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
+  verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
   if (time.get_clock_type() != RCL_ROS_TIME)
   {
     throw std::runtime_error(
@@ -102,6 +118,7 @@ controller_interface::return_type TestChainableController::update_reference_from
 controller_interface::return_type TestChainableController::update_and_write_commands(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
+  verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
   if (time.get_clock_type() != RCL_ROS_TIME)
   {
     throw std::runtime_error(
@@ -132,11 +149,16 @@ controller_interface::return_type TestChainableController::update_and_write_comm
   return update_return_value;
 }
 
-CallbackReturn TestChainableController::on_init() { return CallbackReturn::SUCCESS; }
+CallbackReturn TestChainableController::on_init()
+{
+  verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
+  return CallbackReturn::SUCCESS;
+}
 
 CallbackReturn TestChainableController::on_configure(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
   joints_command_subscriber_ = get_node()->create_subscription<CmdType>(
     "~/commands", rclcpp::SystemDefaultsQoS(),
     [this](const CmdType::SharedPtr msg)
@@ -166,6 +188,7 @@ CallbackReturn TestChainableController::on_configure(
 CallbackReturn TestChainableController::on_activate(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
   if (!is_in_chained_mode())
   {
     auto msg = rt_command_ptr_.readFromRT();
@@ -178,13 +201,15 @@ CallbackReturn TestChainableController::on_activate(
 CallbackReturn TestChainableController::on_cleanup(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
   joints_command_subscriber_.reset();
   return CallbackReturn::SUCCESS;
 }
 
 std::vector<hardware_interface::StateInterface>
 TestChainableController::on_export_state_interfaces()
 {
+  verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
   std::vector<hardware_interface::StateInterface> state_interfaces;
 
   for (size_t i = 0; i < exported_state_interface_names_.size(); ++i)
@@ -200,6 +225,7 @@ TestChainableController::on_export_state_interfaces()
 std::vector<hardware_interface::CommandInterface>
 TestChainableController::on_export_reference_interfaces()
 {
+  verify_internal_lifecycle_id(get_lifecycle_id(), get_lifecycle_state().id());
   std::vector<hardware_interface::CommandInterface> reference_interfaces;
 
   for (size_t i = 0; i < reference_interface_names_.size(); ++i)