From de7ec73dc5e690d25f04aa3f858eb25499d4b04c Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 26 Aug 2021 03:20:23 +0900 Subject: [PATCH] protect state_handle_ with mutex lock. Signed-off-by: Tomoya Fujita --- .../include/rclcpp_lifecycle/state.hpp | 2 + .../src/lifecycle_node_interface_impl.hpp | 17 +++-- rclcpp_lifecycle/src/state.cpp | 70 +++++++++++-------- 3 files changed, 52 insertions(+), 37 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index a0ac997ff3..95b12cdc90 100644 --- 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 #include #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_; }; diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 77478acdbc..c474f08d12 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -182,9 +182,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl std::lock_guard lock(state_machine_mutex_); srv_get_available_transitions_ = std::make_shared>( - 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(srv_get_available_transitions_), @@ -202,9 +202,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl std::lock_guard lock(state_machine_mutex_); srv_get_transition_graph_ = std::make_shared>( - 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(srv_get_transition_graph_), @@ -233,8 +233,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl { std::lock_guard 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 lock(state_machine_mutex_); transition = diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index f7aca2688e..4ca8d02500 100644 --- a/rclcpp_lifecycle/src/state.cpp +++ b/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( - 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 lock(state_handle_mutex_); + state_handle_ = static_cast( + 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_handle); + + { + std::lock_guard lock(state_handle_mutex_); + state_handle_ = const_cast(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 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( - 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 lock(state_handle_mutex_); + state_handle_ = static_cast( + 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 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 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 lock(state_handle_mutex_); if (!owns_rcl_state_handle_) { state_handle_ = nullptr; }