Writing a Simple FSM (C++)

This tutorial shows how to create a simple Finite State Machine (FSM) using YASMIN in C++. We will create two custom states, FooState and BarState, and transition between them. This example demonstrates the fundamental concepts of state machines: defining states with specific behaviors, managing state transitions, and sharing data between states using the blackboard.

1. Create the FooState

First, we define the FooState class inheriting from yasmin::State. This state will increment a counter and switch outcomes based on the counter value. The constructor initializes two possible outcomes: "outcome1" (to continue to BarState) and "outcome2" (to finish execution). In the execute() method, we check if the counter is less than 3 - if so, we increment it, store a message in the blackboard, and return "outcome1" to transition to BarState. After 3 executions, we return "outcome2" to end the state machine.

/**
 * @brief Represents the "Foo" state in the state machine.
 *
 * This state increments a counter each time it is executed and
 * communicates the current count via the blackboard.
 */
class FooState : public yasmin::State {
public:
  /// Counter to track the number of executions.
  int counter;

  /**
   * @brief Constructs a FooState object, initializing the counter.
   */
  FooState() : yasmin::State({"outcome1", "outcome2"}), counter(0){};

  /**
   * @brief Executes the Foo state logic.
   *
   * This method logs the execution, waits for 3 seconds,
   * increments the counter, and sets a string in the blackboard.
   * The state will transition to either "outcome1" or "outcome2"
   * based on the current value of the counter.
   *
   * @param blackboard Shared pointer to the blackboard for state communication.
   * @return std::string The outcome of the execution: "outcome1" or "outcome2".
   */
  std::string
  execute(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) override {
    YASMIN_LOG_INFO("Executing state FOO");
    std::this_thread::sleep_for(std::chrono::seconds(3));

    if (this->counter < 3) {
      this->counter += 1;
      blackboard->set<std::string>("foo_str",
                                   "Counter: " + std::to_string(this->counter));
      return "outcome1";

    } else {
      return "outcome2";
    }
  };
};

2. Create the BarState

Next, we define the BarState, which acts as an intermediary state in our FSM. This state has only one outcome: "outcome3", which will transition back to FooState. In the execute() method, we retrieve the string stored by FooState from the blackboard and log it. This demonstrates how states can communicate by reading and writing shared data through the blackboard. After logging the message, the state returns "outcome3" to loop back to FooState.

/**
 * @brief Represents the "Bar" state in the state machine.
 *
 * This state logs the value from the blackboard and provides
 * a single outcome to transition.
 */
class BarState : public yasmin::State {
public:
  /**
   * @brief Constructs a BarState object.
   */
  BarState() : yasmin::State({"outcome3"}) {}

  /**
   * @brief Executes the Bar state logic.
   *
   * This method logs the execution, waits for 3 seconds,
   * retrieves a string from the blackboard, and logs it.
   *
   * @param blackboard Shared pointer to the blackboard for state communication.
   * @return std::string The outcome of the execution: "outcome3".
   */
  std::string
  execute(std::shared_ptr<yasmin::blackboard::Blackboard> blackboard) override {
    YASMIN_LOG_INFO("Executing state BAR");
    std::this_thread::sleep_for(std::chrono::seconds(3));

    YASMIN_LOG_INFO(blackboard->get<std::string>("foo_str").c_str());

    return "outcome3";
  }
};

3. Main Function

In the main function, we initialize ROS 2 and set up the complete state machine. First, we call rclcpp::init() to initialize the ROS 2 context and configure the YASMIN loggers. Then we create a StateMachine with "outcome4" as the terminal outcome. Using add_state(), we register our FooState and BarState, defining their transitions: FooState can transition to BarState (outcome1) or end the machine (outcome2), while BarState always loops back to FooState (outcome3). The YasminViewerPub enables real-time visualization of the state machine execution. Finally, we execute the state machine and handle any exceptions before shutting down ROS 2.

int main(int argc, char *argv[]) {
  YASMIN_LOG_INFO("yasmin_demo");
  rclcpp::init(argc, argv);

  // Set ROS 2 logs
  yasmin_ros::set_ros_loggers();

  // Create a state machine
  auto sm = std::make_shared<yasmin::StateMachine>(
      std::initializer_list<std::string>{"outcome4"});

  // Cancel state machine on ROS 2 shutdown
  rclcpp::on_shutdown([sm]() {
    if (sm->is_running()) {
      sm->cancel_state();
    }
  });

  // Add states to the state machine
  sm->add_state("FOO", std::make_shared<FooState>(),
                {
                    {"outcome1", "BAR"},
                    {"outcome2", "outcome4"},
                });
  sm->add_state("BAR", std::make_shared<BarState>(),
                {
                    {"outcome3", "FOO"},
                });

  // Publish state machine updates
  yasmin_viewer::YasminViewerPub yasmin_pub(sm, "YASMIN_DEMO");

  // Execute the state machine
  try {
    std::string outcome = (*sm.get())();
    YASMIN_LOG_INFO(outcome.c_str());
  } catch (const std::exception &e) {
    YASMIN_LOG_WARN(e.what());
  }

  rclcpp::shutdown();
  return 0;
}

4. Build and Run

Compile your ROS 2 package using colcon build and execute the demonstration to see the state machine in action, cycling through the FOO and BAR states.

ros2 run yasmin_demos yasmin_demo