(dev) cleanup for c-statemachine

(dev) cleanup for c-statemachine

Author: Karsten1987

rclcpp_lifecycle/include/rcl_lifecycle/lifecycle_state.h

@@ -54,11 +54,10 @@ typedef struct LIFECYCLE_EXPORT _rcl_state_t
  */
 typedef struct LIFECYCLE_EXPORT _rcl_state_transition_t
 {
-  rcl_state_t transition_index;
-  //rcl_state_t transition_state;
-  // function callback
+  rcl_state_t transition_state;
   void* callback;
-  rcl_state_t goal;
+  rcl_state_t* start;
+  rcl_state_t* goal;
 } rcl_state_transition_t;
 
 /**
@@ -71,7 +70,6 @@ typedef struct LIFECYCLE_EXPORT _rcl_state_transition_t
  */
 typedef struct LIFECYCLE_EXPORT _rcl_state_machine_t
 {
-  // current state of the lifecycle
   rcl_state_t* current_state;
   rcl_transition_map_t transition_map;
 } rcl_state_machine_t;
@@ -82,10 +80,17 @@ typedef struct LIFECYCLE_EXPORT _rcl_state_machine_t
  * @brief traverses the transition map of the given
  * state machine to find if there is a transition from the
  * current state to the specified goal state
+ * @return the transition state which is valid
+ * NULL if not available
  */
-bool
+const rcl_state_transition_t*
 LIFECYCLE_EXPORT rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* goal_state);
 
+const rcl_state_transition_t*
+LIFECYCLE_EXPORT rcl_get_registered_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_state_index);
+const rcl_state_transition_t*
+LIFECYCLE_EXPORT rcl_get_registered_transition_by_label(rcl_state_machine_t* state_machine, const char* transition_state_label);
+
 rcl_state_t
 LIFECYCLE_EXPORT rcl_create_state(unsigned int state, char* label);
 

rclcpp_lifecycle/include/rcl_lifecycle/transition_map.h

@@ -24,50 +24,79 @@ extern "C"
 #endif
 
 typedef struct _rcl_state_t rcl_state_t;
-
 typedef struct _rcl_state_transition_t rcl_state_transition_t;
 
 /**
- * @brief The actual data array
- * within the map. One array belongs
- * to one label within the map.
+ * @brief All transitions which are
+ * valid associations for a primary state.
+ * One array belongs to one primary state
+ * within the map.
  */
-typedef struct LIFECYCLE_EXPORT _data_array
+typedef struct LIFECYCLE_EXPORT _transition_array
 {
   rcl_state_transition_t* transitions;
   unsigned int size;
 } rcl_transition_array_t;
 
-/**
- * @brief helper index for keeping
- * track of the map content
- */
-typedef struct LIFECYCLE_EXPORT _index
-{
-  unsigned int index;
-  const char* label;
-} rcl_transition_map_index_t;
-
 /**
  * @brief stores an array of transitions
  * index by a start state
  */
 typedef struct LIFECYCLE_EXPORT _map
 {
   //rcl_state_t* state_index;
-  rcl_transition_map_index_t* state_index;
+  // associative array between primary state and their respective transitions
+  // 1 ps -> 1 transition_array
+  rcl_state_t* primary_states;
   rcl_transition_array_t* transition_arrays;
   unsigned int size;
 } rcl_transition_map_t;
 
+LIFECYCLE_EXPORT
+void
+rcl_register_primary_state(rcl_transition_map_t* m, rcl_state_t primary_state);
+
+/**
+ * @brief appends a transition in a map
+ * the classification is based on the start state
+ * within the given transition
+ */
+LIFECYCLE_EXPORT
+void
+rcl_register_transition_by_state(rcl_transition_map_t* m, rcl_state_t start, rcl_state_t goal, rcl_state_transition_t transition);
+
 /**
  * @brief appends a transition in a map
  * the classification is based on the start state
  * within the given transition
  */
 LIFECYCLE_EXPORT
 void
-rcl_append_transition(rcl_transition_map_t* m, rcl_state_t state, rcl_state_transition_t transition);
+rcl_register_transition_by_label(rcl_transition_map_t* m, const char* start_label, const char* goal_label, rcl_state_transition_t transition);
+
+/**
+ * @brief appends a transition in a map
+ * the classification is based on the start state
+ * within the given transition
+ */
+LIFECYCLE_EXPORT
+void
+rcl_register_transition_by_index(rcl_transition_map_t* m, unsigned int start_index, unsigned int goal_index, rcl_state_transition_t transition);
+
+/**
+ * @brief gets the registered primary state based on a label
+ * @return primary state pointer, NULL if not found
+ */
+LIFECYCLE_EXPORT
+rcl_state_t*
+rcl_get_primary_state_by_label(rcl_transition_map_t* m, const char* label);
+/**
+ * @brief gets the registered primary state based on a index
+ * @return primary state pointer, NULL if not found
+ */
+LIFECYCLE_EXPORT
+rcl_state_t*
+rcl_get_primary_state_by_index(rcl_transition_map_t* m, unsigned int index);
 
 /**
  * @brief gets all transitions based on a label
@@ -89,7 +118,7 @@ rcl_get_transitions_by_index(rcl_transition_map_t* m, unsigned int index);
  */
 LIFECYCLE_EXPORT
 void
-rcl_print_transition_array(const rcl_transition_array_t* da);
+rcl_print_transition_array(const rcl_transition_array_t* transition_array);
 LIFECYCLE_EXPORT
 void
 rcl_print_transition_map(const rcl_transition_map_t* m);

rclcpp_lifecycle/src/default_state_machine.c

@@ -35,9 +35,9 @@ rcl_create_state(unsigned int index, char* label)
 }
 
 rcl_state_transition_t
-rcl_create_state_transition(rcl_state_t index, rcl_state_t goal)
+rcl_create_state_transition(unsigned int index, const char* label)
 {
-  rcl_state_transition_t ret_transition = {.transition_index = index, NULL, .goal = goal};
+  rcl_state_transition_t ret_transition = {{.index = index, .label = label}, NULL, NULL, NULL};
   return ret_transition;
 }
 
@@ -47,39 +47,39 @@ rcl_state_machine_t
 rcl_get_default_state_machine()
 {
   rcl_state_machine_t state_machine;
-  state_machine.transition_map.state_index = NULL;
+  state_machine.transition_map.primary_states = NULL;
   state_machine.transition_map.transition_arrays = NULL;
   state_machine.transition_map.size = 0;
 
   // set default state as unconfigured
-  state_machine.current_state = &rcl_state_unconfigured;
+  //state_machine.current_state = &rcl_state_unconfigured;
 
-  rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_unconfigured_inactive
-    = {rcl_state_configuring, NULL, rcl_state_inactive};
-  rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_unconfigured_finalized
-    = {rcl_state_shuttingdown, NULL, rcl_state_finalized};
+  rcl_state_transition_t rcl_transition_configuring
+    = rcl_create_state_transition(rcl_state_configuring.index, rcl_state_configuring.label);
+  rcl_state_transition_t rcl_transition_shuttingdown
+    = rcl_create_state_transition(rcl_state_shuttingdown.index, rcl_state_shuttingdown.label);
+  rcl_state_transition_t rcl_transition_cleaningup
+    = rcl_create_state_transition(rcl_state_cleaningup.index, rcl_state_cleaningup.label);
+  rcl_state_transition_t rcl_transition_activating
+    = rcl_create_state_transition(rcl_state_activating.index, rcl_state_activating.label);
+  rcl_state_transition_t rcl_transition_deactivating
+    = rcl_create_state_transition(rcl_state_deactivating.index, rcl_state_deactivating.label);;
 
-  rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_inactive_unconfigured
-    = {rcl_state_cleaningup, NULL, rcl_state_unconfigured};
-  rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_inactive_finalized
-    = {rcl_state_shuttingdown, NULL, rcl_state_finalized};
-  rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_inactive_active
-    = {rcl_state_activating, NULL, rcl_state_active};
-
-  rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_active_inactive
-    = {rcl_state_deactivating, NULL, rcl_state_inactive};
-  rcl_state_transition_t LIFECYCLE_EXPORT rcl_transition_active_finalized
-    = {rcl_state_shuttingdown, NULL, rcl_state_finalized};
+  rcl_register_primary_state(&state_machine.transition_map, rcl_state_unconfigured);
+  rcl_register_primary_state(&state_machine.transition_map, rcl_state_inactive);
+  rcl_register_primary_state(&state_machine.transition_map, rcl_state_active);
+  rcl_register_primary_state(&state_machine.transition_map, rcl_state_finalized);
 
   // add transitions to map
-  rcl_append_transition(&state_machine.transition_map, rcl_state_unconfigured, rcl_transition_unconfigured_inactive);
-  rcl_append_transition(&state_machine.transition_map, rcl_state_unconfigured, rcl_transition_unconfigured_finalized);
-  rcl_append_transition(&state_machine.transition_map, rcl_state_inactive, rcl_transition_inactive_unconfigured);
-  rcl_append_transition(&state_machine.transition_map, rcl_state_inactive, rcl_transition_inactive_finalized);
-  rcl_append_transition(&state_machine.transition_map, rcl_state_inactive, rcl_transition_inactive_active);
-  rcl_append_transition(&state_machine.transition_map, rcl_state_active, rcl_transition_active_inactive);
-  rcl_append_transition(&state_machine.transition_map, rcl_state_active, rcl_transition_active_finalized);
+  rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_unconfigured, rcl_state_inactive, rcl_transition_configuring);
+  rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_unconfigured, rcl_transition_cleaningup);
+  rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_unconfigured, rcl_state_finalized, rcl_transition_shuttingdown);
+  rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_finalized, rcl_transition_shuttingdown);
+  rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_inactive, rcl_state_active, rcl_transition_activating);
+  rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_active, rcl_state_inactive, rcl_transition_deactivating);
+  rcl_register_transition_by_state(&state_machine.transition_map, rcl_state_active, rcl_state_finalized, rcl_transition_shuttingdown);
 
+  state_machine.current_state = &state_machine.transition_map.primary_states[0]; // set to first entry
   return state_machine;
 }
 

rclcpp_lifecycle/src/lifecycle_state.c

@@ -17,36 +17,73 @@ extern "C"
 {
 #endif
 
-#include "rcl_lifecycle/lifecycle_state.h"
+#include <stdlib.h>
+#include <string.h>
 
-typedef rcl_state_transition_t rcl_transition;
-typedef rcl_state_t rcl_state;
+#include "rcl_lifecycle/lifecycle_state.h"
 
-bool
+const rcl_state_transition_t*
 rcl_is_valid_transition(rcl_state_machine_t* state_machine, const rcl_state_t* goal_state)
 {
-  rcl_transition_array_t* all_transitions = rcl_get_transitions_by_label(&state_machine->transition_map, state_machine->current_state->label);
+  unsigned int current_index = state_machine->current_state->index;
+  const rcl_transition_array_t* valid_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, current_index);
+  for (unsigned int i=0; i<valid_transitions->size; ++i)
+  {
+    if (valid_transitions->transitions[i].goal->index == goal_state->index)
+    {
+      return &valid_transitions->transitions[i];
+    }
+  }
+  return NULL;
+}
 
-  for (unsigned int i=0; i<all_transitions->size; ++i)
+const rcl_state_transition_t*
+rcl_get_registered_transition_by_index(rcl_state_machine_t* state_machine, unsigned int transition_state_index)
+{
+  // extensive search approach
+  // TODO(karsten1987) can be improved by having a separate array for "registered transition"
+  const rcl_transition_map_t* map = &state_machine->transition_map;
+  for (unsigned int i=0; i<map->size; ++i)
   {
-    if (all_transitions->transitions[i].goal.index == goal_state->index)
+    for (unsigned int j=0; j<map->transition_arrays[i].size; ++j)
     {
-      return true;
+      if (map->transition_arrays[i].transitions[j].transition_state.index == transition_state_index)
+      {
+        return &map->transition_arrays[i].transitions[j];
+      }
     }
   }
-  return false;
+  return NULL;
+}
+
+const rcl_state_transition_t*
+rcl_get_registered_transition_by_label(rcl_state_machine_t* state_machine, const char* transition_state_label)
+{
+  // extensive search approach
+  // TODO(karsten1987) can be improved by having a separate array for "registered transition"
+  const rcl_transition_map_t* map = &state_machine->transition_map;
+  for (unsigned int i=0; i<map->size; ++i)
+  {
+    for (unsigned int j=0; j<map->transition_arrays[i].size; ++j)
+    {
+      if (strcmp(map->transition_arrays[i].transitions[j].transition_state.label, transition_state_label) == 0)
+      {
+        return &map->transition_arrays[i].transitions[j];
+      }
+    }
+  }
+  return NULL;
 }
 
 void
 rcl_register_callback(rcl_state_machine_t* state_machine, unsigned int state_index, unsigned int transition_index, bool(*fcn)(void))
 {
-  rcl_transition_array_t* all_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, state_index);
-
-  for (unsigned int i=0; i<all_transitions->size; ++i)
+  rcl_transition_array_t* transitions = rcl_get_transitions_by_index(&state_machine->transition_map, state_index);
+  for (unsigned int i=0; i<transitions->size; ++i)
   {
-    if (all_transitions->transitions[i].transition_index.index == transition_index)
+    if (transitions->transitions[i].transition_state.index == transition_index)
     {
-      all_transitions->transitions[i].callback = fcn;
+      transitions->transitions[i].callback = fcn;
     }
   }
 }
@@ -56,18 +93,19 @@ rcl_register_callback(rcl_state_machine_t* state_machine, unsigned int state_ind
 bool
 rcl_invoke_transition(rcl_state_machine_t* state_machine, rcl_state_t transition_index)
 {
-  rcl_transition_array_t* all_transitions = rcl_get_transitions_by_index(&state_machine->transition_map, state_machine->current_state->index);
+  unsigned int current_index = state_machine->current_state->index;
+  rcl_transition_array_t* transitions = rcl_get_transitions_by_index(&state_machine->transition_map, current_index);
 
-  if (!all_transitions)
+  if (transitions == NULL)
   {
     return false;
   }
   bool success = false;
-  for (unsigned int i=0; i<all_transitions->size; ++i)
+  for (unsigned int i=0; i<transitions->size; ++i)
   {
-    if (all_transitions->transitions[i].transition_index.index == transition_index.index)
+    if (transitions->transitions[i].transition_state.index == transition_index.index)
     {
-      ((bool(*)(void))all_transitions->transitions[i].callback)();
+      ((bool(*)(void))transitions->transitions[i].callback)();
       success = true;
       // break here ?! would allow only one to one transitions
     }