Finite State Machine library

.travis.yml

@@ -71,7 +71,8 @@ script:
   # Check for clang format
   - cd src/avoidance
   - git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
-  - ./check_code_format.sh
+  - (cd tools && ./check_code_format.sh)
+  - (cd tools && ./check_state_machine_diagrams.sh)
   - catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False -DDISABLE_SIMULATION=ON && catkin build
   - catkin config --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_ENABLE_TESTING=True && catkin build avoidance local_planner global_planner safe_landing_planner --no-deps -v -i --catkin-make-args tests
   - status=0 && for f in ~/catkin_ws/devel/lib/*/*-test; do valgrind --leak-check=full --track-origins=yes --error-exitcode=1 $f || status=1; done

avoidance/CMakeLists.txt

@@ -86,6 +86,7 @@ if(CATKIN_ENABLE_TESTING)
     # Add gtest based cpp test target and link libraries
     catkin_add_gtest(${PROJECT_NAME}-test test/main.cpp
                                           test/test_common.cpp
+                                          test/test_usm.cpp
                     )
 
     if(TARGET ${PROJECT_NAME}-test)

avoidance/include/avoidance/usm.h

@@ -0,0 +1,99 @@
+#pragma once
+
+/*
+* Copyright (c) 2019 Julian Kent. All rights reserved.
+*
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* * Redistributions of source code must retain the above copyright notice, this
+*   list of conditions and the following disclaimer.
+*
+* * Redistributions in binary form must reproduce the above copyright notice,
+*   this list of conditions and the following disclaimer in the documentation
+*   and/or other materials provided with the distribution.
+*
+* * Neither the name of libgnc nor the names of its
+*   contributors may be used to endorse or promote products derived from
+*   this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*
+*
+*
+* For usage examples, look at the tests.
+*/
+
+namespace usm {
+
+enum class Transition { REPEAT, NEXT1, NEXT2, NEXT3, NEXT4, ERROR };
+
+template <typename StateEnum>
+class StateMachine {
+ public:
+  StateMachine(StateEnum startingState);
+  void iterateOnce();
+
+  StateEnum getState();
+
+ protected:
+  virtual Transition runCurrentState(
+      StateEnum currentState) = 0;  // a big switch
+  virtual StateEnum chooseNextState(
+      StateEnum currentState, Transition transition) = 0;  // nested switches
+
+ private:
+  StateEnum m_currentState;
+};
+
+/*---------------IMPLEMENTATION------------------*/
+
+template <typename StateEnum>
+StateMachine<StateEnum>::StateMachine(StateEnum startingState)
+    : m_currentState(startingState) {}
+
+template <typename StateEnum>
+void StateMachine<StateEnum>::iterateOnce() {
+  Transition t = runCurrentState(m_currentState);
+  if (t != Transition::REPEAT)
+    m_currentState = chooseNextState(m_currentState, t);
+}
+
+template <typename StateEnum>
+StateEnum StateMachine<StateEnum>::getState() {
+  return m_currentState;
+}
+}
+
+/*---------------MACROS TO MAKE TRANSITION TABLES EASY------------------*/
+
+// clang-format off
+#define USM_TABLE(current_state, error, ...) \
+switch (current_state) { \
+    __VA_ARGS__; \
+    default: break; \
+} \
+return error
+
+#define USM_STATE(transition, start_state, ...) \
+    case start_state: \
+        switch (transition) { \
+            __VA_ARGS__; \
+            default: break; \
+        } \
+    break
+
+#define USM_MAP(transition, next_state) \
+            case transition: return next_state
+// clang-format on

avoidance/test/test_usm.cpp

@@ -0,0 +1,251 @@
+#include <gtest/gtest.h>
+
+#include <avoidance/usm.h>
+
+#include <iostream>
+
+using namespace usm;
+
+/*
+* Copyright (c) 2019 Julian Kent. All rights reserved.
+*
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* * Redistributions of source code must retain the above copyright notice, this
+*   list of conditions and the following disclaimer.
+*
+* * Redistributions in binary form must reproduce the above copyright notice,
+*   this list of conditions and the following disclaimer in the documentation
+*   and/or other materials provided with the distribution.
+*
+* * Neither the name of libgnc nor the names of its
+*   contributors may be used to endorse or promote products derived from
+*   this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*
+*
+*/
+
+/*
+ * Implement a basic state machine with 2 paths (A & B), and error state,
+ * and a transition table. Use this as an example for your use case!
+ */
+
+enum TestStates {
+  START,
+  PATH_A_1,
+  PATH_A_2,
+  END,
+  PATH_B_1,
+  PATH_B_2,
+  PATH_B_3,
+  CLEANUP
+};
+
+class TestStateMachine final : public StateMachine<TestStates> {
+ public:
+  TestStateMachine() : StateMachine<TestStates>(START) {}
+
+  bool path_a = true;
+  bool trigger_error_once = false;
+
+  bool start_called = false;
+  bool a1_called = false;
+  bool a2_called = false;
+  bool b1_called = false;
+  bool b2_called = false;
+  bool b3_called = false;
+  bool end_called = false;
+  bool cleanup_called = false;
+
+ protected:
+  Transition runCurrentState(TestStates currentState) override {
+    if (trigger_error_once) {
+      trigger_error_once = false;
+      return Transition::ERROR;
+    }
+
+    // clang-format off
+    switch(currentState) {
+        case START: return start();
+        case PATH_A_1: return a1();
+        case PATH_A_2: return a2();
+        case END: return end();
+        case PATH_B_1: return b1();
+        case PATH_B_2: return b2();
+        case PATH_B_3: return b3();
+        case CLEANUP: return cleanup();
+    }
+    // clang-format on
+  }
+
+  TestStates chooseNextState(TestStates currentState,
+                             Transition transition) override {
+    // clang-format off
+    USM_TABLE(currentState, CLEANUP,
+      USM_STATE(transition, START,    USM_MAP(Transition::NEXT1, PATH_A_1);
+                                      USM_MAP(Transition::NEXT2, PATH_B_1));
+      USM_STATE(transition, PATH_A_1, USM_MAP(Transition::NEXT1, PATH_A_2);
+                                      USM_MAP(Transition::ERROR, PATH_B_3));
+      USM_STATE(transition, PATH_A_2, USM_MAP(Transition::NEXT1, END));
+      USM_STATE(transition, PATH_B_1, USM_MAP(Transition::NEXT1, PATH_B_2));
+      USM_STATE(transition, PATH_B_2, USM_MAP(Transition::NEXT1, PATH_B_3));
+      USM_STATE(transition, PATH_B_3, USM_MAP(Transition::NEXT1, END));
+      USM_STATE(transition, CLEANUP,  USM_MAP(Transition::NEXT1, END))
+    );
+    // clang-format on
+  }
+
+ private:
+  Transition start() {
+    start_called = true;
+    return path_a ? Transition::NEXT1 : Transition::NEXT2;
+  }
+  Transition a1() {
+    a1_called = true;
+    return Transition::NEXT1;
+  }
+  Transition a2() {
+    a2_called = true;
+    return Transition::NEXT1;
+  }
+  Transition b1() {
+    b1_called = true;
+    return Transition::NEXT1;
+  }
+  Transition b2() {
+    b2_called = true;
+    return Transition::NEXT1;
+  }
+  Transition b3() {
+    b3_called = true;
+    return Transition::NEXT1;
+  }
+  Transition end() {
+    end_called = true;
+    return Transition::REPEAT;
+  }
+  Transition cleanup() {
+    cleanup_called = true;
+    return Transition::NEXT1;
+  }
+};
+
+TEST(StateMachine, runPathA) {
+  TestStateMachine m;
+  ASSERT_EQ(m.getState(), START);
+
+  ASSERT_FALSE(m.start_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.start_called);
+  ASSERT_EQ(m.getState(), PATH_A_1);
+
+  ASSERT_FALSE(m.a1_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.a1_called);
+  ASSERT_EQ(m.getState(), PATH_A_2);
+
+  ASSERT_FALSE(m.a2_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.a2_called);
+  ASSERT_EQ(m.getState(), END);
+
+  ASSERT_FALSE(m.end_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.end_called);
+}
+
+TEST(StateMachine, runPathB) {
+  TestStateMachine m;
+  m.path_a = false;
+  ASSERT_EQ(m.getState(), START);
+
+  ASSERT_FALSE(m.start_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.start_called);
+  ASSERT_EQ(m.getState(), PATH_B_1);
+
+  ASSERT_FALSE(m.b1_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.b1_called);
+  ASSERT_EQ(m.getState(), PATH_B_2);
+
+  ASSERT_FALSE(m.b2_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.b2_called);
+  ASSERT_EQ(m.getState(), PATH_B_3);
+
+  ASSERT_FALSE(m.b3_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.b3_called);
+  ASSERT_EQ(m.getState(), END);
+
+  ASSERT_FALSE(m.end_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.end_called);
+}
+
+TEST(StateMachine, enterDefaultError) {
+  TestStateMachine m;
+  ASSERT_EQ(m.getState(), START);
+
+  ASSERT_FALSE(m.start_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.start_called);
+  ASSERT_EQ(m.getState(), PATH_A_1);
+
+  ASSERT_FALSE(m.a1_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.a1_called);
+  ASSERT_EQ(m.getState(), PATH_A_2);
+
+  m.trigger_error_once = true;
+  m.iterateOnce();
+  ASSERT_EQ(m.getState(), CLEANUP);
+
+  ASSERT_FALSE(m.cleanup_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.cleanup_called);
+  ASSERT_EQ(m.getState(), END);
+
+  ASSERT_FALSE(m.end_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.end_called);
+}
+
+TEST(StateMachine, enterCustomError) {
+  TestStateMachine m;
+  ASSERT_EQ(m.getState(), START);
+
+  ASSERT_FALSE(m.start_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.start_called);
+  ASSERT_EQ(m.getState(), PATH_A_1);
+
+  m.trigger_error_once = true;
+  m.iterateOnce();
+  ASSERT_EQ(m.getState(), PATH_B_3);
+
+  ASSERT_FALSE(m.b3_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.b3_called);
+  ASSERT_EQ(m.getState(), END);
+
+  ASSERT_FALSE(m.end_called);
+  m.iterateOnce();
+  ASSERT_TRUE(m.end_called);
+  ASSERT_EQ(m.getState(), END);
+}

avoidance/test/test_usm.cpp.dot

@@ -0,0 +1,17 @@
+digraph {
+    "START" -> "PATH_A_1" [label="NEXT1", style="solid", weight=1]
+    "START" -> "PATH_B_1" [label="NEXT2", style="solid", weight=1]
+    "START" -> "CLEANUP" [label="ERROR", style="dotted", weight=0.1]
+    "PATH_A_1" -> "PATH_A_2" [label="NEXT1", style="solid", weight=1]
+    "PATH_A_1" -> "PATH_B_3" [label="ERROR", style="solid", weight=1]
+    "PATH_A_2" -> "END" [label="NEXT1", style="solid", weight=1]
+    "PATH_A_2" -> "CLEANUP" [label="ERROR", style="dotted", weight=0.1]
+    "PATH_B_1" -> "PATH_B_2" [label="NEXT1", style="solid", weight=1]
+    "PATH_B_1" -> "CLEANUP" [label="ERROR", style="dotted", weight=0.1]
+    "PATH_B_2" -> "PATH_B_3" [label="NEXT1", style="solid", weight=1]
+    "PATH_B_2" -> "CLEANUP" [label="ERROR", style="dotted", weight=0.1]
+    "PATH_B_3" -> "END" [label="NEXT1", style="solid", weight=1]
+    "PATH_B_3" -> "CLEANUP" [label="ERROR", style="dotted", weight=0.1]
+    "CLEANUP" -> "END" [label="NEXT1", style="solid", weight=1]
+    "CLEANUP" -> "CLEANUP" [label="ERROR", style="dotted", weight=0.1]
+}