protect state_handle_ with mutex lock.

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

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.hpp

@@ -182,9 +182,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
           std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
           srv_get_available_transitions_ =
             std::make_shared<rclcpp::Service<GetAvailableTransitionsSrv>>(
-              node_base_interface_->get_shared_rcl_node_handle(),
-              &state_machine_.com_interface.srv_get_available_transitions,
-              any_cb);
+            node_base_interface_->get_shared_rcl_node_handle(),
+            &state_machine_.com_interface.srv_get_available_transitions,
+            any_cb);
         }
         node_services_interface_->add_service(
           std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_available_transitions_),
@@ -202,9 +202,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
           std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
           srv_get_transition_graph_ =
             std::make_shared<rclcpp::Service<GetAvailableTransitionsSrv>>(
-              node_base_interface_->get_shared_rcl_node_handle(),
-              &state_machine_.com_interface.srv_get_transition_graph,
-              any_cb);
+            node_base_interface_->get_shared_rcl_node_handle(),
+            &state_machine_.com_interface.srv_get_transition_graph,
+            any_cb);
         }
         node_services_interface_->add_service(
           std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_transition_graph_),
@@ -233,8 +233,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
     {
       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.");
+        throw std::runtime_error("Can't get state. State machine is not initialized.");
       }
 
       transition_id = req->transition.id;
@@ -522,7 +521,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
   const State & trigger_transition(
     const char * transition_label, LifecycleNodeInterface::CallbackReturn & cb_return_code)
   {
-    const rcl_lifecycle_transition_t *transition;
+    const rcl_lifecycle_transition_t * transition;
     {
       std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
       transition =

rclcpp_lifecycle/src/state.cpp

@@ -44,19 +44,22 @@ State::State(
     throw std::runtime_error("Lifecycle State cannot have an empty label.");
   }
 
-  state_handle_ = static_cast<rcl_lifecycle_state_t *>(
-    allocator_.allocate(sizeof(rcl_lifecycle_state_t), allocator_.state));
-  if (!state_handle_) {
-    throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t");
-  }
-  // zero initialize
-  state_handle_->id = 0;
-  state_handle_->label = nullptr;
-
-  auto ret = rcl_lifecycle_state_init(state_handle_, id, label.c_str(), &allocator_);
-  if (ret != RCL_RET_OK) {
-    reset();
-    rclcpp::exceptions::throw_from_rcl_error(ret);
+  {
+    std::lock_guard<std::recursive_mutex> lock(state_handle_mutex_);
+    state_handle_ = static_cast<rcl_lifecycle_state_t *>(
+      allocator_.allocate(sizeof(rcl_lifecycle_state_t), allocator_.state));
+    if (!state_handle_) {
+      throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t");
+    }
+    // zero initialize
+    state_handle_->id = 0;
+    state_handle_->label = nullptr;
+
+    auto ret = rcl_lifecycle_state_init(state_handle_, id, label.c_str(), &allocator_);
+    if (ret != RCL_RET_OK) {
+      reset();
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
   }
 }
 
@@ -70,7 +73,11 @@ 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);
+
+  {
+    std::lock_guard<std::recursive_mutex> lock(state_handle_mutex_);
+    state_handle_ = const_cast<rcl_lifecycle_state_t *>(rcl_lifecycle_state_handle);
+  }
 }
 
 State::State(const State & rhs)
@@ -101,25 +108,29 @@ State::operator=(const State & rhs)
 
   // we don't own the handle, so we can return straight ahead
   if (!owns_rcl_state_handle_) {
+    std::lock_guard<std::recursive_mutex> lock(state_handle_mutex_);
     state_handle_ = rhs.state_handle_;
     return *this;
   }
 
   // we own the handle, so we have to deep-copy the rhs object
-  state_handle_ = static_cast<rcl_lifecycle_state_t *>(
-    allocator_.allocate(sizeof(rcl_lifecycle_state_t), allocator_.state));
-  if (!state_handle_) {
-    throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t");
-  }
-  // zero initialize
-  state_handle_->id = 0;
-  state_handle_->label = nullptr;
-
-  auto ret = rcl_lifecycle_state_init(
-    state_handle_, rhs.id(), rhs.label().c_str(), &allocator_);
-  if (ret != RCL_RET_OK) {
-    reset();
-    throw std::runtime_error("failed to duplicate label for rcl_lifecycle_state_t");
+  {
+    std::lock_guard<std::recursive_mutex> lock(state_handle_mutex_);
+    state_handle_ = static_cast<rcl_lifecycle_state_t *>(
+      allocator_.allocate(sizeof(rcl_lifecycle_state_t), allocator_.state));
+    if (!state_handle_) {
+      throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t");
+    }
+    // zero initialize
+    state_handle_->id = 0;
+    state_handle_->label = nullptr;
+
+    auto ret = rcl_lifecycle_state_init(
+      state_handle_, rhs.id(), rhs.label().c_str(), &allocator_);
+    if (ret != RCL_RET_OK) {
+      reset();
+      throw std::runtime_error("failed to duplicate label for rcl_lifecycle_state_t");
+    }
   }
 
   return *this;
@@ -128,6 +139,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 +149,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 +159,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;
   }