Refactor goal state machine implementation and add unit tests (#311)

* Fix buggy if-conditions in transition functions.
* Bugfix: incease number of states by one
* Cleanup CMakeLists.txt and package.xml
* Move goal state machine implementation details from header to C file

rcl_action/CMakeLists.txt

@@ -6,14 +6,11 @@ find_package(ament_cmake_ros REQUIRED)
 
 find_package(action_msgs REQUIRED)
 find_package(rcl REQUIRED)
-find_package(rcutils REQUIRED)
-# find_package(rmw REQUIRED)
 
 include_directories(
   include
   ${action_msgs_INCLUDE_DIRS}
   ${rcl_INCLUDE_DIRS}
-  ${rcutils_INCLUDE_DIRS}
 )
 
 # Default to C11
@@ -34,77 +31,63 @@ add_executable(test_compile_headers
   src/test_compile_headers.c
 )
 
-# set(rcl_action_sources
+set(rcl_action_sources
-#   src/action_server.c
+  src/${PROJECT_NAME}/goal_state_machine.c
-#   src/action_client.c
+)
-#   src/action_goal.c
-#   src/transition_map.c
-# )
-# set_source_files_properties(
-#   ${rcl_action_sources}
-#   PROPERTIES language "C"
-# )
 
-### C-Library depending only on RCL
+set_source_files_properties(
-# add_library(
+  ${rcl_action_sources}
-#   ${PROJECT_NAME}
+  PROPERTIES language "C"
-#   ${rcl_action_sources}
+)
-# )
 
-# specific order: dependents before dependencies
+add_library(
-# ament_target_dependencies(${PROJECT_NAME}
+  ${PROJECT_NAME}
-#   "rcl"
+  ${rcl_action_sources}
-#   "action_msgs"
+)
-#   "rosidl_generator_c"
+
-#   "rcutils"
+ament_target_dependencies(${PROJECT_NAME}
-# )
+  "rcl"
+  "action_msgs"
+)
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
-# target_compile_definitions(${PROJECT_NAME} PRIVATE "RCL_ACTION_BUILDING_DLL")
+target_compile_definitions(${PROJECT_NAME} PRIVATE "RCL_ACTION_BUILDING_DLL")
-
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION lib
-#   LIBRARY DESTINATION lib
-#   RUNTIME DESTINATION bin
-# )
-
-if(BUILD_TESTING)
-  find_package(ament_cmake_gtest REQUIRED)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-
-  # Gtests
-  # ament_add_gtest(test_action_server
-  #   test/test_action_server.cpp
-  # )
-  # if(TARGET test_action_server)
-  #   target_include_directories(test_action_server PUBLIC
-  #     ${rcl_INCLUDE_DIRS}
-  #   )
-  #   target_link_libraries(test_action_server ${PROJECT_NAME})
-  # endif()
-  # ament_add_gtest(test_action_client
-  #   test/test_action_client.cpp
-  # )
-  # if(TARGET test_action_client)
-  #   target_include_directories(test_action_client PUBLIC
-  #     ${rcl_INCLUDE_DIRS}
-  #   )
-  #   target_link_libraries(test_action_client ${PROJECT_NAME})
-  # endif()
-endif()
-
-# specific order: dependents before dependencies
-ament_export_include_directories(include)
-# ament_export_libraries(${PROJECT_NAME})
-ament_export_dependencies(ament_cmake)
-ament_export_dependencies(rcl)
-# ament_export_dependencies(action_msgs)
-ament_export_dependencies(rcutils)
-ament_package()
 
 install(
   DIRECTORY include/
   DESTINATION include
 )
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  ament_find_gtest()
+  # Gtests
+  ament_add_gtest(test_goal_state_machine
+    test/rcl_action/test_goal_state_machine.cpp
+  )
+  if(TARGET test_goal_state_machine)
+    target_include_directories(test_goal_state_machine PUBLIC
+      ${rcl_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_goal_state_machine
+      ${PROJECT_NAME}
+    )
+  endif()
+endif()
+
+# specific order: dependents before dependencies
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(ament_cmake)
+ament_export_dependencies(rcl)
+ament_export_dependencies(action_msgs)
+ament_package()

rcl_action/include/rcl_action/goal_state_machine.h

@@ -24,83 +24,6 @@ extern "C"
 #include "rcl_action/visibility_control.h"
 
 
-typedef rcl_action_goal_state_t
-(* rcl_action_goal_event_handler)(rcl_action_goal_state_t, rcl_action_goal_event_t);
-
-// Transition event handlers
-static inline rcl_action_goal_state_t
-_execute_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
-{
-  if (GOAL_STATE_ACCEPTED != state || GOAL_EVENT_EXECUTE != event) {
-    return GOAL_STATE_UNKNOWN;
-  }
-  return GOAL_STATE_EXECUTING;
-}
-
-static inline rcl_action_goal_state_t
-_cancel_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
-{
-  if (GOAL_STATE_ACCEPTED != state ||
-    GOAL_STATE_EXECUTING != state ||
-    GOAL_EVENT_CANCEL != event)
-  {
-    return GOAL_STATE_UNKNOWN;
-  }
-  return GOAL_STATE_CANCELING;
-}
-
-static inline rcl_action_goal_state_t
-_set_succeeded_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
-{
-  if (GOAL_STATE_EXECUTING != state ||
-    GOAL_STATE_CANCELING != state ||
-    GOAL_EVENT_SET_SUCCEEDED != event)
-  {
-    return GOAL_STATE_UNKNOWN;
-  }
-  return GOAL_STATE_SUCCEEDED;
-}
-
-static inline rcl_action_goal_state_t
-_set_aborted_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
-{
-  if (GOAL_STATE_EXECUTING != state ||
-    GOAL_STATE_CANCELING != state ||
-    GOAL_EVENT_SET_ABORTED != event)
-  {
-    return GOAL_STATE_UNKNOWN;
-  }
-  return GOAL_STATE_ABORTED;
-}
-
-static inline rcl_action_goal_state_t
-_set_canceled_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
-{
-  if (GOAL_STATE_CANCELING != state || GOAL_EVENT_SET_CANCELED != event) {
-    return GOAL_STATE_UNKNOWN;
-  }
-  return GOAL_STATE_CANCELED;
-}
-
-// Transition map
-rcl_action_goal_event_handler
-  _goal_state_transition_map[GOAL_STATE_NUM_STATES][GOAL_EVENT_NUM_EVENTS] = {
-  [GOAL_STATE_ACCEPTED] = {
-    [GOAL_EVENT_EXECUTE] = _execute_event_handler,
-    [GOAL_EVENT_CANCEL] = _cancel_event_handler,
-  },
-  [GOAL_STATE_EXECUTING] = {
-    [GOAL_EVENT_CANCEL] = _cancel_event_handler,
-    [GOAL_EVENT_SET_SUCCEEDED] = _set_succeeded_event_handler,
-    [GOAL_EVENT_SET_ABORTED] = _set_aborted_event_handler,
-  },
-  [GOAL_STATE_CANCELING] = {
-    [GOAL_EVENT_SET_SUCCEEDED] = _set_succeeded_event_handler,
-    [GOAL_EVENT_SET_ABORTED] = _set_aborted_event_handler,
-    [GOAL_EVENT_SET_CANCELED] = _set_canceled_event_handler,
-  },
-};
-
 /// Transition a goal from one state to the next.
 /**
  * Given a goal state and a goal event, return the next state.
@@ -108,22 +31,14 @@ rcl_action_goal_event_handler
  * \param[in] state the state to transition from
  * \param[in] event the event triggering a transition
  * \return the next goal state if the transition is valid, or
- * \return `GOAl_STATE_UNKNOWN` if the transition is invalid or an error occured
+ * \return `GOAL_STATE_UNKNOWN` if the transition is invalid or an error occured
  */
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
-inline rcl_action_goal_state_t
+rcl_action_goal_state_t
 rcl_action_transition_goal_state(
   const rcl_action_goal_state_t state,
-  const rcl_action_goal_event_t event)
+  const rcl_action_goal_event_t event);
-{
-  // rcl_action_goal_event_handler ** transition_map = get_state_transition_map();
-  rcl_action_goal_event_handler handler = _goal_state_transition_map[state][event];
-  if (NULL == handler) {
-    return GOAL_STATE_UNKNOWN;
-  }
-  return handler(state, event);
-}
 
 #ifdef __cplusplus
 }

rcl_action/include/rcl_action/types.h

@@ -72,7 +72,7 @@ typedef int8_t rcl_action_goal_state_t;
 #define GOAL_STATE_SUCCEEDED action_msgs__msg__GoalStatus__STATUS_SUCCEEDED
 #define GOAL_STATE_CANCELED action_msgs__msg__GoalStatus__STATUS_CANCELED
 #define GOAL_STATE_ABORTED action_msgs__msg__GoalStatus__STATUS_ABORTED
-#define GOAL_STATE_NUM_STATES 6  // not counting `UNKNOWN`
+#define GOAL_STATE_NUM_STATES 7
 
 /// Goal state transition events
 typedef enum rcl_action_goal_event_t

rcl_action/package.xml

@@ -11,15 +11,9 @@
 
   <build_depend>action_msgs</build_depend>
   <build_depend>rcl</build_depend>
-  <build_depend>rcutils</build_depend>
-  <build_depend>rmw_implementation</build_depend>
-  <build_depend>rosidl_generator_c</build_depend>
 
   <exec_depend>action_msgs</exec_depend>
   <exec_depend>rcl</exec_depend>
-  <exec_depend>rcutils</exec_depend>
-  <exec_depend>rmw_implementation</exec_depend>
-  <exec_depend>rosidl_generator_c</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>

rcl_action/src/rcl_action/goal_state_machine.c

@@ -0,0 +1,111 @@
+// Copyright 2018 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.
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include "rcl_action/goal_state_machine.h"
+
+
+typedef rcl_action_goal_state_t
+(* rcl_action_goal_event_handler)(rcl_action_goal_state_t, rcl_action_goal_event_t);
+
+// Transition event handlers
+rcl_action_goal_state_t
+_execute_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
+{
+  if (GOAL_STATE_ACCEPTED != state || GOAL_EVENT_EXECUTE != event) {
+    return GOAL_STATE_UNKNOWN;
+  }
+  return GOAL_STATE_EXECUTING;
+}
+
+rcl_action_goal_state_t
+_cancel_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
+{
+  if ((GOAL_STATE_ACCEPTED != state && GOAL_STATE_EXECUTING != state) ||
+    GOAL_EVENT_CANCEL != event)
+  {
+    return GOAL_STATE_UNKNOWN;
+  }
+  return GOAL_STATE_CANCELING;
+}
+
+rcl_action_goal_state_t
+_set_succeeded_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
+{
+  if ((GOAL_STATE_EXECUTING != state && GOAL_STATE_CANCELING != state) ||
+    GOAL_EVENT_SET_SUCCEEDED != event)
+  {
+    return GOAL_STATE_UNKNOWN;
+  }
+  return GOAL_STATE_SUCCEEDED;
+}
+
+rcl_action_goal_state_t
+_set_aborted_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
+{
+  if ((GOAL_STATE_EXECUTING != state && GOAL_STATE_CANCELING != state) ||
+    GOAL_EVENT_SET_ABORTED != event)
+  {
+    return GOAL_STATE_UNKNOWN;
+  }
+  return GOAL_STATE_ABORTED;
+}
+
+rcl_action_goal_state_t
+_set_canceled_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event)
+{
+  if (GOAL_STATE_CANCELING != state || GOAL_EVENT_SET_CANCELED != event) {
+    return GOAL_STATE_UNKNOWN;
+  }
+  return GOAL_STATE_CANCELED;
+}
+
+// Transition map
+static rcl_action_goal_event_handler
+  _goal_state_transition_map[GOAL_STATE_NUM_STATES][GOAL_EVENT_NUM_EVENTS] = {
+  [GOAL_STATE_ACCEPTED] = {
+    [GOAL_EVENT_EXECUTE] = _execute_event_handler,
+    [GOAL_EVENT_CANCEL] = _cancel_event_handler,
+  },
+  [GOAL_STATE_EXECUTING] = {
+    [GOAL_EVENT_CANCEL] = _cancel_event_handler,
+    [GOAL_EVENT_SET_SUCCEEDED] = _set_succeeded_event_handler,
+    [GOAL_EVENT_SET_ABORTED] = _set_aborted_event_handler,
+  },
+  [GOAL_STATE_CANCELING] = {
+    [GOAL_EVENT_SET_SUCCEEDED] = _set_succeeded_event_handler,
+    [GOAL_EVENT_SET_ABORTED] = _set_aborted_event_handler,
+    [GOAL_EVENT_SET_CANCELED] = _set_canceled_event_handler,
+  },
+};
+
+rcl_action_goal_state_t
+rcl_action_transition_goal_state(
+  const rcl_action_goal_state_t state,
+  const rcl_action_goal_event_t event)
+{
+  rcl_action_goal_event_handler handler = _goal_state_transition_map[state][event];
+  if (NULL == handler) {
+    return GOAL_STATE_UNKNOWN;
+  }
+  return handler(state, event);
+}
+
+#ifdef __cplusplus
+}
+#endif

rcl_action/test/rcl_action/test_goal_state_machine.cpp

@@ -0,0 +1,152 @@
+// Copyright 2018 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 "rcl_action/goal_state_machine.h"
+
+
+TEST(TestGoalStateMachine, test_valid_transitions)
+{
+  rcl_action_goal_state_t state = rcl_action_transition_goal_state(
+    GOAL_STATE_ACCEPTED,
+    GOAL_EVENT_EXECUTE);
+  EXPECT_EQ(GOAL_STATE_EXECUTING, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_ACCEPTED,
+    GOAL_EVENT_CANCEL);
+  EXPECT_EQ(GOAL_STATE_CANCELING, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_EXECUTING,
+    GOAL_EVENT_CANCEL);
+  EXPECT_EQ(GOAL_STATE_CANCELING, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_EXECUTING,
+    GOAL_EVENT_SET_SUCCEEDED);
+  EXPECT_EQ(GOAL_STATE_SUCCEEDED, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_EXECUTING,
+    GOAL_EVENT_SET_ABORTED);
+  EXPECT_EQ(GOAL_STATE_ABORTED, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_CANCELING,
+    GOAL_EVENT_SET_CANCELED);
+  EXPECT_EQ(GOAL_STATE_CANCELED, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_CANCELING,
+    GOAL_EVENT_SET_ABORTED);
+  EXPECT_EQ(GOAL_STATE_ABORTED, state);
+}
+
+TEST(TestGoalStateMachine, test_invalid_transitions)
+{
+  // Invalid from ACCEPTED
+  rcl_action_goal_state_t state = rcl_action_transition_goal_state(
+    GOAL_STATE_ACCEPTED,
+    GOAL_EVENT_SET_SUCCEEDED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_ACCEPTED,
+    GOAL_EVENT_SET_ABORTED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_ACCEPTED,
+    GOAL_EVENT_SET_CANCELED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+
+  // Invalid from EXECUTING
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_EXECUTING,
+    GOAL_EVENT_EXECUTE);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_EXECUTING,
+    GOAL_EVENT_SET_CANCELED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+
+  // Invalid from CANCELING
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_CANCELING,
+    GOAL_EVENT_EXECUTE);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_CANCELING,
+    GOAL_EVENT_CANCEL);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+
+  // Invalid from SUCCEEDED
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_SUCCEEDED,
+    GOAL_EVENT_EXECUTE);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_SUCCEEDED,
+    GOAL_EVENT_CANCEL);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_SUCCEEDED,
+    GOAL_EVENT_SET_SUCCEEDED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_SUCCEEDED,
+    GOAL_EVENT_SET_ABORTED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_SUCCEEDED,
+    GOAL_EVENT_SET_CANCELED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+
+  // Invalid from ABORTED
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_ABORTED,
+    GOAL_EVENT_EXECUTE);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_ABORTED,
+    GOAL_EVENT_CANCEL);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_ABORTED,
+    GOAL_EVENT_SET_SUCCEEDED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_ABORTED,
+    GOAL_EVENT_SET_ABORTED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_ABORTED,
+    GOAL_EVENT_SET_CANCELED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+
+  // Invalid from CANCELED
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_CANCELED,
+    GOAL_EVENT_EXECUTE);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_CANCELED,
+    GOAL_EVENT_CANCEL);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_CANCELED,
+    GOAL_EVENT_SET_SUCCEEDED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_CANCELED,
+    GOAL_EVENT_SET_ABORTED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+  state = rcl_action_transition_goal_state(
+    GOAL_STATE_CANCELED,
+    GOAL_EVENT_SET_CANCELED);
+  EXPECT_EQ(GOAL_STATE_UNKNOWN, state);
+}