Bugfix 20210810 get current state (ros2#1756)

* protect state_machine_ with mutex lock.

protect state_handle_ with mutex lock.

reconsider mutex lock scope.

remove mutex lock from constructors.

lock just once during initialization of LifecycleNodeInterfaceImpl.

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>

* Move updating of current_state to right after initialization.

This is slightly more correct in the case that registering one
of the services fails.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
Co-authored-by: Chris Lalancette <clalancette@openrobotics.org>

rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP_LIFECYCLE__STATE_HPP_
 #define RCLCPP_LIFECYCLE__STATE_HPP_
 
+#include <mutex>
 #include <string>
 
 #include "rcl_lifecycle/data_types.h"
@@ -91,6 +92,7 @@ class State
 
   bool owns_rcl_state_handle_;
 
+  mutable std::recursive_mutex state_handle_mutex_;
   rcl_lifecycle_state_t * state_handle_;
 };
 

rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp

@@ -14,6 +14,7 @@
 
 #include <functional>
 #include <memory>
+#include <mutex>
 #include <stdexcept>
 #include <string>
 #include <utility>
@@ -59,7 +60,11 @@ LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl(
 LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl()
 {
   rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle();
-  auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle);
+  rcl_ret_t ret;
+  {
+    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+    ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle);
+  }
   if (ret != RCL_RET_OK) {
     RCUTILS_LOG_FATAL_NAMED(
       "rclcpp_lifecycle",
@@ -73,7 +78,6 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf
   rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle();
   const rcl_node_options_t * node_options =
     rcl_node_get_options(node_base_interface_->get_rcl_node_handle());
-  state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();
   auto state_machine_options = rcl_lifecycle_get_default_state_machine_options();
   state_machine_options.enable_com_interface = enable_communication_interface;
   state_machine_options.allocator = node_options->allocator;
@@ -92,6 +96,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf
   // The publisher takes a C-Typesupport since the publishing (i.e. creating
   // the message) is done fully in RCL.
   // Services are handled in C++, so that it needs a C++ typesupport structure.
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+  state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();
   rcl_ret_t ret = rcl_lifecycle_state_machine_init(
     &state_machine_,
     node_handle,
@@ -108,6 +114,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf
             node_base_interface_->get_name());
   }
 
+  current_state_ = State(state_machine_.current_state);
+
   if (enable_communication_interface) {
     { // change_state
       auto cb = std::bind(
@@ -196,8 +204,6 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf
         nullptr);
     }
   }
-
-  current_state_ = State(state_machine_.current_state);
 }
 
 bool
@@ -216,28 +222,31 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state(
   std::shared_ptr<ChangeStateSrv::Response> resp)
 {
   (void)header;
-  if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
-    throw std::runtime_error(
-            "Can't get state. State machine is not initialized.");
-  }
+  std::uint8_t transition_id;
+  {
+    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+    if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
+      throw std::runtime_error("Can't get state. State machine is not initialized.");
+    }
 
-  auto transition_id = req->transition.id;
-  // if there's a label attached to the request,
-  // we check the transition attached to this label.
-  // we further can't compare the id of the looked up transition
-  // because ros2 service call defaults all intergers to zero.
-  // that means if we call ros2 service call ... {transition: {label: shutdown}}
-  // the id of the request is 0 (zero) whereas the id from the lookup up transition
-  // can be different.
-  // the result of this is that the label takes presedence of the id.
-  if (req->transition.label.size() != 0) {
-    auto rcl_transition = rcl_lifecycle_get_transition_by_label(
-      state_machine_.current_state, req->transition.label.c_str());
-    if (rcl_transition == nullptr) {
-      resp->success = false;
-      return;
+    transition_id = req->transition.id;
+    // if there's a label attached to the request,
+    // we check the transition attached to this label.
+    // we further can't compare the id of the looked up transition
+    // because ros2 service call defaults all intergers to zero.
+    // that means if we call ros2 service call ... {transition: {label: shutdown}}
+    // the id of the request is 0 (zero) whereas the id from the lookup up transition
+    // can be different.
+    // the result of this is that the label takes presedence of the id.
+    if (req->transition.label.size() != 0) {
+      auto rcl_transition = rcl_lifecycle_get_transition_by_label(
+        state_machine_.current_state, req->transition.label.c_str());
+      if (rcl_transition == nullptr) {
+        resp->success = false;
+        return;
+      }
+      transition_id = static_cast<std::uint8_t>(rcl_transition->id);
     }
-    transition_id = static_cast<std::uint8_t>(rcl_transition->id);
   }
 
   node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code;
@@ -258,6 +267,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state(
 {
   (void)header;
   (void)req;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
   if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
     throw std::runtime_error(
             "Can't get state. State machine is not initialized.");
@@ -274,6 +284,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states(
 {
   (void)header;
   (void)req;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
   if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
     throw std::runtime_error(
             "Can't get available states. State machine is not initialized.");
@@ -296,6 +307,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions(
 {
   (void)header;
   (void)req;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
   if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
     throw std::runtime_error(
             "Can't get available transitions. State machine is not initialized.");
@@ -323,6 +335,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph(
 {
   (void)header;
   (void)req;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
   if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
     throw std::runtime_error(
             "Can't get available transitions. State machine is not initialized.");
@@ -352,6 +365,7 @@ std::vector<State>
 LifecycleNode::LifecycleNodeInterfaceImpl::get_available_states() const
 {
   std::vector<State> states;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
   states.reserve(state_machine_.transition_map.states_size);
 
   for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) {
@@ -364,6 +378,7 @@ std::vector<Transition>
 LifecycleNode::LifecycleNodeInterfaceImpl::get_available_transitions() const
 {
   std::vector<Transition> transitions;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
   transitions.reserve(state_machine_.current_state->valid_transition_size);
 
   for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) {
@@ -376,6 +391,7 @@ std::vector<Transition>
 LifecycleNode::LifecycleNodeInterfaceImpl::get_transition_graph() const
 {
   std::vector<Transition> transitions;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
   transitions.reserve(state_machine_.transition_map.transitions_size);
 
   for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
@@ -389,26 +405,33 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
   std::uint8_t transition_id,
   node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code)
 {
-  if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
-    RCUTILS_LOG_ERROR(
-      "Unable to change state for state machine for %s: %s",
-      node_base_interface_->get_name(), rcl_get_error_string().str);
-    return RCL_RET_ERROR;
-  }
-
   constexpr bool publish_update = true;
-  // keep the initial state to pass to a transition callback
-  State initial_state(state_machine_.current_state);
+  State initial_state;
+  unsigned int current_state_id;
 
-  if (
-    rcl_lifecycle_trigger_transition_by_id(
-      &state_machine_, transition_id, publish_update) != RCL_RET_OK)
   {
-    RCUTILS_LOG_ERROR(
-      "Unable to start transition %u from current state %s: %s",
-      transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
-    rcutils_reset_error();
-    return RCL_RET_ERROR;
+    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+    if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR(
+        "Unable to change state for state machine for %s: %s",
+        node_base_interface_->get_name(), rcl_get_error_string().str);
+      return RCL_RET_ERROR;
+    }
+
+    // keep the initial state to pass to a transition callback
+    initial_state = State(state_machine_.current_state);
+
+    if (
+      rcl_lifecycle_trigger_transition_by_id(
+        &state_machine_, transition_id, publish_update) != RCL_RET_OK)
+    {
+      RCUTILS_LOG_ERROR(
+        "Unable to start transition %u from current state %s: %s",
+        transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
+      rcutils_reset_error();
+      return RCL_RET_ERROR;
+    }
+    current_state_id = state_machine_.current_state->id;
   }
 
   // Update the internal current_state_
@@ -425,18 +448,22 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
       return rcl_lifecycle_transition_error_label;
     };
 
-  cb_return_code = execute_callback(state_machine_.current_state->id, initial_state);
+  cb_return_code = execute_callback(current_state_id, initial_state);
   auto transition_label = get_label_for_return_code(cb_return_code);
 
-  if (
-    rcl_lifecycle_trigger_transition_by_label(
-      &state_machine_, transition_label, publish_update) != RCL_RET_OK)
   {
-    RCUTILS_LOG_ERROR(
-      "Failed to finish transition %u. Current state is now: %s (%s)",
-      transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
-    rcutils_reset_error();
-    return RCL_RET_ERROR;
+    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+    if (
+      rcl_lifecycle_trigger_transition_by_label(
+        &state_machine_, transition_label, publish_update) != RCL_RET_OK)
+    {
+      RCUTILS_LOG_ERROR(
+        "Failed to finish transition %u. Current state is now: %s (%s)",
+        transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
+      rcutils_reset_error();
+      return RCL_RET_ERROR;
+    }
+    current_state_id = state_machine_.current_state->id;
   }
 
   // Update the internal current_state_
@@ -447,8 +474,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
   if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) {
     RCUTILS_LOG_WARN("Error occurred while doing error handling.");
 
-    auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state);
+    auto error_cb_code = execute_callback(current_state_id, initial_state);
     auto error_cb_label = get_label_for_return_code(error_cb_code);
+    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
     if (
       rcl_lifecycle_trigger_transition_by_label(
         &state_machine_, error_cb_label, publish_update) != RCL_RET_OK)
@@ -500,8 +528,13 @@ const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition(
   const char * transition_label,
   node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code)
 {
-  auto transition =
-    rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label);
+  const rcl_lifecycle_transition_t * transition;
+  {
+    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+
+    transition =
+      rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label);
+  }
   if (transition) {
     change_state(static_cast<uint8_t>(transition->id), cb_return_code);
   }

rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp

@@ -143,6 +143,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final
   node_interfaces::LifecycleNodeInterface::CallbackReturn
   execute_callback(unsigned int cb_id, const State & previous_state) const;
 
+  mutable std::recursive_mutex state_machine_mutex_;
   rcl_lifecycle_state_machine_t state_machine_;
   State current_state_;
   std::map<

rclcpp_lifecycle/src/state.cpp

@@ -70,6 +70,7 @@ State::State(
   if (!rcl_lifecycle_state_handle) {
     throw std::runtime_error("rcl_lifecycle_state_handle is null");
   }
+
   state_handle_ = const_cast<rcl_lifecycle_state_t *>(rcl_lifecycle_state_handle);
 }
 
@@ -93,6 +94,8 @@ State::operator=(const State & rhs)
     return *this;
   }
 
+  // hold the lock until state_handle_ is reconstructed
+  std::lock_guard<std::recursive_mutex> lock(state_handle_mutex_);
   // reset all currently used resources
   reset();
 
@@ -128,6 +131,7 @@ State::operator=(const State & rhs)
 uint8_t
 State::id() const
 {
+  std::lock_guard<std::recursive_mutex> lock(state_handle_mutex_);
   if (!state_handle_) {
     throw std::runtime_error("Error in state! Internal state_handle is NULL.");
   }
@@ -137,6 +141,7 @@ State::id() const
 std::string
 State::label() const
 {
+  std::lock_guard<std::recursive_mutex> lock(state_handle_mutex_);
   if (!state_handle_) {
     throw std::runtime_error("Error in state! Internal state_handle is NULL.");
   }
@@ -146,6 +151,7 @@ State::label() const
 void
 State::reset() noexcept
 {
+  std::lock_guard<std::recursive_mutex> lock(state_handle_mutex_);
   if (!owns_rcl_state_handle_) {
     state_handle_ = nullptr;
   }