diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 7bf52ef..98a3db2 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -90,6 +90,15 @@ if(BUILD_TESTING) ) target_link_libraries(test_state_wrapper ${PROJECT_NAME}) endif() + ament_add_gtest(test_transition_wrapper test/test_transition_wrapper.cpp) + if(TARGET test_transition_wrapper) + target_include_directories(test_transition_wrapper PUBLIC + ${rcl_lifecycle_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${rclcpp_lifecycle_INCLUDE_DIRS} + ) + target_link_libraries(test_transition_wrapper ${PROJECT_NAME}) + endif() endif() ament_export_dependencies(rclcpp) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index 2d153b6..6a1a27e 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -19,6 +19,8 @@ #include "rclcpp_lifecycle/visibility_control.h" +#include "rcutils/allocator.h" + // forward declare rcl_state_t typedef struct rcl_lifecycle_state_t rcl_lifecycle_state_t; @@ -29,13 +31,18 @@ class State { public: RCLCPP_LIFECYCLE_PUBLIC - State(); + explicit State(rcutils_allocator_t allocator = rcutils_get_default_allocator()); RCLCPP_LIFECYCLE_PUBLIC - State(uint8_t id, const std::string & label); + State( + uint8_t id, + const std::string & label, + rcutils_allocator_t allocator = rcutils_get_default_allocator()); RCLCPP_LIFECYCLE_PUBLIC - explicit State(const rcl_lifecycle_state_t * rcl_lifecycle_state_handle); + explicit State( + const rcl_lifecycle_state_t * rcl_lifecycle_state_handle, + rcutils_allocator_t allocator = rcutils_get_default_allocator()); RCLCPP_LIFECYCLE_PUBLIC virtual ~State(); @@ -49,8 +56,15 @@ public: label() const; protected: + RCLCPP_LIFECYCLE_PUBLIC + void + reset(); + + rcutils_allocator_t allocator_; + bool owns_rcl_state_handle_; - const rcl_lifecycle_state_t * state_handle_; + + rcl_lifecycle_state_t * state_handle_; }; } // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp index 529a8fb..647079d 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -20,6 +20,8 @@ #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/visibility_control.h" +#include "rcutils/allocator.h" + // forward declare rcl_transition_t typedef struct rcl_lifecycle_transition_t rcl_lifecycle_transition_t; @@ -33,15 +35,21 @@ public: Transition() = delete; RCLCPP_LIFECYCLE_PUBLIC - explicit Transition(uint8_t id, const std::string & label = ""); + explicit Transition( + uint8_t id, + const std::string & label = "", + rcutils_allocator_t allocator = rcutils_get_default_allocator()); RCLCPP_LIFECYCLE_PUBLIC Transition( uint8_t id, const std::string & label, - State && start, State && goal); + State && start, State && goal, + rcutils_allocator_t allocator = rcutils_get_default_allocator()); RCLCPP_LIFECYCLE_PUBLIC - explicit Transition(const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle); + explicit Transition( + const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle, + rcutils_allocator_t allocator = rcutils_get_default_allocator()); RCLCPP_LIFECYCLE_PUBLIC virtual ~Transition(); @@ -63,9 +71,15 @@ public: goal_state() const; protected: + RCLCPP_LIFECYCLE_PUBLIC + void + reset(); + + rcutils_allocator_t allocator_; + bool owns_rcl_transition_handle_; - const rcl_lifecycle_transition_t * transition_handle_; + rcl_lifecycle_transition_t * transition_handle_; }; } // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index 98a9105..37e1408 100644 --- a/rclcpp_lifecycle/src/state.cpp +++ b/rclcpp_lifecycle/src/state.cpp @@ -14,47 +14,65 @@ #include "rclcpp_lifecycle/state.hpp" -#include - -#include -#include -#include - #include +#include "lifecycle_msgs/msg/state.hpp" + +#include "rcl_lifecycle/rcl_lifecycle.h" + +#include "rclcpp/exceptions.hpp" + +#include "rcutils/allocator.h" +#include "rcutils/strdup.h" + namespace rclcpp_lifecycle { -State::State() -: State(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, "unknown") +State::State(rcutils_allocator_t allocator) +: State(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, "unknown", allocator) {} -State::State(uint8_t id, const std::string & label) -: owns_rcl_state_handle_(true) +State::State( + uint8_t id, + const std::string & label, + rcutils_allocator_t allocator) +: allocator_(allocator), + owns_rcl_state_handle_(true), + state_handle_(nullptr) { if (label.empty()) { throw std::runtime_error("Lifecycle State cannot have an empty label."); } - auto state_handle = new rcl_lifecycle_state_t; - state_handle->id = id; - state_handle->label = - rcutils_strndup(label.c_str(), label.size(), rcutils_get_default_allocator()); + 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"); + } - state_handle_ = state_handle; + 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); + } } -State::State(const rcl_lifecycle_state_t * rcl_lifecycle_state_handle) -: owns_rcl_state_handle_(false) +State::State( + const rcl_lifecycle_state_t * rcl_lifecycle_state_handle, + rcutils_allocator_t allocator) +: allocator_(allocator), + owns_rcl_state_handle_(false), + state_handle_(nullptr) { - state_handle_ = rcl_lifecycle_state_handle; + if (!rcl_lifecycle_state_handle) { + throw std::runtime_error("rcl_lifecycle_state_handle is null"); + } + state_handle_ = const_cast(rcl_lifecycle_state_handle); } State::~State() { - if (owns_rcl_state_handle_) { - delete state_handle_; - } + reset(); } uint8_t @@ -75,4 +93,21 @@ State::label() const return state_handle_->label; } +void +State::reset() +{ + if (!owns_rcl_state_handle_) { + state_handle_ = nullptr; + } + + if (!state_handle_) { + return; + } + + auto ret = rcl_lifecycle_state_fini(state_handle_, &allocator_); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + } // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/transition.cpp b/rclcpp_lifecycle/src/transition.cpp index 8ca0333..2b313af 100644 --- a/rclcpp_lifecycle/src/transition.cpp +++ b/rclcpp_lifecycle/src/transition.cpp @@ -14,89 +14,146 @@ #include "rclcpp_lifecycle/transition.hpp" -#include -#include - #include +#include "lifecycle_msgs/msg/transition.hpp" + +#include "rcl_lifecycle/rcl_lifecycle.h" + +#include "rclcpp/exceptions.hpp" + +#include "rcutils/allocator.h" +#include "rcutils/strdup.h" + namespace rclcpp_lifecycle { -Transition::Transition(uint8_t id, const std::string & label) -: owns_rcl_transition_handle_(true) +Transition::Transition( + uint8_t id, + const std::string & label, + rcutils_allocator_t allocator) +: allocator_(allocator), + owns_rcl_transition_handle_(true), + transition_handle_(nullptr) { - auto transition_handle = new rcl_lifecycle_transition_t; - transition_handle->id = id; - transition_handle->label = label.c_str(); + transition_handle_ = static_cast( + allocator_.allocate(sizeof(rcl_lifecycle_transition_t), allocator_.state)); + if (!transition_handle_) { + throw std::runtime_error("failed to allocate memory for rcl_lifecycle_transition_t"); + } - transition_handle->start = nullptr; - transition_handle->goal = nullptr; - transition_handle_ = transition_handle; + auto ret = rcl_lifecycle_transition_init( + transition_handle_, id, label.c_str(), nullptr, nullptr, &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } } Transition::Transition( uint8_t id, const std::string & label, - State && start, State && goal) -: owns_rcl_transition_handle_(true) + State && start, State && goal, + rcutils_allocator_t allocator) +: Transition(id, label, allocator) { - auto transition_handle = new rcl_lifecycle_transition_t; - transition_handle->id = id; - transition_handle->label = label.c_str(); + transition_handle_->start = static_cast( + allocator_.allocate(sizeof(rcl_lifecycle_state_t), allocator_.state)); + if (!transition_handle_->start) { + reset(); + throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t"); + } + auto ret = rcl_lifecycle_state_init( + transition_handle_->start, start.id(), start.label().c_str(), &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } - auto start_state = new rcl_lifecycle_state_t; - start_state->id = start.id(); - start_state->label = start.label().c_str(); - - auto goal_state = new rcl_lifecycle_state_t; - goal_state->id = goal.id(); - goal_state->label = start.label().c_str(); - - transition_handle->start = start_state; - transition_handle->goal = goal_state; - transition_handle_ = transition_handle; + transition_handle_->goal = static_cast( + allocator_.allocate(sizeof(rcl_lifecycle_state_t), allocator_.state)); + if (!transition_handle_->goal) { + reset(); + throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t"); + } + ret = rcl_lifecycle_state_init( + transition_handle_->goal, goal.id(), goal.label().c_str(), &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } } -Transition::Transition(const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle) -: owns_rcl_transition_handle_(false) +Transition::Transition( + const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle, + rcutils_allocator_t allocator) +: allocator_(allocator), + owns_rcl_transition_handle_(false), + transition_handle_(nullptr) { - transition_handle_ = rcl_lifecycle_transition_handle; + if (!rcl_lifecycle_transition_handle) { + throw std::runtime_error("rcl_lifecycle_transition_handle is null"); + } + transition_handle_ = const_cast(rcl_lifecycle_transition_handle); } Transition::~Transition() { - if (owns_rcl_transition_handle_) { - if (transition_handle_->start) { - delete transition_handle_->start; - } - if (transition_handle_->goal) { - delete transition_handle_->goal; - } - delete transition_handle_; - } + reset(); } uint8_t Transition::id() const { + if (!transition_handle_) { + throw std::runtime_error("Error in state! Internal transition_handle_ is NULL."); + } return transition_handle_->id; } std::string Transition::label() const { + if (!transition_handle_) { + throw std::runtime_error("Error in state! Internal transition_handle_ is NULL."); + } return transition_handle_->label; } State Transition::start_state() const { - return State(transition_handle_->start); + if (!transition_handle_) { + throw std::runtime_error("Error in state! Internal transition_handle_ is NULL."); + } + // State constructor throws if start pointer is null + return State(transition_handle_->start, allocator_); } State Transition::goal_state() const { - return State(transition_handle_->goal); + if (!transition_handle_) { + throw std::runtime_error("Error in state! Internal transition_handle_ is NULL."); + } + // State constructor throws if goal pointer is null + return State(transition_handle_->goal, allocator_); } +void +Transition::reset() +{ + // can't free anything which is not owned + if (!owns_rcl_transition_handle_) { + transition_handle_ = nullptr; + } + + if (!transition_handle_) { + return; + } + + auto ret = rcl_lifecycle_transition_fini(transition_handle_, &allocator_); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} } // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/test/test_state_wrapper.cpp b/rclcpp_lifecycle/test/test_state_wrapper.cpp index 6d0e490..3e7abcf 100644 --- a/rclcpp_lifecycle/test/test_state_wrapper.cpp +++ b/rclcpp_lifecycle/test/test_state_wrapper.cpp @@ -36,6 +36,13 @@ TEST_F(TestStateWrapper, wrapper) { EXPECT_STREQ("my_state", state.label().c_str()); } + { + std::string state_name("my_state"); + rclcpp_lifecycle::State state(1, state_name); + state_name = "not_my_state"; + EXPECT_STREQ("my_state", state.label().c_str()); + } + { rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, NULL, 0}; rclcpp_lifecycle::State c_state(lc_state.id, lc_state.label); @@ -59,9 +66,9 @@ TEST_F(TestStateWrapper, wrapper) { EXPECT_EQ(3, c_state.id()); EXPECT_FALSE(c_state.label().empty()); EXPECT_STREQ("my_c_state", c_state.label().c_str()); + delete lc_state; } - // introduces flakiness // unsupported behavior! // fails when compiled with memory sanitizer diff --git a/rclcpp_lifecycle/test/test_transition_wrapper.cpp b/rclcpp_lifecycle/test/test_transition_wrapper.cpp new file mode 100644 index 0000000..c5ca131 --- /dev/null +++ b/rclcpp_lifecycle/test/test_transition_wrapper.cpp @@ -0,0 +1,65 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +class TestTransitionWrapper : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + } +}; + +TEST_F(TestTransitionWrapper, empty_transition) { + auto a = std::make_shared(1, "my_transition"); + EXPECT_NO_THROW(a.reset()); +} + +TEST_F(TestTransitionWrapper, wrapper) { + { + rclcpp_lifecycle::Transition t(12, "no_states_set"); + EXPECT_EQ(12, t.id()); + EXPECT_STREQ("no_states_set", t.label().c_str()); + } + + { + std::string transition_name = "no_states_set"; + rclcpp_lifecycle::Transition t(12, transition_name); + transition_name = "not_no_states_set"; + EXPECT_EQ(12, t.id()); + EXPECT_STREQ("no_states_set", t.label().c_str()); + } + + { + rclcpp_lifecycle::State start_state(1, "start_state"); + rclcpp_lifecycle::State goal_state(2, "goal_state"); + + rclcpp_lifecycle::Transition t( + 12, + "from_start_to_goal", + std::move(start_state), + std::move(goal_state)); + + EXPECT_EQ(12, t.id()); + EXPECT_FALSE(t.label().empty()); + EXPECT_STREQ("from_start_to_goal", t.label().c_str()); + } +}