behavior planning——13. implement a cost function in C++

In the previous quizzes, you designed a cost function to choose a lane when trying to reach a goal in highway driving:

cost=1−e​−​​∣Δd∣​​​​/​Δs

Here, Δ was the lateral distance between the goal lane and the final chosen lane, and Δ was the longitudinal distance from the vehicle to the goal.

In this quiz, we'd like you to implement the cost function in C++, but with one important change. The finite state machine we use for vehicle behavior also includes states for planning a lane change right or left (PLCR or PLCL), and the cost function should incorporate this information. We will provide the following four inputs to the function:

• Intended lane: the intended lane for the given behavior. For PLCR, PLCL, LCR, and LCL, this would be the one lane over from the current lane.
• Final lane: the immediate resulting lane of the given behavior. For LCR and LCL, this would be one lane over.
• The Δs distance to the goal.
• The goal lane.

Your task in the implementation will be to modify Δd in the equation above so that it satisifes:

• Δd is smaller as both intended lane and final lane are closer to the goal lane.
• The cost function provides different costs for each possible behavior: KL, PLCR/PLCL, LCR/LCL.
• The values produced by the cost function are in the range 0 to 1.

You can implement your solution in cost.cpp below.

 cost.cpp

float goal_distance_cost(int goal_lane, int intended_lane, int final_lane, float distance_to_goal) {
    /*
    The cost increases with both the distance of intended lane from the goal
    and the distance of the final lane from the goal. The cost of being out of the 
    goal lane also becomes larger as vehicle approaches the goal.
    */
    int delta_d = 2.0*goal_lane - intended_lane - final_lane;
    float cost = 1 - exp(-(abs(delta_d) / distance_to_goal));
    return cost;
}