---
title: "refactoring a state machine"
description: "Improving the design and performance of a complex state machine used in autonomous racing systems"
date: "05/06/2025"
useKatex: true
---

Given the recent switch to the [Marelli flag system](https://www.racecar-engineering.com/articles/wrc-electronic-yellow-flag-system/) in autonomous races, the [CAR](https://autonomousracing.dev/) flag state machine needed to be expanded and refactored.

# the problem

Our original state machine implementation had grown organically over several racing seasons. What started as a simple finite state machine under a more trivial flag system grew vastly more complex as it maladapted to the new flag system. Specifically, the original flag system only had one flag system for the entire track and a few car-specific ones (i.e. request to retire the car after a rules violation). The Marelli flag system has both flag *and* vehicle flags, each of which enforces different requirements.

The state machine worked "fine" for its initial use case:

1. States were read in through a declarative `state_machine.yaml` in which states had a few entry/exit conditions.

```yaml
State_Machine:
  init_state: "Red Track"
  states:
    red_track:
      id: "Red Track"
      entry_condition: "track_flag == 3"
      state_actions:
        desired_velocity: "velocity=0"
      next_states: ["Overtake", "Switch to Pits", ...]
```

2. Subsequently, the code was read in and parsed with a [cpp yaml library](https://github.com/jbeder/yaml-cpp).

3. Since all of this was done at runtime, `state_actions`, `entry_condition`, and `next_states` were *manually parsed*, hard-coded in a sort of custom language encoded in a string. Opening up our codebase, I found this set of foundational structs that the data was converted into:

```cpp
struct Action {
  std::string func;
  std::vector<std::string> param_types;
  std::vector<std::variant<int, double, std::string>> params;
};

struct Expression {
  std::string var, func, value;
};

struct Condition {
  int left, right;
  std::string logic_op = "";
};
```

A state action could be anything from a variable assignment to a function call, all of which had variable syntax and parsing logic by extension. One method had the following signature:

```cpp
std::unordered_map<std::string, std::vector<std::variant<int, double>>> StateMachine::check_state_change;
```

It was time for a change.

# the refactor

## testing

As I was unaware of the internals of every possible state transition in the machine, I simply coded up an exhaustive test across all transitions looking something like below. Invalid/impossible state transitions according to the Marelli flag system are omitted for simplicity.

```cpp
state_machine_->override_state(current_state);

for (int tf : track_flags) {
    for (int vf : vehicle_flags) {
        for (int pl : pit_locations.at(current_state)) {
            for (double vs : vehicle_speeds.at(current_state)) {
                if (state_machine_->get_current_state() != expected_state) {
                    FAIL() << "incorrect state change\n";
                ...
```

This was integrated into our CI via `CMake` + `colcon test`, giving (near) certainty that any state machine passing the test was exactly what we needed.

## design considerations

I knew state machines were ubiquitous in software so I explored a few previous approaches:

1. One big switch: nested conditions (i.e. all combinations of vehicle + track flags) seemed a bit nightmarish to look at as we continue to build out the state machine

2. [Boost SMS](https://www.boost.org/library/latest/msm/): this and other existing libraries looked enticing but three things stopped me:
    1. Integrating dependencies into our production code is simple but takes time for other members of the team to vet it
    2. It also bloats our deployment latency and final size because:
    3. It is overkill and writing things from scratch is more fun

3. A minimalist and expressive approach:

## final design: templates and bitmasks

Considering each element of the state machine, I simplified it into the following components:

- State: uniquely encode vehicle and track flags
- Transitions: perform arbitrary side effects depending on state
- Conditions: limit state transitions according to entry and exit states

### struct design

Since we only have $N\ll 32$ vehicle flags and $M\ll 32$ track flags, all of this information (all of the state flags) can be encoded in a `uint32_t`:

![state-encoding](/refactoring-a-state-machine/state-encoding.webp)

The state must also store a few other relevant details:

```cpp
using VehicleTrackFlag = uint32_t;

struct StateInfo {
    VehicleTrackFlag flags;
    uint8_t     pit_location;
    bool        pit_stop;
    double      speed;
    ...
};

enum class StateID { State1, State2, ..., Last };
```

A transition has entry and exit conditions which are compositions of state (i.e. just our bitmask) and an arbitrary action (i.e. a lambda). The entry/exit bits must be set/unset in order to enter/exit the state.


```cpp
struct Transition {
  VehicleTrackFlags entry;
  VehicleTrackFlags exit;
  std::function<void(State&)> action{};
  StateID to;
};
```

### the event loop

Now that some of the low-level details are resolved, I considered the design top-down. I wanted the main loop to simply iterate through all connected states and check if they could be entered:

```cpp
void tick(State& state, StateInfo& state_id) {
    auto mask = state.flags;
    for (auto&& prospective_state : state_table[current]) {
        bool enter = (mask & prospective_state.entry) == prospective_state.entry;
        bool exit  = (mask & prospective_state.exit ) == 0;
        if (enter && exit) {
            prospective_state.action(state);
            state_id = prospective_state.to;
            break;
        }
    }
}
```

The `state_table` would need to be a "master lookup" mapping a state id (enum index) to several `Transition`s[^1]:

```cpp
constexpr std::array<std::span<const Transition>,
                     static_cast<std::size_t>(StateID::Last)>
state_table {
    red_track_flags_transitions,
    switch_to_pits_transitions,
    ...
};
```

Finally, the collection of transitions from some state to another are declaratively defined. I inlined each `Transition` with [anonymous struct syntax](https://en.cppreference.com/w/c/language/struct_initialization.html) for the purposes of brevity:

```cpp
constexpr Transition red_track_flag_transitions[] = {
    // entry  exit                action       destination
    ...
    // example: stay in the same state
    {  0,     StateID::RedTrack,  null_action, make_vtf(TrackFlag::Red) }
};
```

## final considerations

The final state machine is not bad:

- Expressive and (relatively) succinct considering the massive amounts of logic encoded
- Easily extendible
- Memory efficient (no heap allocations, compressed state)
- Quick: the [tight event loop](https://www.wikiwand.com/en/dictionary/tight_loop) is fast, especially given the outdegree of any state machine node is $\leq5$

but it of course has some downsides:

- Inflexible for new logic: entry/exit conditions are mere bits. More comprehensive conditions (i.e. "perform at least 3 laps before entering this state") would require a refactor with lambdas
- Harder to read than the YAML custom language (we're not going back to that)

A large improvement if you ask me.

[^1]: Why a [std::span](https://www.cppreference.com/w/cpp/container/span.html)? Here, the number of transitions for each array is known at compile-time. A `std::vector` would heap-allocate the structs and a `std::array` would be difficult given that the `Transition` arrays are each of varying sizes. In contrast, one can extend this state machine by adding an entry to the array and nothing more—the `std::span` performs the magic.
