Fix 393 (#418)

* correctly copy state label

* correctly copy transition label

* uncrustify

* address comments

* up

* use init and fini functions from rcl

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)

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

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

rclcpp_lifecycle/src/state.cpp

@@ -14,47 +14,65 @@
 
 #include "rclcpp_lifecycle/state.hpp"
 
-#include <lifecycle_msgs/msg/state.hpp>
-
-#include <rcutils/allocator.h>
-#include <rcutils/strdup.h>
-#include <rcl_lifecycle/data_types.h>
-
 #include <string>
 
+#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_ = state_handle;
+  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");
   }
 
-State::State(const rcl_lifecycle_state_t * rcl_lifecycle_state_handle)
-: owns_rcl_state_handle_(false)
+  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,
+  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_t *>(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

rclcpp_lifecycle/src/transition.cpp

@@ -14,89 +14,146 @@
 
 #include "rclcpp_lifecycle/transition.hpp"
 
-#include <lifecycle_msgs/msg/transition.hpp>
-#include <rcl_lifecycle/data_types.h>
-
 #include <string>
 
+#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<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");
+  }
 
-  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();
-
-  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_->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");
+  }
+  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);
   }
 
-Transition::Transition(const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle)
-: owns_rcl_transition_handle_(false)
+  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");
+  }
+  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,
+  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_t *>(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

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

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 <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#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<rclcpp_lifecycle::Transition>(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());
+  }
+}