Bugfix 20210810 get current state (#1756)

fujitatomoya · clalancette · web-flow · commit e5d13a247815 · 2022-10-28T09:15:14.000-07:00
* 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 &lt;Tomoya.Fujita@sony.com&gt;

* 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 &lt;clalancette@openrobotics.org&gt;

Signed-off-by: Tomoya Fujita &lt;Tomoya.Fujita@sony.com&gt;
Signed-off-by: Chris Lalancette &lt;clalancette@openrobotics.org&gt;
Co-authored-by: Chris Lalancette &lt;clalancette@openrobotics.org&gt;
diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/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"
@@ -92,6 +93,7 @@ class State
 
   bool owns_rcl_state_handle_;
 
+  mutable std::recursive_mutex state_handle_mutex_;
   rcl_lifecycle_state_t * state_handle_;
 };
 
diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp
@@ -14,6 +14,7 @@
 
 #include <functional>
 #include <memory>
+#include <mutex>
 #include <stdexcept>
 #include <string>
 #include <utility>
@@ -58,7 +59,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",
@@ -72,7 +77,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;
@@ -83,6 +87,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,
@@ -99,6 +105,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(
@@ -182,8 +190,6 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf
         nullptr);
     }
   }
-
-  current_state_ = State(state_machine_.current_state);
 }
 
 bool
@@ -202,28 +208,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;
@@ -244,6 +253,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.");
@@ -260,6 +270,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.");
@@ -282,6 +293,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.");
@@ -309,6 +321,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.");
@@ -338,6 +351,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) {
@@ -350,6 +364,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) {
@@ -362,6 +377,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) {
@@ -375,26 +391,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_
@@ -411,18 +434,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_
@@ -433,8 +460,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)
@@ -486,8 +514,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);
   }
diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/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<
diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp
@@ -71,6 +71,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);
 }
 
@@ -94,6 +95,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();
 
@@ -129,6 +132,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.");
   }
@@ -138,6 +142,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.");
   }
@@ -147,6 +152,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;
   }

Original file line number	Diff line number	Diff line change
`@@ -71,6 +71,7 @@ State::State(`
`71`	`71`	`if (!rcl_lifecycle_state_handle) {`
`72`	`72`	`throw std::runtime_error("rcl_lifecycle_state_handle is null");`
`73`	`73`	`}`
	`74`	`+`
`74`	`75`	`state_handle_ = const_cast<rcl_lifecycle_state_t *>(rcl_lifecycle_state_handle);`
`75`	`76`	`}`
`76`	`77`
`@@ -94,6 +95,8 @@ State::operator=(const State & rhs)`
`94`	`95`	`return *this;`
`95`	`96`	`}`
`96`	`97`
	`98`	`+ // hold the lock until state_handle_ is reconstructed`
	`99`	`+ std::lock_guard<std::recursive_mutex> lock(state_handle_mutex_);`
`97`	`100`	`// reset all currently used resources`
`98`	`101`	`reset();`
`99`	`102`
`@@ -129,6 +132,7 @@ State::operator=(const State & rhs)`
`129`	`132`	`uint8_t`
`130`	`133`	`State::id() const`
`131`	`134`	`{`
	`135`	`+ std::lock_guard<std::recursive_mutex> lock(state_handle_mutex_);`
`132`	`136`	`if (!state_handle_) {`
`133`	`137`	`throw std::runtime_error("Error in state! Internal state_handle is NULL.");`
`134`	`138`	`}`
`@@ -138,6 +142,7 @@ State::id() const`
`138`	`142`	`std::string`
`139`	`143`	`State::label() const`
`140`	`144`	`{`
	`145`	`+ std::lock_guard<std::recursive_mutex> lock(state_handle_mutex_);`
`141`	`146`	`if (!state_handle_) {`
`142`	`147`	`throw std::runtime_error("Error in state! Internal state_handle is NULL.");`
`143`	`148`	`}`
`@@ -147,6 +152,7 @@ State::label() const`
`147`	`152`	`void`
`148`	`153`	`State::reset() noexcept`
`149`	`154`	`{`
	`155`	`+ std::lock_guard<std::recursive_mutex> lock(state_handle_mutex_);`
`150`	`156`	`if (!owns_rcl_state_handle_) {`
`151`	`157`	`state_handle_ = nullptr;`
`152`	`158`	`}`