1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
| #include "cost.h"
#include <cmath>
#include <functional>
#include <iterator>
#include <map>
#include <string>
#include <vector>
#include "vehicle.h"
using std::string;
using std::vector;
/**
* TODO: change weights for cost functions.
*/
//这里如果修改会怎么样
const float REACH_GOAL = pow(10, 6);
const float EFFICIENCY = pow(10, 5);
// Here we have provided two possible suggestions for cost functions, but feel
// free to use your own! The weighted cost over all cost functions is computed
// in calculate_cost. The data from get_helper_data will be very useful in
// your implementation of the cost functions below. Please see get_helper_data
// for details on how the helper data is computed.
float goal_distance_cost(const Vehicle &vehicle,
const vector<Vehicle> &trajectory,
const map<int, vector<Vehicle>> &predictions,
map<string, float> &data) {
// Cost increases based on distance of intended lane (for planning a lane
// change) and final lane of trajectory.
// Cost of being out of goal lane also becomes larger as vehicle approaches
// goal distance.
// This function is very similar to what you have already implemented in the
// "Implement a Cost Function in C++" quiz.
float cost;
float distance = data["distance_to_goal"];
if (distance > 0) {
cost = 1 - 2*exp(-(abs(2.0*vehicle.goal_lane - data["intended_lane"]
- data["final_lane"]) / distance));
} else {
cost = 1;
}
return cost;
}
float inefficiency_cost(const Vehicle &vehicle,
const vector<Vehicle> &trajectory,
const map<int, vector<Vehicle>> &predictions,
map<string, float> &data) {
// Cost becomes higher for trajectories with intended lane and final lane
// that have traffic slower than vehicle's target speed.
// You can use the lane_speed function to determine the speed for a lane.
// This function is very similar to what you have already implemented in
// the "Implement a Second Cost Function in C++" quiz.
float proposed_speed_intended = lane_speed(predictions, data["intended_lane"]);
if (proposed_speed_intended < 0) {
proposed_speed_intended = vehicle.target_speed;
}
float proposed_speed_final = lane_speed(predictions, data["final_lane"]);
if (proposed_speed_final < 0) {
proposed_speed_final = vehicle.target_speed;
}
float cost = (2.0*vehicle.target_speed - proposed_speed_intended
- proposed_speed_final)/vehicle.target_speed;
return cost;
}
float lane_speed(const map<int, vector<Vehicle>> &predictions, int lane) {
// All non ego vehicles in a lane have the same speed, so to get the speed
// limit for a lane, we can just find one vehicle in that lane.
for (map<int, vector<Vehicle>>::const_iterator it = predictions.begin();
it != predictions.end(); ++it) {
int key = it->first;
Vehicle vehicle = it->second[0];
if (vehicle.lane == lane && key != -1) {
return vehicle.v;
}
}
// Found no vehicle in the lane
return -1.0;
}
//两个cost得用权值来相加
float calculate_cost(const Vehicle &vehicle,
const map<int, vector<Vehicle>> &predictions,
const vector<Vehicle> &trajectory) {
// Sum weighted cost functions to get total cost for trajectory.
map<string, float> trajectory_data = get_helper_data(vehicle, trajectory,
predictions);
float cost = 0.0;
// Add additional cost functions here.
vector<std::function<float(const Vehicle &, const vector<Vehicle> &,
const map<int, vector<Vehicle>> &,
map<string, float> &)
>> cf_list = {goal_distance_cost, inefficiency_cost};
vector<float> weight_list = {REACH_GOAL, EFFICIENCY};
for (int i = 0; i < cf_list.size(); ++i) {
float new_cost = weight_list[i]*cf_list[i](vehicle, trajectory, predictions,
trajectory_data);
cost += new_cost;
}
return cost;//权值相加的cost
}
map<string, float> get_helper_data(const Vehicle &vehicle,
const vector<Vehicle> &trajectory,
const map<int, vector<Vehicle>> &predictions) {
// Generate helper data to use in cost functions:
// intended_lane: the current lane +/- 1 if vehicle is planning or
// executing a lane change.
// final_lane: the lane of the vehicle at the end of the trajectory.
// distance_to_goal: the distance of the vehicle to the goal.
// Note that intended_lane and final_lane are both included to help
// differentiate between planning and executing a lane change in the
// cost functions.
map<string, float> trajectory_data;
Vehicle trajectory_last = trajectory[1];
float intended_lane;
if (trajectory_last.state.compare("PLCL") == 0) {
intended_lane = trajectory_last.lane + 1;
} else if (trajectory_last.state.compare("PLCR") == 0) {
intended_lane = trajectory_last.lane - 1;
} else {
intended_lane = trajectory_last.lane;
}
float distance_to_goal = vehicle.goal_s - trajectory_last.s;
float final_lane = trajectory_last.lane;
trajectory_data["intended_lane"] = intended_lane;
trajectory_data["final_lane"] = final_lane;
trajectory_data["distance_to_goal"] = distance_to_goal;
return trajectory_data;
}
|
