Algorithm 1.

Trajectory Planner and Speed Profile Assignment

1: Receive the high level decision at

2: if at = 0 then

3:     Query for the waypoints from current lane: (s1, s2, s3, ...., sn)

4:     sCollision = sFrontCar − sEgo

5:     for waypoint si = s1, s2, s3, ..... do

6:         if sCollision > sn then

7:             vi = Query_Reference_Speed(si)

8:         else

9:             vi = Brake

10:         end if

11:     end for

12: else

13:         Check the availability of target lane (at = −1 is the left lane, at = 1 is the right lane)

14:     if Target Lane is not available then

15:         Return to Lane Keeping Mode

16:     else

17:         Get the end pose of lane changing send

18:         (s1, s2, s3, ...., sn) = QuinticPlanner(sego, send)

19:         for waypoint si = s1, s2, s3, ..... do

20:             vi = Query_Reference_Speed(si)

21:         end for

22:     end if

23: end if

24: Return (s1, v1, s2, v2, s3, v3, ....sn, vn)