Skip to content

Instantly share code, notes, and snippets.

@kenshiro-o
Last active July 8, 2018 00:16
Show Gist options
  • Save kenshiro-o/f91574ffa01095d21af458f4deca762e to your computer and use it in GitHub Desktop.
Save kenshiro-o/f91574ffa01095d21af458f4deca762e to your computer and use it in GitHub Desktop.
Path Planning - FSM
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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment