udacity_highway04
代码解析
从csv文件里读入数据
csv文件数据格式1
2
3
4
5784.6001 1135.571 0 -0.02359831 -0.9997216
815.2679 1134.93 30.6744785308838 -0.01099479 -0.9999396
844.6398 1134.911 60.0463714599609 -0.002048373 -0.9999979
875.0436 1134.808 90.4504146575928 -0.001847863 -0.9999983
905.283 1134.799 120.689735412598 0.004131136 -0.9999915
- 建立五个向量创建路径 字符串
1
2
3
4
5vector<double> map_waypoints_x;
vector<double> map_waypoints_y;
vector<double> map_waypoints_s;
vector<double> map_waypoints_dx;
vector<double> map_waypoints_dy;string map_file_ = "../data/highway_map.csv";
使用头文件#include <fstream> 创建一个输入流
ifstream in_map_(map_file_.c_str(), ifstream::in);
开始逐行获取数据
1 | string line; |
定义初始所在的道路和速度1
2
3
4// Start on lane 1
int lane = 1;
// Reference velocity (mph)
double ref_vel = 0.0;
uWebSockets将事件循环集成到一个线程中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
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226h.onMessage([&map_waypoints_x,&map_waypoints_y,&map_waypoints_s,&map_waypoints_dx,&map_waypoints_dy,&ref_vel, &lane](uWS::WebSocket<uWS::SERVER> ws, char *data, size_t length,
uWS::OpCode opCode) {//使用这些数据
// "42" at the start of the message means there's a websocket message event.
// The 4 signifies a websocket message
// The 2 signifies a websocket event
//auto sdata = string(data).substr(0, length);
//cout << sdata << endl;
if (length && length > 2 && data[0] == '4' && data[1] == '2') {//先验证输入的数据是否正确
auto s = hasData(data);// Checks if the SocketIO event has JSON data
if (s != "") {
json j = json::parse(s);
string event = j[0].get<string>();
if (event == "telemetry") {
// j[1] is the data JSON object
// Main car's localization data
double car_x = j[1]["x"];
double car_y = j[1]["y"];
double car_s = j[1]["s"];
double car_d = j[1]["d"];
double car_yaw = j[1]["yaw"];
double car_speed = j[1]["speed"];
// Previous path data given to the Planner
auto previous_path_x = j[1]["previous_path_x"];
auto previous_path_y = j[1]["previous_path_y"];
//判断previous_path_x[-1] 变换后是否等于end_path_s
// Previous path's end `s` and `d` values
double end_path_s = j[1]["end_path_s"];
double end_path_d = j[1]["end_path_d"];
// Sensor Fusion data - a list of all other cars on the same side of the road.在同一侧的所有其他车辆的列表，里面存放的是类对象车
auto sensor_fusion = j[1]["sensor_fusion"];
int prev_size = previous_path_x.size();
if (prev_size > 0)
car_s = end_path_s;
//用于转弯的信息
bool is_too_close = false;
bool prepare_for_lane_change = false;
bool ready_for_lane_change = false;
bool is_left_lane_free = true;
bool is_right_lane_free = true;
for (size_t i = 0; i < sensor_fusion.size(); ++i) {
Vehicle vehicle(sensor_fusion[i]);//初始化附近的第i个车辆
//只要出现在同一车道，0.02s后这辆车还在本车前，且与本车的end_path距离小于安全距离
if (is_in_lane(vehicle.d, lane)) {//判断传感器里的车辆是否和本车辆同一个车道
//如果是，则对这辆车进行预测，
vehicle.s += (double)prev_size * 0.02 * vehicle.speed; // 获取0.02s预测后的距离use previous points to project s value onward
bool is_in_front_of_us = vehicle.s > car_s;//预测后的距离与现在车辆进行比较，查看是否在本车前
//查看预测后的end_path的距离是否是安全距离
bool is_closer_than_safety_margin = vehicle.s - car_s < safety_margin;
if (is_in_front_of_us && is_closer_than_safety_margin) {
//在我们前面，且和本车的end_path小于安全距离，就表明很近
is_too_close = true;
prepare_for_lane_change = true;//就可以准备变道
}
}
}
if (prepare_for_lane_change) {//如果开始直行变道
int num_vehicles_left = 0;//记录左侧车道的车辆
int num_vehicles_right = 0;//记录右侧车道的车辆
// Check if left and right lanes are free 检查左侧或者右侧车道是都空闲
for (size_t i = 0; i < sensor_fusion.size(); ++i) {
Vehicle vehicle(sensor_fusion[i]);//从传感器里取出车辆
// Check left lane 检查是不是在左侧
if (is_in_lane(vehicle.d, lane - 1)) {
++num_vehicles_left;//如果在左侧
vehicle.s += (double)prev_size * 0.02 * vehicle.speed;
//检查左侧的车辆在相同的end时间 prev_size * 0.02s后的预测地点和车辆目前规划的路径的终点是否太近
//太近不能变道的条件
// 1. 汽车的终点的距离减去一半的安全距离，小于预测距离
// 2. 汽车终点距离加上一半安全距离，大于预测距离
bool too_close_to_change = (vehicle.s > car_s - safety_margin / 2) && (vehicle.s < car_s + safety_margin / 2);
if (too_close_to_change)
is_left_lane_free = false;
}
// Check right lane
else if (is_in_lane(vehicle.d, lane + 1)) {
++num_vehicles_right;
vehicle.s += (double)prev_size * 0.02 * vehicle.speed;
bool too_close_to_change = (vehicle.s > car_s - safety_margin / 2) && (vehicle.s < car_s + safety_margin / 2);
if (too_close_to_change)
is_right_lane_free = false;
}
//如果两个有一个可以变道，就说明可以转
if (is_left_lane_free || is_right_lane_free)
ready_for_lane_change = true;
}//输出左侧和右侧车辆个数
cout << "LEFT " << num_vehicles_left << "RIGHT " << num_vehicles_right << endl;
}
// Actually perform lane change 开始实行变道
if (ready_for_lane_change && is_left_lane_free && lane > 0)
lane -= 1;//左侧可以变道，就将变道放到左侧
else if (ready_for_lane_change && is_right_lane_free && lane < 2)
lane += 1;//右侧可以变道，就将车辆lane加1
// Eventually slow down if too close the car before
if (is_too_close) //如果当前车辆的前车很近，就开始减速 加速度5，增加速度0.224
ref_vel -= 0.224; // deceleration around 5 m/s^2
else if (ref_vel < max_safe_speed)//如果当前车辆的前车不近，就开始加速
ref_vel += 0.224;
// List of widely spaced (x, y) waypoints. These will be later interpolated 开始使用插值法对路径进行平滑
// with a spline, filling it with more points which control speed.
// 用插值法添加更多的点
vector<double> pts_x;
vector<double> pts_y;
// Reference x, y, yaw state
//插值法填入的速度和角度信息
double ref_x = car_x;
double ref_y = car_y;
double ref_yaw = deg2rad(car_yaw);
// If previous size is almost empty, use the car as a starting reference
// 如果之间的道路的size都差不多是空的，就使用汽车现状作为初始参考
if (prev_size < 2) { //如果以前的路径的节点小于2
double prev_car_x = car_x - cos(car_yaw);//汽车以前路径的终点减去一个汽车转角的cos乘以单位1
double prev_car_y = car_y - sin(car_yaw);
将这两个点都放到需要做插值的向量里
pts_x.push_back(prev_car_x); pts_x.push_back(car_x);
pts_y.push_back(prev_car_y); pts_y.push_back(car_y);
}
// Use the previous path's end points as starting reference
else {//如果之前规划的路径的点很多，参考点就设置成之前路径的终点
ref_x = previous_path_x[prev_size - 1];
ref_y = previous_path_y[prev_size - 1];
//参考点前面的一个点就设置成之前路径的终点前一个点
double ref_x_prev = previous_path_x[prev_size - 2];
double ref_y_prev = previous_path_y[prev_size - 2];
ref_yaw = atan2(ref_y - ref_y_prev, ref_x - ref_x_prev);
//参考角度就是最后两个点之间的转角
pts_x.push_back(ref_x_prev); pts_x.push_back(ref_x);
pts_y.push_back(ref_y_prev); pts_y.push_back(ref_y);
}
// In Frenet coordinates, add evenly 30m spaced points ahead of the starting reference 在起始点之后每30m处添加间隔的点
//frenet_to_cartesian(s,d,向量地图里所有s,向量地图里所有x,向量地图里所有y) 坐标系变换 下一个路径点
//car_s + 30
vector<double> next_wp0 = frenet_to_cartesian(car_s + 30, (lane_width * lane + lane_width / 2), map_waypoints_s, map_waypoints_x, map_waypoints_y);
vector<double> next_wp1 = frenet_to_cartesian(car_s + 60, (lane_width * lane + lane_width / 2), map_waypoints_s, map_waypoints_x, map_waypoints_y);
vector<double> next_wp2 = frenet_to_cartesian(car_s + 90, (lane_width * lane + lane_width / 2), map_waypoints_s, map_waypoints_x, map_waypoints_y);
//将30米一个距离的点放入pt里
pts_x.push_back(next_wp0[0]); pts_x.push_back(next_wp1[0]); pts_x.push_back(next_wp2[0]);
pts_y.push_back(next_wp0[1]); pts_y.push_back(next_wp1[1]); pts_y.push_back(next_wp2[1]);
// Rototranslate into car's reference system to make the math easier
//旋转到汽车参考系
for (size_t i = 0; i < pts_x.size(); ++i) {//将pt里的点转到汽车坐标系
double shift_x = pts_x[i] - ref_x;//原来汽车的终点作为圆点
double shift_y = pts_y[i] - ref_y;
pts_x[i] = shift_x * cos(0 - ref_yaw) - shift_y * sin(0 - ref_yaw);
pts_y[i] = shift_x * sin(0 - ref_yaw) + shift_y * cos(0 - ref_yaw);
}
// Create a spline
//建立插值
tk::spline s;
s.set_points(pts_x, pts_y);
// Define he actual (x, y) points will be used for the planner
vector<double> next_x_vals;
vector<double> next_y_vals;
// Start with all previous points from last time
for (size_t i = 0; i < previous_path_x.size(); ++i) {
next_x_vals.push_back(previous_path_x[i]);
next_y_vals.push_back(previous_path_y[i]);
}
// Calculate how to break up spline points to travel at reference velocity
double target_x = 30.0;
double target_y = s(target_y);
double target_dist = sqrt(target_x * target_x + target_y * target_y);
double x_add_on = 0.0;
for (size_t i = 1; i <= 50 - previous_path_x.size(); ++i) {
//1 m/s = 2.24mph
double N = target_dist / (0.02 * ref_vel / 2.24);
double x_point = x_add_on + target_x / N;
double y_point = s(x_point);
x_add_on = x_point;
double x_ref = x_point;
double y_ref = y_point;
// Rotate back into previous coordinate system
//在转到笛卡尔坐标系
x_point = x_ref * cos(ref_yaw) - y_ref * sin(ref_yaw);
y_point = x_ref * sin(ref_yaw) + y_ref * cos(ref_yaw);
x_point += ref_x;
y_point += ref_y;
next_x_vals.push_back(x_point);
next_y_vals.push_back(y_point);
}
// Prepare message and send it to the simulator
json msgJson;
msgJson["next_x"] = next_x_vals;
msgJson["next_y"] = next_y_vals;
auto msg = "42[\"control\","+ msgJson.dump()+"]";
ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
}
} else {
// Manual driving
std::string msg = "42[\"manual\",{}]";
ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
}
}
});
