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
| #include <iostream>
#include <iterator>
#include <map>
#include <sstream>
#include <string>
#include <vector>
#include "road.h"
#include "vehicle.h"
using std::map;
using std::string;
using std::vector;
// Initializes Road 初始化
Road::Road(int speed_limit, double traffic_density, vector<int> &lane_speeds) {
this->num_lanes = lane_speeds.size(); //有多少个道路
this->lane_speeds = lane_speeds;//每个道路的限速
this->speed_limit = speed_limit;//自己车辆的限速
this->density = traffic_density;//交通流
this->camera_center = this->update_width/2;//相机位置
}
Road::~Road() {}
Vehicle Road::get_ego() {
return this->vehicles.find(this->ego_key)->second;//找到当前车 ego_key 是-1
}
void Road::populate_traffic() {//给道路更新车流
//起始点从0开始计算
int start_s = std::max(this->camera_center - (this->update_width/2), 0);
for (int l = 0; l < this->num_lanes; ++l) {//遍历每个道路
int lane_speed = this->lane_speeds[l];//获取当前道路的速度
bool vehicle_just_added = false;//是不是刚刚添加过
for (int s = start_s; s < start_s+this->update_width; ++s) {
if (vehicle_just_added) {
vehicle_just_added = false;//如果之前添加过，就设置为true
}
if (((double) rand() / (RAND_MAX)) < this->density) {//0~1.0 得到一个随机浮点数
Vehicle vehicle = Vehicle(l,s,lane_speed,0);//初始化车辆的 所在车道，坐标，车道的速度，加速度，cs：constant_speed
vehicle.state = "CS";//表示是定速
this->vehicles_added += 1;//汽车编号
this->vehicles.insert(std::pair<int,Vehicle>(vehicles_added,vehicle));//存放车辆的一个字典
vehicle_just_added = true;//返回刚是否增加了车辆
}
}
}
}
void Road::advance() {
map<int ,vector<Vehicle> > predictions;//定义一个预测字典
map<int, Vehicle>::iterator it = this->vehicles.begin(); //迭代所有车辆
while (it != this->vehicles.end()) {//遍历所有汽车
int v_id = it->first;
vector<Vehicle> preds = it->second.generate_predictions();
predictions[v_id] = preds;//将非自我车辆的下两秒预测信息放入predictions里，车道线，s，速度，加速度
++it;
}
it = this->vehicles.begin();//将指针移到开头
while (it != this->vehicles.end()) {
int v_id = it->first;
if (v_id == ego_key) {
vector<Vehicle> trajectory = it->second.choose_next_state(predictions);//为当前车选择下一刻轨迹
it->second.realize_next_state(trajectory);//实现这个轨迹
} else {
it->second.increment(1);//微分时间再加1
}
++it;
}
}
void Road::add_ego(int lane_num, int s, vector<int> &config_data) {//增加自我车辆
map<int, Vehicle>::iterator it = this->vehicles.begin();
while (it != this->vehicles.end()) {
int v_id = it->first;
Vehicle v = it->second;
if (v.lane == lane_num && v.s == s) {
this->vehicles.erase(v_id);
}
++it;
}
Vehicle ego = Vehicle(lane_num, s, this->lane_speeds[lane_num], 0);
ego.configure(config_data);
ego.state = "KL";
this->vehicles.insert(std::pair<int,Vehicle>(ego_key,ego));
}
void Road::display(int timestep) {
Vehicle ego = this->vehicles.find(this->ego_key)->second;
int s = ego.s;
string state = ego.state;
this->camera_center = std::max(s, this->update_width/2);
int s_min = std::max(this->camera_center - this->update_width/2, 0);
int s_max = s_min + this->update_width;
vector<vector<string>> road;
for (int i = 0; i < this->update_width; ++i) {
vector<string> road_lane;
for (int ln = 0; ln < this->num_lanes; ++ln) {
road_lane.push_back(" ");
}
road.push_back(road_lane);
}
map<int, Vehicle>::iterator it = this->vehicles.begin();
while (it != this->vehicles.end()) {
int v_id = it->first;
Vehicle v = it->second;
if (s_min <= v.s && v.s < s_max) {
string marker = "";
if (v_id == this->ego_key) {
marker = this->ego_rep;
} else {
std::stringstream oss;
std::stringstream buffer;
buffer << " ";
oss << v_id;
for (int buffer_i = oss.str().length(); buffer_i < 3; ++buffer_i) {
buffer << "0";
}
buffer << oss.str() << " ";
marker = buffer.str();
}
road[int(v.s - s_min)][int(v.lane)] = marker;
}
++it;
}
std::ostringstream oss;
oss << "+Meters ======================+ step: " << timestep << std::endl;
int i = s_min;
for (int lj = 0; lj < road.size(); ++lj) {
if (i%20 ==0) {
std::stringstream buffer;
std::stringstream dis;
dis << i;
for (int buffer_i = dis.str().length(); buffer_i < 3; ++buffer_i) {
buffer << "0";
}
oss << buffer.str() << dis.str() << " - ";
} else {
oss << " ";
}
++i;
for (int li = 0; li < road[0].size(); ++li) {
oss << "|" << road[lj][li];
}
oss << "|";
oss << "\n";
}
std::cout << oss.str();
}
|
