fix(goal_planner): revert "refactor(goal_planner): rebuild state transition (#5371)" (#5399)

fix(goal_planner): revert "refactor(goal_planner): rebuild state transition (#5371)" (#5399) #1132