Self-Driving Car Engineer Nanodegree Program

Sample video is here: report_video

Files are include in the submission:
    src/main.cpp
    src/cost.cpp
    src/cost.h
    src/vehcicle.cpp
    src/vehicle.h
    src/help.h

Path planning:
    Use the state machine to track states between "KL", "PLCL", "PLCR", "LCR", "LCL"
    Psudo code is as follows: (this is in main.cpp)
    create list of vehicle on lanes of same side of road
    check to see if there is any car in front and closer within 25 miles for 1 second time
    if yes, generate projectory.
    for each projector, calculate the cost: inefficiency, collision, goal to distance
    choose lowest trajectory. prepare change lane.
    state change to "PLCL", or "PLCR" dependes on the trajectory.
    in Prepare change lane state, update the "goal lane" and then change state to "LCL" or "LCR"
    In "LCL" or "LCR", calculate the new path and determine if lane change to target lane.

    for every vehicle on the same side of road, calculate the next positions apart every 0.02 second.
    that says, there are 50 vehicles for one car predicted postistion. these vehicles are use to calculate if there is collison when possible lane change.

    The alrogithm to calculate the next 50 points is using the spline alrogithm, the ego vehicle state determine to use the current lane or target lane to change.

    * inefficiency cost * : calculate the lane change efficiency as follows:
    cost = (2.0vehicle.target_speed - proposed_speed_intended - proposed_speed_final)/vehicle.target_speed;
    * goal_distance_cost * : calculate the cost to goal distance as follows:
    1 - 2exp(-(abs(2.0*vehicle.goal_lane - data["intended_lane"] - data["final_lane"]) / distance));
    * collision_cost : calcualte if any collision happens on current lane or predicted lane.
    for collision on current lane, add cost "COLLISION"
    for collison on predicted target lane, add 2"COLLISION"
    in this case, if change lane cause collison, vehicle will keep on current lane.
    collision is measured if changed lane for ego car, the distance between ego and other car is less than 30m. this 30m distance guardband is also experimental number and can be better determined by current speed and other car speed as well.

Note that Vehicle.cpp from class quiz is not naturally fit into this project. in the quiz, the car change lane directly and predicted position in unit of second. it needs a lots of additional change to fit into this project. such as, lane determination, lane numbering from 0, 1, 2, prepare the lane change vehicles, next state logic change.

Notice some case might have collision. the best parameter to adjust the two collision distance guardband, 25m used in main.cpp and 30m used in collision_cost.(future work to have better calculation).