Path Planning - FSM

fsm.cpp

enum LongitudinalState
{
    ACCELERATE = 0,
    DECELERATE = 1,
    MAINTAIN_COURSE = 2,
    STOP = 3
};
enum LateralState
{
    STAY_IN_LANE = 0,
    PREPARE_CHANGE_LANE_LEFT = 1,
    PREPARE_CHANGE_LANE_RIGHT = 2,
    CHANGE_LANE_LEFT = 3,
    CHANGE_LANE_RIGHT = 4
};

vector<State> StateMachine::nextPossibleStates()
{
    vector<State> future_states;
    switch (this->current_state.d_state)
    {
    case LateralState::STAY_IN_LANE:
        future_states.push_back(State(LongitudinalState::MAINTAIN_COURSE,
                                      LateralState::STAY_IN_LANE,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane));
        future_states.push_back(State(LongitudinalState::ACCELERATE,
                                      LateralState::STAY_IN_LANE,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane));
        future_states.push_back(State(LongitudinalState::DECELERATE,
                                      LateralState::STAY_IN_LANE,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane));        

        future_states.push_back(State(LongitudinalState::MAINTAIN_COURSE,
                                      LateralState::PREPARE_CHANGE_LANE_LEFT,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane - 1));
        future_states.push_back(State(LongitudinalState::ACCELERATE,
                                      LateralState::PREPARE_CHANGE_LANE_LEFT,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane - 1));
        future_states.push_back(State(LongitudinalState::DECELERATE,
                                      LateralState::PREPARE_CHANGE_LANE_LEFT,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane - 1));

        future_states.push_back(State(LongitudinalState::MAINTAIN_COURSE,
                                      LateralState::PREPARE_CHANGE_LANE_RIGHT,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane + 1));
        future_states.push_back(State(LongitudinalState::ACCELERATE,
                                      LateralState::PREPARE_CHANGE_LANE_RIGHT,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane + 1));
        future_states.push_back(State(LongitudinalState::DECELERATE,
                                      LateralState::PREPARE_CHANGE_LANE_RIGHT,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane + 1));

        break;
    case LateralState::PREPARE_CHANGE_LANE_LEFT:
        future_states.push_back(State(LongitudinalState::MAINTAIN_COURSE,
                                      LateralState::CHANGE_LANE_LEFT,
                                      this->current_state.future_lane,
                                      this->current_state.future_lane));
        future_states.push_back(State(LongitudinalState::MAINTAIN_COURSE,
                                      LateralState::STAY_IN_LANE,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane));
        future_states.push_back(State(LongitudinalState::DECELERATE,
                                      LateralState::STAY_IN_LANE,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane));
        break;

    case LateralState::PREPARE_CHANGE_LANE_RIGHT:
        future_states.push_back(State(LongitudinalState::MAINTAIN_COURSE,
                                      LateralState::CHANGE_LANE_RIGHT,
                                      this->current_state.future_lane,
                                      this->current_state.future_lane));
        future_states.push_back(State(LongitudinalState::MAINTAIN_COURSE,
                                      LateralState::STAY_IN_LANE,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane));
        future_states.push_back(State(LongitudinalState::DECELERATE,
                                      LateralState::STAY_IN_LANE,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane));
        break;

    default:       
        future_states.push_back(State(LongitudinalState::MAINTAIN_COURSE,
                                      LateralState::STAY_IN_LANE,
                                      this->current_state.current_lane,
                                      this->current_state.current_lane));       
    }
    return future_states;
}