Increase rcl_lifecycle test coverage and add more safety checks (#649)

* Increase test coverage and add more safety checks

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* More coverage

Signed-off-by: Stephen Brawner <brawner@gmail.com>

* Concatenating error messages

Signed-off-by: Stephen Brawner <stephenbrawner@verbsurgical.com>

* PR Fixup

Signed-off-by: Stephen Brawner <stephenbrawner@verbsurgical.com>

* Forgot a

Signed-off-by: Stephen Brawner <stephenbrawner@verbsurgical.com>

* PR Fixup

Signed-off-by: Stephen Brawner <stephenbrawner@verbsurgical.com>

* Moving var to top

Signed-off-by: Stephen Brawner <stephenbrawner@verbsurgical.com>

Co-authored-by: Stephen Brawner <stephenbrawner@verbsurgical.com>

rcl_lifecycle/CMakeLists.txt

@@ -89,6 +89,16 @@ if(BUILD_TESTING)
     )
     target_link_libraries(test_multiple_instances ${PROJECT_NAME})
   endif()
+  ament_add_gtest(test_rcl_lifecycle
+    test/test_rcl_lifecycle.cpp
+  )
+  if(TARGET test_rcl_lifecycle)
+    ament_target_dependencies(test_rcl_lifecycle
+      "rcl"
+      "osrf_testing_tools_cpp"
+    )
+    target_link_libraries(test_rcl_lifecycle ${PROJECT_NAME})
+  endif()
   ament_add_gtest(test_transition_map
     test/test_transition_map.cpp
   )

rcl_lifecycle/src/default_state_machine.c

@@ -650,13 +650,15 @@ _register_transitions(
   return ret;
 }
 
-// default implementation as despicted on
+// default implementation as depicted on
 // design.ros2.org
 rcl_ret_t
 rcl_lifecycle_init_default_state_machine(
   rcl_lifecycle_state_machine_t * state_machine, const rcutils_allocator_t * allocator)
 {
   rcl_ret_t ret = RCL_RET_ERROR;
+  // Used for concatenating error messages in the fail: block.
+  const char * fail_error_message = "";
 
   // ***************************
   // register all primary states
@@ -691,8 +693,26 @@ rcl_lifecycle_init_default_state_machine(
   return ret;
 
 fail:
+  // If rcl_lifecycle_transition_map_fini() fails, it will clobber the error string here.
+  // Concatenate the error strings if that happens
+  if (rcl_error_is_set()) {
+    fail_error_message = rcl_get_error_string().str;
+  }
+
   if (rcl_lifecycle_transition_map_fini(&state_machine->transition_map, allocator) != RCL_RET_OK) {
-    RCL_SET_ERROR_MSG("could not free lifecycle transition map. Leaking memory!\n");
+    const char * fini_error = "";
+    if (rcl_error_is_set()) {
+      fini_error = rcl_get_error_string().str;
+      rcl_reset_error();
+    }
+    RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "Freeing transition map failed while handling a previous error. Leaking memory!"
+      "\nOriginal error:\n\t%s\nError encountered in rcl_lifecycle_transition_map_fini():\n\t%s\n",
+      fail_error_message, fini_error);
+  }
+
+  if (!rcl_error_is_set()) {
+    RCL_SET_ERROR_MSG("Unspecified error in default_state_machine _register_transitions()");
   }
   return RCL_RET_ERROR;
 }

rcl_lifecycle/src/rcl_lifecycle.c

@@ -40,6 +40,8 @@ rcl_lifecycle_get_zero_initialized_state()
   rcl_lifecycle_state_t state;
   state.id = 0;
   state.label = NULL;
+  state.valid_transitions = NULL;
+  state.valid_transition_size = 0;
   return state;
 }
 
@@ -58,6 +60,10 @@ rcl_lifecycle_state_init(
     RCL_SET_ERROR_MSG("state pointer is null\n");
     return RCL_RET_ERROR;
   }
+  if (!label) {
+    RCL_SET_ERROR_MSG("State label is null\n");
+    return RCL_RET_ERROR;
+  }
 
   state->id = id;
   state->label = rcutils_strndup(label, strlen(label), *allocator);
@@ -118,7 +124,22 @@ rcl_lifecycle_transition_init(
 
   if (!transition) {
     RCL_SET_ERROR_MSG("transition pointer is null\n");
-    return RCL_RET_OK;
+    return RCL_RET_ERROR;
+  }
+
+  if (!label) {
+    RCL_SET_ERROR_MSG("label pointer is null\n");
+    return RCL_RET_ERROR;
+  }
+
+  if (!start) {
+    RCL_SET_ERROR_MSG("start state pointer is null\n");
+    return RCL_RET_ERROR;
+  }
+
+  if (!goal) {
+    RCL_SET_ERROR_MSG("goal state pointer is null\n");
+    return RCL_RET_ERROR;
   }
 
   transition->start = start;
@@ -140,7 +161,7 @@ rcl_lifecycle_transition_fini(
   const rcl_allocator_t * allocator)
 {
   if (!allocator) {
-    RCL_SET_ERROR_MSG("can't initialize transition, no allocator given\n");
+    RCL_SET_ERROR_MSG("can't finalize transition, no allocator given\n");
     return RCL_RET_ERROR;
   }
   // it is already NULL
@@ -192,6 +213,14 @@ rcl_lifecycle_state_machine_init(
   bool default_states,
   const rcl_allocator_t * allocator)
 {
+  if (!state_machine) {
+    RCL_SET_ERROR_MSG("State machine is null\n");
+    return RCL_RET_ERROR;
+  }
+  if (!node_handle) {
+    RCL_SET_ERROR_MSG("Node handle is null\n");
+    return RCL_RET_ERROR;
+  }
   if (!allocator) {
     RCL_SET_ERROR_MSG("can't initialize state machine, no allocator given\n");
     return RCL_RET_ERROR;
@@ -207,17 +236,14 @@ rcl_lifecycle_state_machine_init(
   }
 
   if (default_states) {
-    rcl_ret_t ret =
-      rcl_lifecycle_init_default_state_machine(state_machine, allocator);
+    ret = rcl_lifecycle_init_default_state_machine(state_machine, allocator);
     if (ret != RCL_RET_OK) {
       // init default state machine might have allocated memory,
       // so we have to call fini
-      if (rcl_lifecycle_state_machine_fini(state_machine, node_handle, allocator) != RCL_RET_OK) {
-        // error already set
+      ret = rcl_lifecycle_state_machine_fini(state_machine, node_handle, allocator);
       return RCL_RET_ERROR;
     }
   }
-  }
 
   return RCL_RET_OK;
 }

rcl_lifecycle/src/transition_map.c

@@ -53,6 +53,11 @@ rcl_lifecycle_transition_map_fini(
   rcl_lifecycle_transition_map_t * transition_map,
   const rcutils_allocator_t * allocator)
 {
+  if (!allocator) {
+    RCL_SET_ERROR_MSG("can't free transition map, no allocator given\n");
+    return RCL_RET_ERROR;
+  }
+
   rcl_ret_t fcn_ret = RCL_RET_OK;
 
   // free valid transitions for all states
@@ -87,15 +92,16 @@ rcl_lifecycle_register_state(
     allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT)
 
   // add new primary state memory
-  transition_map->states_size += 1;
+  unsigned int new_states_size = transition_map->states_size + 1;
   rcl_lifecycle_state_t * new_states = allocator->reallocate(
     transition_map->states,
-    transition_map->states_size * sizeof(rcl_lifecycle_state_t),
+    new_states_size * sizeof(rcl_lifecycle_state_t),
     allocator->state);
   if (!new_states) {
     RCL_SET_ERROR_MSG("failed to reallocate memory for new states");
     return RCL_RET_ERROR;
   }
+  transition_map->states_size = new_states_size;
   transition_map->states = new_states;
   transition_map->states[transition_map->states_size - 1] = state;
 
@@ -117,16 +123,22 @@ rcl_lifecycle_register_transition(
     return RCL_RET_ERROR;
   }
 
-  // we add a new transition, so increase the size
-  transition_map->transitions_size += 1;
+  rcl_lifecycle_state_t * goal = rcl_lifecycle_get_state(transition_map, transition.goal->id);
+  if (!goal) {
+    RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("state %u is not registered\n", transition.goal->id);
+    return RCL_RET_ERROR;
+  }
+  // Attempt to add new transition, don't update map if it fails
+  unsigned int new_transitions_size = transition_map->transitions_size + 1;
   rcl_lifecycle_transition_t * new_transitions = allocator->reallocate(
     transition_map->transitions,
-    transition_map->transitions_size * sizeof(rcl_lifecycle_transition_t),
+    new_transitions_size * sizeof(rcl_lifecycle_transition_t),
     allocator->state);
   if (!new_transitions) {
     RCL_SET_ERROR_MSG("failed to reallocate memory for new transitions");
     return RCL_RET_BAD_ALLOC;
   }
+  transition_map->transitions_size = new_transitions_size;
   transition_map->transitions = new_transitions;
   // finally set the new transition to the end of the array
   transition_map->transitions[transition_map->transitions_size - 1] = transition;
@@ -134,15 +146,16 @@ rcl_lifecycle_register_transition(
   // we have to copy the transitons here once more to the actual state
   // as we can't assign only the pointer. This pointer gets invalidated whenever
   // we add a new transition and re-shuffle/re-allocate new memory for it.
-  state->valid_transition_size += 1;
+  unsigned int new_valid_transitions_size = state->valid_transition_size + 1;
   rcl_lifecycle_transition_t * new_valid_transitions = allocator->reallocate(
     state->valid_transitions,
-    state->valid_transition_size * sizeof(rcl_lifecycle_transition_t),
+    new_valid_transitions_size * sizeof(rcl_lifecycle_transition_t),
     allocator->state);
   if (!new_valid_transitions) {
     RCL_SET_ERROR_MSG("failed to reallocate memory for new transitions on state");
     return RCL_RET_ERROR;
   }
+  state->valid_transition_size = new_valid_transitions_size;
   state->valid_transitions = new_valid_transitions;
 
   state->valid_transitions[state->valid_transition_size - 1] = transition;

rcl_lifecycle/test/test_default_state_machine.cpp

@@ -113,7 +113,13 @@ TEST_F(TestDefaultStateMachine, zero_init) {
 TEST_F(TestDefaultStateMachine, default_init) {
   rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine();
 
-  auto ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator);
+  // Because this init method is so complex, the succession of failures caused by a null
+  // allocator will result in several error messages overwriting themselves.
+  auto ret = rcl_lifecycle_init_default_state_machine(&state_machine, nullptr);
+  EXPECT_EQ(RCL_RET_ERROR, ret);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_init_default_state_machine(&state_machine, this->allocator);
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
   ret = rcl_lifecycle_state_machine_fini(&state_machine, this->node_ptr, this->allocator);

rcl_lifecycle/test/test_rcl_lifecycle.cpp

@@ -0,0 +1,427 @@
+// Copyright 2020 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.
+
+// testing default transition sequence.
+// This test requires that the transitions are set
+// as depicted in design.ros2.org
+
+#include <gtest/gtest.h>
+
+#include "rcl_lifecycle/rcl_lifecycle.h"
+#include "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp"
+#include "osrf_testing_tools_cpp/scope_exit.hpp"
+#include "rcl/error_handling.h"
+#include "lifecycle_msgs/msg/transition_event.h"
+#include "lifecycle_msgs/srv/change_state.h"
+#include "lifecycle_msgs/srv/get_available_states.h"
+#include "lifecycle_msgs/srv/get_available_transitions.h"
+#include "lifecycle_msgs/srv/get_state.h"
+
+static void * bad_malloc(size_t, void *)
+{
+  return nullptr;
+}
+
+static void * bad_realloc(void *, size_t, void *)
+{
+  return nullptr;
+}
+
+TEST(TestRclLifecycle, lifecycle_state) {
+  rcl_lifecycle_state_t state = rcl_lifecycle_get_zero_initialized_state();
+  EXPECT_EQ(state.id, 0u);
+  EXPECT_EQ(state.label, nullptr);
+
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  unsigned int expected_id = 42;
+  const char expected_label[] = "label";
+  rcl_ret_t ret = rcl_lifecycle_state_init(&state, expected_id, &expected_label[0], nullptr);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_state_init(&state, expected_id, nullptr, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_state_init(nullptr, expected_id, &expected_label[0], &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  rcl_allocator_t bad_allocator = rcl_get_default_allocator();
+  bad_allocator.allocate = bad_malloc;
+  bad_allocator.reallocate = bad_realloc;
+  ret = rcl_lifecycle_state_init(&state, expected_id, &expected_label[0], &bad_allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_state_init(&state, expected_id, &expected_label[0], &allocator);
+  EXPECT_EQ(state.id, expected_id);
+  EXPECT_STREQ(state.label, &expected_label[0]);
+
+  ret = rcl_lifecycle_state_fini(&state, nullptr);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  // Already finalized
+  ret = rcl_lifecycle_state_fini(nullptr, &allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  ret = rcl_lifecycle_state_fini(&state, &allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+}
+
+TEST(TestRclLifecycle, lifecycle_transition) {
+  rcl_lifecycle_transition_t transition = rcl_lifecycle_get_zero_initialized_transition();
+  EXPECT_EQ(transition.id, 0u);
+  EXPECT_EQ(transition.label, nullptr);
+  EXPECT_EQ(transition.start, nullptr);
+  EXPECT_EQ(transition.goal, nullptr);
+
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+
+  // These need to be allocated on heap so rcl_lifecycle_transition_fini doesn't free a stack
+  // allocated variable
+  rcl_lifecycle_state_t * start = reinterpret_cast<rcl_lifecycle_state_t *>(
+    allocator.allocate(sizeof(rcl_lifecycle_state_t), allocator.state));
+  EXPECT_NE(start, nullptr);
+  rcl_lifecycle_state_t * end = reinterpret_cast<rcl_lifecycle_state_t *>(
+    allocator.allocate(sizeof(rcl_lifecycle_state_t), allocator.state));
+  EXPECT_NE(end, nullptr);
+  const char start_label[] = "start";
+  const char end_label[] = "end";
+  *start = rcl_lifecycle_get_zero_initialized_state();
+  *end = rcl_lifecycle_get_zero_initialized_state();
+
+  rcl_ret_t ret = rcl_lifecycle_state_init(start, 0u, &start_label[0], &allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  ret = rcl_lifecycle_state_init(end, 1u, &end_label[0], &allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  unsigned int expected_id = 42;
+  const char expected_label[] = "label";
+
+  ret = rcl_lifecycle_transition_init(
+    nullptr, expected_id, nullptr, nullptr, nullptr, nullptr);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_transition_init(
+    &transition, expected_id, nullptr, nullptr, nullptr, nullptr);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_transition_init(
+    nullptr, expected_id, nullptr, nullptr, nullptr, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_transition_init(
+    &transition, expected_id, nullptr, nullptr, nullptr, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_transition_init(
+    &transition, expected_id, &expected_label[0], nullptr, nullptr, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_transition_init(
+    &transition, expected_id, &expected_label[0], start, nullptr, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  rcl_allocator_t bad_allocator = rcl_get_default_allocator();
+  bad_allocator.allocate = bad_malloc;
+  bad_allocator.reallocate = bad_realloc;
+  ret = rcl_lifecycle_transition_init(
+    &transition, expected_id, &expected_label[0], start, end, &bad_allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_transition_init(
+    &transition, expected_id, &expected_label[0], start, end, &allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  EXPECT_EQ(transition.id, expected_id);
+  EXPECT_STREQ(transition.label, &expected_label[0]);
+
+  ret = rcl_lifecycle_transition_fini(nullptr, nullptr);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_transition_fini(&transition, nullptr);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  // Already finalized
+  ret = rcl_lifecycle_transition_fini(nullptr, &allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  ret = rcl_lifecycle_transition_fini(&transition, &allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+}
+
+TEST(TestRclLifecycle, state_machine) {
+  rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine();
+  EXPECT_EQ(state_machine.current_state, nullptr);
+  EXPECT_EQ(state_machine.transition_map.states, nullptr);
+  EXPECT_EQ(state_machine.transition_map.transitions, nullptr);
+  EXPECT_EQ(state_machine.transition_map.states_size, 0u);
+  EXPECT_EQ(state_machine.transition_map.transitions_size, 0u);
+
+  rcl_node_t node = rcl_get_zero_initialized_node();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_context_t context = rcl_get_zero_initialized_context();
+  rcl_node_options_t options = rcl_node_get_default_options();
+  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
+  rcl_ret_t ret = rcl_init_options_init(&init_options, allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  ret = rcl_init(0, nullptr, &init_options, &context);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context));
+    ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context));
+  });
+
+  ret = rcl_node_init(&node, "node", "namespace", &context, &options);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  const rosidl_message_type_support_t * pn =
+    ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent);
+  const rosidl_service_type_support_t * cs =
+    ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, ChangeState);
+  const rosidl_service_type_support_t * gs =
+    ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetState);
+  const rosidl_service_type_support_t * gas =
+    ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableStates);
+  const rosidl_service_type_support_t * gat =
+    ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions);
+  const rosidl_service_type_support_t * gtg =
+    ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions);
+
+  // Check various arguments are null
+  ret = rcl_lifecycle_state_machine_init(
+    nullptr, &node, pn, cs, gs, gas, gat, gtg, false, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+  EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_state_machine_init(
+    &state_machine, nullptr, pn, cs, gs, gas, gat, gtg, false, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+  EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_state_machine_init(
+    &state_machine, &node, nullptr, cs, gs, gas, gat, gtg, false, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+  EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_state_machine_init(
+    &state_machine, &node, pn, nullptr, gs, gas, gat, gtg, false, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+  EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_state_machine_init(
+    &state_machine, &node, pn, cs, nullptr, gas, gat, gtg, false, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+  EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_state_machine_init(
+    &state_machine, &node, pn, cs, gs, nullptr, gat, gtg, false, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+  EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_state_machine_init(
+    &state_machine, &node, pn, cs, gs, gas, nullptr, gtg, false, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+  EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_state_machine_init(
+    &state_machine, &node, pn, cs, gs, gas, gat, nullptr, false, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+  EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_state_machine_init(
+    &state_machine, &node, pn, cs, gs, gas, gat, gtg, false, nullptr);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+  EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR);
+  rcutils_reset_error();
+
+
+  // Everything should be good
+  ret = rcl_lifecycle_state_machine_init(
+    &state_machine, &node, pn, cs, gs, gas, gat, gtg, false, &allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  // Transition_map is not initialized
+  EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  void * temp_function = state_machine.com_interface.srv_change_state.impl;
+  state_machine.com_interface.srv_change_state.impl = nullptr;
+  // get_state service is valid, but not change_state service
+  EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_ERROR);
+  rcutils_reset_error();
+  state_machine.com_interface.srv_change_state.impl =
+    reinterpret_cast<rcl_service_impl_t *>(temp_function);
+
+  // Allocate some memory and initialize states and transitions so is_initialized will pass
+  state_machine.transition_map.states_size = 1u;
+  state_machine.transition_map.states = reinterpret_cast<rcl_lifecycle_state_t *>(
+    allocator.allocate(
+      state_machine.transition_map.states_size * sizeof(rcl_lifecycle_state_t),
+      allocator.state));
+  ASSERT_NE(state_machine.transition_map.states, nullptr);
+  state_machine.transition_map.states[0] = rcl_lifecycle_get_zero_initialized_state();
+
+  state_machine.transition_map.transitions_size = 1u;
+  state_machine.transition_map.transitions =
+    reinterpret_cast<rcl_lifecycle_transition_t *>(allocator.allocate(
+      state_machine.transition_map.transitions_size * sizeof(rcl_lifecycle_transition_t),
+      allocator.state));
+  ASSERT_NE(state_machine.transition_map.transitions, nullptr);
+  state_machine.transition_map.transitions[0] = rcl_lifecycle_get_zero_initialized_transition();
+
+  EXPECT_EQ(rcl_lifecycle_state_machine_is_initialized(&state_machine), RCL_RET_OK);
+
+  // allocator is nullptr
+  ret = rcl_lifecycle_state_machine_fini(&state_machine, &node, nullptr);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_state_machine_fini(&state_machine, &node, &allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  state_machine = rcl_lifecycle_get_zero_initialized_state_machine();
+
+  // Node is null
+  ret = rcl_lifecycle_state_machine_fini(&state_machine, nullptr, &allocator);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  std::cout << "state_machine: " << __LINE__ << std::endl;
+}
+
+TEST(TestRclLifecycle, state_transitions) {
+  rcl_lifecycle_state_machine_t state_machine =
+    rcl_lifecycle_get_zero_initialized_state_machine();
+  EXPECT_EQ(state_machine.current_state, nullptr);
+  EXPECT_EQ(state_machine.transition_map.states, nullptr);
+  EXPECT_EQ(state_machine.transition_map.transitions, nullptr);
+  EXPECT_EQ(state_machine.transition_map.states_size, 0u);
+  EXPECT_EQ(state_machine.transition_map.transitions_size, 0u);
+
+  rcl_node_t node = rcl_get_zero_initialized_node();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_context_t context = rcl_get_zero_initialized_context();
+  rcl_node_options_t options = rcl_node_get_default_options();
+  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
+  rcl_ret_t ret = rcl_init_options_init(&init_options, allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  ret = rcl_init(0, nullptr, &init_options, &context);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context));
+    ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context));
+  });
+
+  ret = rcl_node_init(&node, "node", "namespace", &context, &options);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  const rosidl_message_type_support_t * pn =
+    ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent);
+  const rosidl_service_type_support_t * cs =
+    ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, ChangeState);
+  const rosidl_service_type_support_t * gs =
+    ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetState);
+  const rosidl_service_type_support_t * gas =
+    ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableStates);
+  const rosidl_service_type_support_t * gat =
+    ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions);
+  const rosidl_service_type_support_t * gtg =
+    ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions);
+
+  ret = rcl_lifecycle_state_machine_init(
+    &state_machine, &node, pn, cs, gs, gas, gat, gtg, true, &allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  ret = rcl_lifecycle_state_machine_is_initialized(&state_machine);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  const rcl_lifecycle_transition_t * transition = rcl_lifecycle_get_transition_by_id(nullptr, 0);
+  EXPECT_EQ(transition, nullptr) << rcl_get_error_string().str;
+  rcutils_reset_error();
+
+  transition = rcl_lifecycle_get_transition_by_id(
+    state_machine.current_state, lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE);
+  EXPECT_EQ(transition->id, lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE);
+
+  // Update this test with a new invalid number if 42 ever becomes a valid state id
+  transition = rcl_lifecycle_get_transition_by_id(state_machine.current_state, 42);
+  EXPECT_EQ(transition, nullptr) << rcl_get_error_string().str;
+  rcutils_reset_error();
+
+  transition = rcl_lifecycle_get_transition_by_label(state_machine.current_state, "configure");
+  EXPECT_STREQ(transition->label, "configure");
+
+  transition = rcl_lifecycle_get_transition_by_label(state_machine.current_state, "NOT A LABEL");
+  EXPECT_EQ(transition, nullptr) << rcl_get_error_string().str;
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_trigger_transition_by_id(nullptr, 0, false);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_trigger_transition_by_id(
+    &state_machine, lifecycle_msgs__msg__Transition__TRANSITION_CONFIGURE, false);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  ret = rcl_lifecycle_trigger_transition_by_label(nullptr, "transition_success", true);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  ret = rcl_lifecycle_trigger_transition_by_label(&state_machine, "transition_success", true);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+  // If using the public interface to register transitions, this case should already be checked.
+  state_machine.current_state->valid_transitions[0].goal = nullptr;
+  ret = rcl_lifecycle_trigger_transition_by_label(&state_machine, "transition_success", true);
+  EXPECT_EQ(ret, RCL_RET_ERROR);
+  rcutils_reset_error();
+
+  rcl_print_state_machine(&state_machine);
+  EXPECT_FALSE(rcutils_error_is_set());
+
+  ret = rcl_lifecycle_state_machine_fini(&state_machine, &node, &allocator);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+}

rcl_lifecycle/test/test_transition_map.cpp

@@ -35,11 +35,22 @@ protected:
   }
 };
 
+static void * bad_malloc(size_t, void *)
+{
+  return nullptr;
+}
+
+static void * bad_realloc(void *, size_t, void *)
+{
+  return nullptr;
+}
+
 TEST_F(TestTransitionMap, zero_initialized) {
   rcl_lifecycle_transition_map_t transition_map =
     rcl_lifecycle_get_zero_initialized_transition_map();
 
   EXPECT_EQ(RCL_RET_ERROR, rcl_lifecycle_transition_map_is_initialized(&transition_map));
+  rcutils_reset_error();
 
   rcl_allocator_t allocator = rcl_get_default_allocator();
   EXPECT_EQ(RCL_RET_OK, rcl_lifecycle_transition_map_fini(&transition_map, &allocator));
@@ -57,11 +68,25 @@ TEST_F(TestTransitionMap, initialized) {
   EXPECT_EQ(RCL_RET_OK, rcl_lifecycle_transition_map_is_initialized(&transition_map));
 
   ret = rcl_lifecycle_register_state(&transition_map, state0, &allocator);
-  EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str;
+  EXPECT_EQ(RCL_RET_ERROR, ret);
+  rcutils_reset_error();
 
   rcl_lifecycle_state_t state1 = {"my_state_1", 1, NULL, 0};
   ret = rcl_lifecycle_register_state(&transition_map, state1, &allocator);
 
+  rcl_lifecycle_state_t unregistered = {"my_state_2", 2, NULL, 0};
+
+  rcl_allocator_t bad_allocator = rcl_get_default_allocator();
+  bad_allocator.allocate = bad_malloc;
+  bad_allocator.reallocate = bad_realloc;
+  rcl_lifecycle_state_t * original_ptr = transition_map.states;
+  size_t original_size = transition_map.states_size;
+  ret = rcl_lifecycle_register_state(&transition_map, unregistered, &bad_allocator);
+  EXPECT_EQ(RCL_RET_ERROR, ret);
+  rcutils_reset_error();
+  EXPECT_EQ(transition_map.states, original_ptr);
+  EXPECT_EQ(original_size, transition_map.states_size);
+
   rcl_lifecycle_state_t * start_state =
     rcl_lifecycle_get_state(&transition_map, state0.id);
   rcl_lifecycle_state_t * goal_state =
@@ -71,25 +96,70 @@ TEST_F(TestTransitionMap, initialized) {
 
   rcl_lifecycle_transition_t transition01 = {"from0to1", 0,
     start_state, goal_state};
+  original_size = transition_map.transitions_size;
   ret = rcl_lifecycle_register_transition(
     &transition_map, transition01, &allocator);
   EXPECT_EQ(RCL_RET_OK, ret);
+  EXPECT_EQ(1u, transition_map.transitions_size);
 
   rcl_lifecycle_transition_t transition10 = {"from1to0", 1,
     goal_state, start_state};
+  original_size = transition_map.transitions_size;
   ret = rcl_lifecycle_register_transition(
     &transition_map, transition10, &allocator);
   EXPECT_EQ(RCL_RET_OK, ret);
+  EXPECT_EQ(2u, transition_map.transitions_size);
+
+  rcl_lifecycle_transition_t * old_transitions_ptr = transition_map.transitions;
+  original_size = transition_map.transitions_size;
+  rcl_lifecycle_transition_t transition_bad1 = {"from0tobad", 2,
+    start_state, &unregistered};
+  ret = rcl_lifecycle_register_transition(
+    &transition_map, transition_bad1, &allocator);
+  EXPECT_EQ(RCL_RET_ERROR, ret);
+  rcutils_reset_error();
+  EXPECT_EQ(old_transitions_ptr, transition_map.transitions);
+  EXPECT_EQ(original_size, transition_map.transitions_size);
+
+  old_transitions_ptr = transition_map.transitions;
+  original_size = transition_map.transitions_size;
+  rcl_lifecycle_transition_t transition_bad2 = {"frombadto1", 3,
+    &unregistered, goal_state};
+  ret = rcl_lifecycle_register_transition(
+    &transition_map, transition_bad2, &allocator);
+  EXPECT_EQ(RCL_RET_ERROR, ret);
+  rcutils_reset_error();
+  EXPECT_EQ(old_transitions_ptr, transition_map.transitions);
+  EXPECT_EQ(original_size, transition_map.transitions_size);
+
+  old_transitions_ptr = transition_map.transitions;
+  original_size = transition_map.transitions_size;
+  rcl_lifecycle_transition_t transition00 = {"from0to0", 4,
+    start_state, start_state};
+  ret = rcl_lifecycle_register_transition(
+    &transition_map, transition00, &bad_allocator);
+  EXPECT_EQ(RCL_RET_BAD_ALLOC, ret);
+  rcutils_reset_error();
+  EXPECT_EQ(old_transitions_ptr, transition_map.transitions);
+  EXPECT_EQ(original_size, transition_map.transitions_size);
 
   const rcl_lifecycle_transition_t * trans =
     rcl_lifecycle_get_transition_by_id(start_state, 0);
   EXPECT_EQ(0u, trans->id);
+  trans = rcl_lifecycle_get_transitions(&transition_map, 0);
+  EXPECT_EQ(0u, trans->id);
   trans = rcl_lifecycle_get_transition_by_label(start_state, "from0to1");
   EXPECT_EQ(0u, trans->id);
   trans = rcl_lifecycle_get_transition_by_id(goal_state, 1);
   EXPECT_EQ(1u, trans->id);
+  trans = rcl_lifecycle_get_transitions(&transition_map, 1);
+  EXPECT_EQ(1u, trans->id);
   trans = rcl_lifecycle_get_transition_by_label(goal_state, "from1to0");
   EXPECT_EQ(1u, trans->id);
+  // Check nonexistent transition
+  trans = rcl_lifecycle_get_transitions(&transition_map, 2);
+  EXPECT_EQ(nullptr, trans);
+  rcutils_reset_error();
 
   EXPECT_EQ(RCL_RET_OK, rcl_lifecycle_transition_map_fini(&transition_map, &allocator));
 }