Lifecycle refactor (#298)

* no static initialization of states anymore

* make transition labels more descriptive

* introduce labeled keys

* define default transition keys

* fix memory management

* introduce service for transition graph

* export transition keys

* remove keys, transition id unique, label ambiguous

* semicolon for macro call

rcl_lifecycle/src/com_interface.c

@@ -42,6 +42,7 @@ static const char * srv_change_state_service = "~/change_state";
 static const char * srv_get_state_service = "~/get_state";
 static const char * srv_get_available_states_service = "~/get_available_states";
 static const char * srv_get_available_transitions_service = "~/get_available_transitions";
+static const char * srv_get_transition_graph = "~/get_transition_graph";
 
 rcl_lifecycle_com_interface_t
 rcl_lifecycle_get_zero_initialized_com_interface()
@@ -53,6 +54,7 @@ rcl_lifecycle_get_zero_initialized_com_interface()
   com_interface.srv_get_state = rcl_get_zero_initialized_service();
   com_interface.srv_get_available_states = rcl_get_zero_initialized_service();
   com_interface.srv_get_available_transitions = rcl_get_zero_initialized_service();
+  com_interface.srv_get_transition_graph = rcl_get_zero_initialized_service();
   return com_interface;
 }
 
@@ -65,6 +67,7 @@ rcl_lifecycle_com_interface_init(
   const rosidl_service_type_support_t * ts_srv_get_state,
   const rosidl_service_type_support_t * ts_srv_get_available_states,
   const rosidl_service_type_support_t * ts_srv_get_available_transitions,
+  const rosidl_service_type_support_t * ts_srv_get_transition_graph,
   const rcl_allocator_t * allocator)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator())
@@ -77,6 +80,8 @@ rcl_lifecycle_com_interface_init(
     ts_srv_get_available_states, RCL_RET_INVALID_ARGUMENT, *allocator)
   RCL_CHECK_ARGUMENT_FOR_NULL(
     ts_srv_get_available_transitions, RCL_RET_INVALID_ARGUMENT, *allocator)
+  RCL_CHECK_ARGUMENT_FOR_NULL(
+    ts_srv_get_transition_graph, RCL_RET_INVALID_ARGUMENT, *allocator)
 
   // initialize publisher
   {
@@ -141,6 +146,17 @@ rcl_lifecycle_com_interface_init(
     }
   }
 
+  // initialize get transition graph service
+  {
+    rcl_service_options_t service_options = rcl_service_get_default_options();
+    rcl_ret_t ret = rcl_service_init(
+      &com_interface->srv_get_transition_graph, node_handle,
+      ts_srv_get_transition_graph, srv_get_transition_graph, &service_options);
+
+    if (ret != RCL_RET_OK) {
+      goto fail;
+    }
+  }
   return RCL_RET_OK;
 
 fail:
@@ -160,6 +176,10 @@ fail:
     RCUTILS_LOG_ERROR_NAMED(
       ROS_PACKAGE_NAME, "Failed to destroy get_available_transitions service");
   }
+  if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_transition_graph, node_handle)) {
+    RCUTILS_LOG_ERROR_NAMED(
+      ROS_PACKAGE_NAME, "Failed to destroy get_transition_graph service");
+  }
 
   return RCL_RET_ERROR;
 }
@@ -171,6 +191,15 @@ rcl_lifecycle_com_interface_fini(
 {
   rcl_ret_t fcn_ret = RCL_RET_OK;
 
+  // destroy get transition graph srv
+  {
+    rcl_ret_t ret = rcl_service_fini(
+      &com_interface->srv_get_transition_graph, node_handle);
+    if (ret != RCL_RET_OK) {
+      fcn_ret = RCL_RET_ERROR;
+    }
+  }
+
   // destroy get available transitions srv
   {
     rcl_ret_t ret = rcl_service_fini(

rcl_lifecycle/src/com_interface.h

@@ -37,6 +37,7 @@ rcl_lifecycle_com_interface_init(
   const rosidl_service_type_support_t * ts_srv_get_state,
   const rosidl_service_type_support_t * ts_srv_get_available_states,
   const rosidl_service_type_support_t * ts_srv_get_available_transitions,
+  const rosidl_service_type_support_t * ts_srv_get_transition_graph,
   const rcl_allocator_t * allocator);
 
 rcl_ret_t

rcl_lifecycle/src/default_state_machine.h

@@ -1,39 +0,0 @@
-// Copyright 2016 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.
-
-#ifndef DEFAULT_STATE_MACHINE_H_
-#define DEFAULT_STATE_MACHINE_H_
-
-#include "rcl/macros.h"
-#include "rcl/types.h"
-
-#include "rcl_lifecycle/data_types.h"
-#include "rcl_lifecycle/visibility_control.h"
-
-#ifdef __cplusplus
-extern "C"
-{
-#endif
-
-RCL_LIFECYCLE_PUBLIC
-RCL_WARN_UNUSED
-rcl_ret_t
-rcl_lifecycle_init_default_state_machine(
-  rcl_lifecycle_state_machine_t * state_machine, const rcl_allocator_t * allocator);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif  // DEFAULT_STATE_MACHINE_H_

rcl_lifecycle/src/rcl_lifecycle.c

@@ -17,6 +17,8 @@ extern "C"
 {
 #endif
 
+#include "rcl_lifecycle/rcl_lifecycle.h"
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -27,12 +29,10 @@ extern "C"
 #include "rcutils/logging_macros.h"
 #include "rcutils/strdup.h"
 
-#include "rcl_lifecycle/rcl_lifecycle.h"
+#include "rcl_lifecycle/default_state_machine.h"
 #include "rcl_lifecycle/transition_map.h"
 
-#include "com_interface.h"  // NOLINT
-#include "default_state_machine.h"  // NOLINT
-#include "states.h"  // NOLINT
+#include "./com_interface.h"
 
 rcl_lifecycle_state_t
 rcl_lifecycle_get_zero_initialized_state()
@@ -192,6 +192,7 @@ rcl_lifecycle_state_machine_init(
   const rosidl_service_type_support_t * ts_srv_get_state,
   const rosidl_service_type_support_t * ts_srv_get_available_states,
   const rosidl_service_type_support_t * ts_srv_get_available_transitions,
+  const rosidl_service_type_support_t * ts_srv_get_transition_graph,
   bool default_states,
   const rcl_allocator_t * allocator)
 {
@@ -205,7 +206,7 @@ rcl_lifecycle_state_machine_init(
     &state_machine->com_interface, node_handle,
     ts_pub_notify,
     ts_srv_change_state, ts_srv_get_state,
-    ts_srv_get_available_states, ts_srv_get_available_transitions,
+    ts_srv_get_available_states, ts_srv_get_available_transitions, ts_srv_get_transition_graph,
     allocator);
   if (ret != RCL_RET_OK) {
     return RCL_RET_ERROR;
@@ -277,45 +278,57 @@ rcl_lifecycle_state_machine_is_initialized(const rcl_lifecycle_state_machine_t *
 }
 
 const rcl_lifecycle_transition_t *
-rcl_lifecycle_is_valid_transition(
-  rcl_lifecycle_state_machine_t * state_machine,
-  rcl_lifecycle_transition_key_t key)
+rcl_lifecycle_get_transition_by_id(
+  const rcl_lifecycle_state_t * state,
+  uint8_t id)
 {
-  unsigned int current_id = state_machine->current_state->id;
-  const rcl_lifecycle_state_t * current_state = rcl_lifecycle_get_state(
-    &state_machine->transition_map, current_id);
+  RCL_CHECK_FOR_NULL_WITH_MSG(state,
+    "state pointer is null", return NULL, rcl_get_default_allocator());
 
-  RCL_CHECK_FOR_NULL_WITH_MSG(current_state,
-    "rcl_lifecycle_get_state returns NULL", return NULL, rcl_get_default_allocator());
-
-  for (unsigned int i = 0; i < current_state->valid_transition_size; ++i) {
-    if (current_state->valid_transition_keys[i] == key) {
-      return &current_state->valid_transitions[i];
+  for (unsigned int i = 0; i < state->valid_transition_size; ++i) {
+    if (state->valid_transitions[i].id == id) {
+      return &state->valid_transitions[i];
     }
   }
 
   RCUTILS_LOG_WARN_NAMED(
     ROS_PACKAGE_NAME,
-    "No callback transition matching %d found for current state %s",
-    key, state_machine->current_state->label);
+    "No transition matching %d found for current state %s",
+    id, state->label);
+
+  return NULL;
+}
+
+const rcl_lifecycle_transition_t *
+rcl_lifecycle_get_transition_by_label(
+  const rcl_lifecycle_state_t * state,
+  const char * label)
+{
+  RCL_CHECK_FOR_NULL_WITH_MSG(state,
+    "state pointer is null", return NULL, rcl_get_default_allocator());
+
+  for (unsigned int i = 0; i < state->valid_transition_size; ++i) {
+    if (strcmp(state->valid_transitions[i].label, label) == 0) {
+      return &state->valid_transitions[i];
+    }
+  }
+
+  RCUTILS_LOG_WARN_NAMED(
+    ROS_PACKAGE_NAME,
+    "No transition matching %s found for current state %s",
+    label, state->label);
 
   return NULL;
 }
 
 rcl_ret_t
-rcl_lifecycle_trigger_transition(
+_trigger_transition(
   rcl_lifecycle_state_machine_t * state_machine,
-  rcl_lifecycle_transition_key_t key, bool publish_notification)
+  const rcl_lifecycle_transition_t * transition,
+  bool publish_notification)
 {
-  const rcl_lifecycle_transition_t * transition =
-    rcl_lifecycle_is_valid_transition(state_machine, key);
-
   // If we have a faulty transition pointer
   if (!transition) {
-    RCUTILS_LOG_ERROR_NAMED(
-      ROS_PACKAGE_NAME,
-      "No transition found for node %s with key %d",
-      state_machine->current_state->label, key);
     RCL_SET_ERROR_MSG("Transition is not registered.", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
@@ -326,6 +339,7 @@ rcl_lifecycle_trigger_transition(
     return RCL_RET_ERROR;
   }
   state_machine->current_state = transition->goal;
+
   if (publish_notification) {
     rcl_ret_t ret = rcl_lifecycle_com_interface_publish_notification(
       &state_machine->com_interface, transition->start, state_machine->current_state);
@@ -338,6 +352,39 @@ rcl_lifecycle_trigger_transition(
   return RCL_RET_OK;
 }
 
+rcl_ret_t
+rcl_lifecycle_trigger_transition_by_id(
+  rcl_lifecycle_state_machine_t * state_machine,
+  uint8_t id,
+  bool publish_notification)
+{
+  if (!state_machine) {
+    RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "state machine pointer is null");
+    return RCL_RET_ERROR;
+  }
+
+  const rcl_lifecycle_transition_t * transition =
+    rcl_lifecycle_get_transition_by_id(state_machine->current_state, id);
+
+  return _trigger_transition(state_machine, transition, publish_notification);
+}
+
+rcl_ret_t
+rcl_lifecycle_trigger_transition_by_label(
+  rcl_lifecycle_state_machine_t * state_machine,
+  const char * label,
+  bool publish_notification)
+{
+  if (!state_machine) {
+    RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "state machine pointer is null");
+    return RCL_RET_ERROR;
+  }
+
+  const rcl_lifecycle_transition_t * transition =
+    rcl_lifecycle_get_transition_by_label(state_machine->current_state, label);
+
+  return _trigger_transition(state_machine, transition, publish_notification);
+}
 
 void
 rcl_print_state_machine(const rcl_lifecycle_state_machine_t * state_machine)
@@ -353,9 +400,8 @@ rcl_print_state_machine(const rcl_lifecycle_state_machine_t * state_machine)
     for (size_t j = 0; j < map->states[i].valid_transition_size; ++j) {
       RCUTILS_LOG_INFO_NAMED(
         ROS_PACKAGE_NAME,
-        "\tNode %s: Key %d: Transition: %s",
+        "\tNode %s: Transition: %s",
         map->states[i].label,
-        map->states[i].valid_transition_keys[j],
         map->states[i].valid_transitions[j].label);
     }
   }

rcl_lifecycle/src/states.h

@@ -1,51 +0,0 @@
-// Copyright 2015 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.
-
-#ifndef STATES_H_
-#define STATES_H_
-
-#include "rcl_lifecycle/data_types.h"
-#include "rcl_lifecycle/visibility_control.h"
-
-#ifdef __cplusplus
-extern "C"
-{
-#endif
-
-// primary states based on
-// design.ros2.org/articles/node_lifecycle.html
-extern rcl_lifecycle_state_t rcl_state_unknown;
-extern rcl_lifecycle_state_t rcl_state_unconfigured;
-extern rcl_lifecycle_state_t rcl_state_inactive;
-extern rcl_lifecycle_state_t rcl_state_active;
-extern rcl_lifecycle_state_t rcl_state_finalized;
-
-extern rcl_lifecycle_state_t rcl_state_configuring;
-extern rcl_lifecycle_state_t rcl_state_cleaningup;
-extern rcl_lifecycle_state_t rcl_state_shuttingdown;
-extern rcl_lifecycle_state_t rcl_state_activating;
-extern rcl_lifecycle_state_t rcl_state_deactivating;
-extern rcl_lifecycle_state_t rcl_state_errorprocessing;
-
-extern const rcl_lifecycle_transition_t rcl_transition_configure;
-extern const rcl_lifecycle_transition_t rcl_transition_cleanup;
-extern const rcl_lifecycle_transition_t rcl_transition_shutdown;
-extern const rcl_lifecycle_transition_t rcl_transition_activate;
-extern const rcl_lifecycle_transition_t rcl_transition_deactivate;
-
-#ifdef __cplusplus
-}
-#endif  // extern "C"
-
-#endif  // STATES_H_

rcl_lifecycle/src/transition_map.c

@@ -55,11 +55,6 @@ rcl_lifecycle_transition_map_fini(
 {
   rcl_ret_t fcn_ret = RCL_RET_OK;
 
-  // for each state free the allocations for their keys/transitions
-  for (unsigned int i = 0; i < transition_map->states_size; ++i) {
-    allocator->deallocate(transition_map->states[i].valid_transition_keys, allocator->state);
-    allocator->deallocate(transition_map->states[i].valid_transitions, allocator->state);
-  }
   // free the primary states
   allocator->deallocate(transition_map->states, allocator->state);
   transition_map->states = NULL;
@@ -106,7 +101,6 @@ rcl_ret_t
 rcl_lifecycle_register_transition(
   rcl_lifecycle_transition_map_t * transition_map,
   rcl_lifecycle_transition_t transition,
-  rcl_lifecycle_transition_key_t key,
   const rcutils_allocator_t * allocator)
 {
   RCUTILS_CHECK_ALLOCATOR_WITH_MSG(
@@ -135,7 +129,6 @@ rcl_lifecycle_register_transition(
   // finally set the new transition to the end of the array
   transition_map->transitions[transition_map->transitions_size - 1] = transition;
 
-  // connect transition to state key
   // 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.
@@ -152,20 +145,6 @@ rcl_lifecycle_register_transition(
   }
   state->valid_transitions = new_valid_transitions;
 
-  rcl_lifecycle_transition_key_t * new_valid_transition_keys = allocator->reallocate(
-    state->valid_transition_keys,
-    state->valid_transition_size * sizeof(rcl_lifecycle_transition_key_t),
-    allocator->state);
-  if (!new_valid_transitions) {
-    RCL_SET_ERROR_MSG(
-      "failed to reallocate memory for new transitions keys on state",
-      rcl_get_default_allocator());
-    return RCL_RET_ERROR;
-  }
-  state->valid_transition_keys = new_valid_transition_keys;
-
-  // assign key
-  state->valid_transition_keys[state->valid_transition_size - 1] = key;
   state->valid_transitions[state->valid_transition_size - 1] = transition;
 
   return RCL_RET_OK;