Feature #24: Build state machine.

rise_motion_dev_ws/src/rise_motion/CMakeLists.txt

@@ -7,9 +7,16 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rise_motion_messages REQUIRED)
+
+include_directories(include)
+
+add_executable(rise_motion_main src/rise_motion_main.cpp src/node_fsm.cpp)
+target_link_libraries(rise_motion_main PUBLIC rclcpp::rclcpp ${rise_motion_messages_TARGETS})
+install(TARGETS
+  rise_motion_main
+  DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp

@@ -0,0 +1,24 @@
+#pragma once
+#include <map>
+#include <rclcpp/logger.hpp>
+#include <string>
+#include <utility>
+
+class StateMachine {
+public:
+  enum class State { Idle, Initialized, Operational, Error };
+  enum class Event { ToIdle, ToInitialized, ToOperational, ToError };
+
+  StateMachine();
+
+  State handle_event(Event e);
+  void set_state(State s);
+  State get_state();
+  static std::string state_to_string(State s);
+  static std::string event_to_string(Event e);
+
+private:
+  State state;
+  static std::map<std::pair<State, Event>, State> transition_table;
+  static rclcpp::Logger logger;
+};

rise_motion_dev_ws/src/rise_motion/package.xml

@@ -9,6 +9,9 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>rclcpp</depend>
+  <depend>rise_motion_messages</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp

@@ -0,0 +1,70 @@
+#include <map>
+#include <utility>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rise_motion/node_fsm.hpp>
+
+StateMachine::StateMachine() : state(State::Idle) {}
+
+StateMachine::State StateMachine::handle_event(Event e) {
+  std::pair<StateMachine::State, StateMachine::Event> keypair = {this->state,
+                                                                 e};
+  auto it = StateMachine::transition_table.find(keypair);
+  if (it != StateMachine::transition_table.end()) {
+    State old = state;
+    this->state = it->second;
+    RCLCPP_INFO(logger,
+                "Transition %s --(%s)--> %s", state_to_string(old).c_str(),
+                event_to_string(e).c_str(), state_to_string(state).c_str());
+    return it->second;
+  } else {
+    RCLCPP_WARN(logger,
+                "Invalid transition: (State: %s, Event: %s)",
+                state_to_string(state).c_str(), event_to_string(e).c_str());
+    return this->state;
+  }
+}
+
+void StateMachine::set_state(State s) { state = s; }
+
+StateMachine::State StateMachine::get_state() { return state; }
+
+std::string StateMachine::state_to_string(State s) {
+  switch (s) {
+  case State::Idle:
+    return "Idle";
+  case State::Initialized:
+    return "Initialized";
+  case State::Operational:
+    return "Operational";
+  case State::Error:
+    return "Error";
+  }
+  return "Unknown";
+}
+
+std::string StateMachine::event_to_string(Event e) {
+  switch (e) {
+  case Event::ToIdle:
+    return "ToIdle";
+  case Event::ToInitialized:
+    return "ToInitialized";
+  case Event::ToOperational:
+    return "ToOperational";
+  case Event::ToError:
+    return "ToError";
+  }
+  return "Unknown";
+}
+
+rclcpp::Logger StateMachine::logger = rclcpp::get_logger("StateMachine");
+
+std::map<std::pair<StateMachine::State, StateMachine::Event>,
+         StateMachine::State>
+    StateMachine::transition_table = {
+        {{State::Idle, Event::ToInitialized}, State::Initialized},
+        {{State::Idle, Event::ToError}, State::Initialized},
+        {{State::Initialized, Event::ToOperational}, State::Operational},
+        {{State::Initialized, Event::ToError}, State::Error},
+        {{State::Operational, Event::ToError}, State::Error},
+        {{State::Error, Event::ToIdle}, State::Idle}};

rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp

@@ -0,0 +1,60 @@
+#include <chrono>
+#include <memory>
+#include <rclcpp/executors.hpp>
+#include <rclcpp/node.hpp>
+#include <rise_motion/node_fsm.hpp>
+#include <rise_motion_messages/msg/state_current_msg.hpp>
+#include <string>
+
+class RiseMotionMain : public rclcpp::Node {
+public:
+  RiseMotionMain() : Node("rise_motion_main"), sm_() {
+    publisher_ =
+        this->create_publisher<rise_motion_messages::msg::StateCurrentMsg>(
+            "rise_motion_state_changes", 10);
+    timer_ = this->create_wall_timer(std::chrono::milliseconds(500),
+                                     [this]() -> void { this->on_time_out(); });
+  }
+
+private:
+  void on_time_out() {
+    auto message = rise_motion_messages::msg::StateCurrentMsg();
+    StateMachine::State previous_state = sm_.get_state();
+    StateMachine::State new_state;
+
+    switch (previous_state) {
+    case StateMachine::State::Idle:
+      new_state = sm_.handle_event(StateMachine::Event::ToInitialized);
+      break;
+    case StateMachine::State::Initialized:
+      new_state = sm_.handle_event(StateMachine::Event::ToOperational);
+      break;
+    case StateMachine::State::Operational:
+      new_state = sm_.handle_event(StateMachine::Event::ToError);
+      break;
+    case StateMachine::State::Error:
+    default:
+      new_state = sm_.handle_event(StateMachine::Event::ToIdle);
+      break;
+    }
+
+    if (sm_.get_state() != previous_state) {
+      message.previous_state = static_cast<uint8_t>(previous_state);
+      message.current_state = static_cast<uint8_t>(new_state);
+      sm_.set_state(new_state);
+      this->publisher_->publish(message);
+    }
+  }
+
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<rise_motion_messages::msg::StateCurrentMsg>::SharedPtr
+      publisher_;
+  StateMachine sm_;
+};
+
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<RiseMotionMain>());
+  rclcpp::shutdown();
+  return 0;
+}