From 6d13bcb0fc7aa81dc0f8011ce6fe813b42cbca7c Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 5 Dec 2017 22:51:52 -0800 Subject: [PATCH] Fix 394 (#419) * copy and assignment operator for state copy and assignment operator for transition remove unused const_casts address comments check for null in copy constructor up use init and fini functions from rcl remove unused include * explicitly zero initialize state and transitions * add todo comment for follow up --- .../include/rclcpp_lifecycle/state.hpp | 6 + .../include/rclcpp_lifecycle/transition.hpp | 6 + rclcpp_lifecycle/src/state.cpp | 55 +++++++- rclcpp_lifecycle/src/transition.cpp | 120 +++++++++++++++++- rclcpp_lifecycle/test/test_state_wrapper.cpp | 84 ++++++++++++ .../test/test_transition_wrapper.cpp | 21 +++ 6 files changed, 286 insertions(+), 6 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index 6a1a27e..eecbf43 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -44,9 +44,15 @@ public: const rcl_lifecycle_state_t * rcl_lifecycle_state_handle, rcutils_allocator_t allocator = rcutils_get_default_allocator()); + RCLCPP_LIFECYCLE_PUBLIC + State(const State & rhs); + RCLCPP_LIFECYCLE_PUBLIC virtual ~State(); + RCLCPP_LIFECYCLE_PUBLIC + State & operator=(const State & rhs); + RCLCPP_LIFECYCLE_PUBLIC uint8_t id() const; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp index 647079d..a566a22 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -51,9 +51,15 @@ public: const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle, rcutils_allocator_t allocator = rcutils_get_default_allocator()); + RCLCPP_LIFECYCLE_PUBLIC + Transition(const Transition & rhs); + RCLCPP_LIFECYCLE_PUBLIC virtual ~Transition(); + RCLCPP_LIFECYCLE_PUBLIC + Transition & operator=(const Transition & rhs); + RCLCPP_LIFECYCLE_PUBLIC uint8_t id() const; diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index 37e1408..9691472 100644 --- a/rclcpp_lifecycle/src/state.cpp +++ b/rclcpp_lifecycle/src/state.cpp @@ -23,7 +23,6 @@ #include "rclcpp/exceptions.hpp" #include "rcutils/allocator.h" -#include "rcutils/strdup.h" namespace rclcpp_lifecycle { @@ -49,6 +48,9 @@ State::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) { @@ -70,11 +72,58 @@ State::State( state_handle_ = const_cast(rcl_lifecycle_state_handle); } +State::State(const State & rhs) +: allocator_(rhs.allocator_), + owns_rcl_state_handle_(false), + state_handle_(nullptr) +{ + *this = rhs; +} + State::~State() { reset(); } +State & +State::operator=(const State & rhs) +{ + if (this == &rhs) { + return *this; + } + + // reset all currently used resources + reset(); + + allocator_ = rhs.allocator_; + owns_rcl_state_handle_ = rhs.owns_rcl_state_handle_; + + // we don't own the handle, so we can return straight ahead + if (!owns_rcl_state_handle_) { + 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"); + } + + return *this; +} + uint8_t State::id() const { @@ -104,6 +153,10 @@ State::reset() return; } + // TODO(karsten1987): Fini currently deallocate the state_handle_ instance as well + // this should be changed to only deallocate members of the pointer so that stack + // variables can be correctly used as well. + // see https://github.com/ros2/rclcpp/pull/419#discussion_r155157098 auto ret = rcl_lifecycle_state_fini(state_handle_, &allocator_); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret); diff --git a/rclcpp_lifecycle/src/transition.cpp b/rclcpp_lifecycle/src/transition.cpp index 2b313af..ff64533 100644 --- a/rclcpp_lifecycle/src/transition.cpp +++ b/rclcpp_lifecycle/src/transition.cpp @@ -23,7 +23,6 @@ #include "rclcpp/exceptions.hpp" #include "rcutils/allocator.h" -#include "rcutils/strdup.h" namespace rclcpp_lifecycle { @@ -41,6 +40,11 @@ Transition::Transition( if (!transition_handle_) { throw std::runtime_error("failed to allocate memory for rcl_lifecycle_transition_t"); } + // zero initialize + transition_handle_->id = 0; + transition_handle_->label = nullptr; + transition_handle_->start = nullptr; + transition_handle_->goal = nullptr; auto ret = rcl_lifecycle_transition_init( transition_handle_, id, label.c_str(), nullptr, nullptr, &allocator_); @@ -62,6 +66,10 @@ Transition::Transition( reset(); throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t"); } + // zero initialize + transition_handle_->start->id = 0; + transition_handle_->start->label = nullptr; + auto ret = rcl_lifecycle_state_init( transition_handle_->start, start.id(), start.label().c_str(), &allocator_); if (ret != RCL_RET_OK) { @@ -75,6 +83,10 @@ Transition::Transition( reset(); throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t"); } + // zero initialize + transition_handle_->goal->id = 0; + transition_handle_->goal->label = nullptr; + ret = rcl_lifecycle_state_init( transition_handle_->goal, goal.id(), goal.label().c_str(), &allocator_); if (ret != RCL_RET_OK) { @@ -96,16 +108,110 @@ Transition::Transition( transition_handle_ = const_cast(rcl_lifecycle_transition_handle); } +Transition::Transition(const Transition & rhs) +: allocator_(rhs.allocator_), + owns_rcl_transition_handle_(false), + transition_handle_(nullptr) +{ + *this = rhs; +} + Transition::~Transition() { reset(); } +Transition & +Transition::operator=(const Transition & rhs) +{ + if (this == &rhs) { + return *this; + } + + // reset all currently used resources + reset(); + + allocator_ = rhs.allocator_; + owns_rcl_transition_handle_ = rhs.owns_rcl_transition_handle_; + + // we don't own the handle, so we can return straight ahead + if (!owns_rcl_transition_handle_) { + transition_handle_ = rhs.transition_handle_; + return *this; + } + + // we own the handle, so we have to deep-copy the rhs object + 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"); + } + // zero initialize + transition_handle_->id = 0; + transition_handle_->label = nullptr; + transition_handle_->start = nullptr; + transition_handle_->goal = nullptr; + + auto ret = rcl_lifecycle_transition_init( + transition_handle_, rhs.id(), rhs.label().c_str(), nullptr, nullptr, &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + // only copy start state when available + if (rhs.transition_handle_->start) { + 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"); + } + // zero initialize + transition_handle_->start->id = 0; + transition_handle_->start->label = nullptr; + + ret = rcl_lifecycle_state_init( + transition_handle_->start, + rhs.start_state().id(), + rhs.start_state().label().c_str(), + &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + + // only copy goal state when available + if (rhs.transition_handle_->goal) { + 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"); + } + // zero initialize + transition_handle_->goal->id = 0; + transition_handle_->goal->label = nullptr; + + ret = rcl_lifecycle_state_init( + transition_handle_->goal, + rhs.goal_state().id(), + rhs.goal_state().label().c_str(), + &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + return *this; +} + uint8_t Transition::id() const { if (!transition_handle_) { - throw std::runtime_error("Error in state! Internal transition_handle_ is NULL."); + throw std::runtime_error("internal transition_handle is null"); } return transition_handle_->id; } @@ -114,7 +220,7 @@ std::string Transition::label() const { if (!transition_handle_) { - throw std::runtime_error("Error in state! Internal transition_handle_ is NULL."); + throw std::runtime_error("internal transition_handle is null"); } return transition_handle_->label; } @@ -123,7 +229,7 @@ State Transition::start_state() const { if (!transition_handle_) { - throw std::runtime_error("Error in state! Internal transition_handle_ is NULL."); + throw std::runtime_error("internal transition_handle is null"); } // State constructor throws if start pointer is null return State(transition_handle_->start, allocator_); @@ -133,7 +239,7 @@ State Transition::goal_state() const { if (!transition_handle_) { - throw std::runtime_error("Error in state! Internal transition_handle_ is NULL."); + throw std::runtime_error("internal transition_handle is null"); } // State constructor throws if goal pointer is null return State(transition_handle_->goal, allocator_); @@ -151,6 +257,10 @@ Transition::reset() return; } + // TODO(karsten1987): Fini currently deallocate the transition_handle_ instance as well + // this should be changed to only deallocate members of the pointer so that stack + // variables can be correctly used as well. + // see https://github.com/ros2/rclcpp/pull/419#discussion_r155157098 auto ret = rcl_lifecycle_transition_fini(transition_handle_, &allocator_); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret); diff --git a/rclcpp_lifecycle/test/test_state_wrapper.cpp b/rclcpp_lifecycle/test/test_state_wrapper.cpp index 3e7abcf..393cef1 100644 --- a/rclcpp_lifecycle/test/test_state_wrapper.cpp +++ b/rclcpp_lifecycle/test/test_state_wrapper.cpp @@ -82,3 +82,87 @@ TEST_F(TestStateWrapper, wrapper) { // EXPECT_STREQ("my_c_state", c_state.label().c_str()); // } } + +TEST_F(TestStateWrapper, copy_constructor) { + auto a = std::make_shared(1, "my_c_state"); + rclcpp_lifecycle::State b(*a); + + a.reset(); + + EXPECT_EQ(1, b.id()); + EXPECT_STREQ("my_c_state", b.label().c_str()); +} + +TEST_F(TestStateWrapper, assignment_operator) { + auto a = std::make_shared(1, "one"); + auto b = std::make_shared(2, "two"); + *b = *a; + + a.reset(); + + EXPECT_EQ(1, b->id()); + EXPECT_STREQ("one", b->label().c_str()); +} + +TEST_F(TestStateWrapper, assignment_operator2) { + // Non-owning State + rcl_lifecycle_state_t * lc_state1 = + new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, NULL, 0}; + auto non_owning_state1 = std::make_shared(lc_state1); + + // Non-owning State + rcl_lifecycle_state_t * lc_state2 = + new rcl_lifecycle_state_t{"my_c_state2", 2, NULL, NULL, 0}; + auto non_owning_state2 = std::make_shared(lc_state2); + + *non_owning_state2 = *non_owning_state1; + + EXPECT_EQ(1, non_owning_state2->id()); + EXPECT_STREQ("my_c_state1", non_owning_state2->label().c_str()); + + non_owning_state1.reset(); + non_owning_state2.reset(); + + delete lc_state1; + delete lc_state2; +} + +TEST_F(TestStateWrapper, assignment_operator3) { + // Non-owning State + rcl_lifecycle_state_t * lc_state1 = + new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, NULL, 0}; + auto non_owning_state1 = std::make_shared(lc_state1); + + // owning State + auto owning_state2 = std::make_shared(2, "my_c_state2"); + + *owning_state2 = *non_owning_state1; + + EXPECT_EQ(1, owning_state2->id()); + EXPECT_STREQ("my_c_state1", owning_state2->label().c_str()); + + non_owning_state1.reset(); + owning_state2.reset(); + + delete lc_state1; +} + +TEST_F(TestStateWrapper, assignment_operator4) { + // Non-owning State + rcl_lifecycle_state_t * lc_state1 = + new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, NULL, 0}; + auto non_owning_state1 = std::make_shared(lc_state1); + + // owning State + auto owning_state2 = std::make_shared(2, "my_c_state2"); + + *non_owning_state1 = *owning_state2; + + EXPECT_EQ(2, non_owning_state1->id()); + EXPECT_STREQ("my_c_state2", non_owning_state1->label().c_str()); + + non_owning_state1.reset(); + owning_state2.reset(); + + delete lc_state1; +} diff --git a/rclcpp_lifecycle/test/test_transition_wrapper.cpp b/rclcpp_lifecycle/test/test_transition_wrapper.cpp index c5ca131..b920c74 100644 --- a/rclcpp_lifecycle/test/test_transition_wrapper.cpp +++ b/rclcpp_lifecycle/test/test_transition_wrapper.cpp @@ -63,3 +63,24 @@ TEST_F(TestTransitionWrapper, wrapper) { EXPECT_STREQ("from_start_to_goal", t.label().c_str()); } } + +TEST_F(TestTransitionWrapper, copy_constructor) { + auto a = std::make_shared(1, "my_transition"); + rclcpp_lifecycle::Transition b(*a); + + a.reset(); + + EXPECT_EQ(1, b.id()); + EXPECT_STREQ("my_transition", b.label().c_str()); +} + +TEST_F(TestTransitionWrapper, assignment_operator) { + auto a = std::make_shared(1, "one"); + auto b = std::make_shared(2, "two"); + *b = *a; + + a.reset(); + + EXPECT_EQ(1, b->id()); + EXPECT_STREQ("one", b->label().c_str()); +}