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

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;

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;

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_t *>(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<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;
+}
+
 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);

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_t *>(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<rcl_lifecycle_transition_t *>(
+    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<rcl_lifecycle_state_t *>(
+      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<rcl_lifecycle_state_t *>(
+      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);

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<rclcpp_lifecycle::State>(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<rclcpp_lifecycle::State>(1, "one");
+  auto b = std::make_shared<rclcpp_lifecycle::State>(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<rclcpp_lifecycle::State>(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<rclcpp_lifecycle::State>(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<rclcpp_lifecycle::State>(lc_state1);
+
+  // owning State
+  auto owning_state2 = std::make_shared<rclcpp_lifecycle::State>(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<rclcpp_lifecycle::State>(lc_state1);
+
+  // owning State
+  auto owning_state2 = std::make_shared<rclcpp_lifecycle::State>(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;
+}

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<rclcpp_lifecycle::Transition>(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<rclcpp_lifecycle::Transition>(1, "one");
+  auto b = std::make_shared<rclcpp_lifecycle::Transition>(2, "two");
+  *b = *a;
+
+  a.reset();
+
+  EXPECT_EQ(1, b->id());
+  EXPECT_STREQ("one", b->label().c_str());
+}