CARLA
 
载入中...
搜索中...
未找到
CollisionStage.cpp
浏览该文件的文档.
1
2#include "carla/geom/Math.h"
3
6
8
9namespace carla {
10namespace traffic_manager {
11
12using Point2D = bg::model::point<double, 2, bg::cs::cartesian>; // 定义一个二维笛卡尔坐标系点类型
13using TLS = carla::rpc::TrafficLightState; // 简化交通信号灯状态的命名空间
14
15using namespace constants::Collision; // 引入碰撞相关的常量
16using constants::WaypointSelection::JUNCTION_LOOK_AHEAD; // 引入路口前瞻距离常量
17
18// 碰撞阶段的构造函数
20 const std::vector<ActorId> &vehicle_id_list, // 所有车辆的ID列表
21 const SimulationState &simulation_state, // 仿真状态
22 const BufferMap &buffer_map, // 每辆车的路径缓存
23 const TrackTraffic &track_traffic, // 交通跟踪器
24 const Parameters &parameters, // 仿真参数
25 CollisionFrame &output_array, // 碰撞信息输出
26 RandomGenerator &random_device) // 随机数生成器
27 : vehicle_id_list(vehicle_id_list),
28 simulation_state(simulation_state),
29 buffer_map(buffer_map),
30 track_traffic(track_traffic),
31 parameters(parameters),
32 output_array(output_array),
33 random_device(random_device) {}
34
35// 更新指定索引的车辆碰撞状态
36void CollisionStage::Update(const unsigned long index) {
37 ActorId obstacle_id = 0u; // 障碍物ID ,初始为0
38 bool collision_hazard = false; //碰撞风险标志
39 float available_distance_margin = std::numeric_limits<float>::infinity(); // 可用距离裕量,初始为正无穷
40
41 // 获取当前车辆的ID
42 const ActorId ego_actor_id = vehicle_id_list.at(index);
43 if (simulation_state.ContainsActor(ego_actor_id)) { // 检查仿真中是否包含此车辆
44 const cg::Location ego_location = simulation_state.GetLocation(ego_actor_id); // 获取车辆当前位置
45 const Buffer &ego_buffer = buffer_map.at(ego_actor_id); // 获取车辆的路径缓存
46 const unsigned long look_ahead_index = GetTargetWaypoint(ego_buffer, JUNCTION_LOOK_AHEAD).second; // 计算前瞻路径点索引
47 const float velocity = simulation_state.GetVelocity(ego_actor_id).Length(); // 获取车辆速度
48
49 // 获取与当前车辆路径重叠的其他车辆ID
50 ActorIdSet overlapping_actors = track_traffic.GetOverlappingVehicles(ego_actor_id);
51 std::vector<ActorId> collision_candidate_ids; // 碰撞候选车辆ID列表
52 // 根据速度和参数计算碰撞检测的最大半径平方
53 const float distance_to_leading = parameters.GetDistanceToLeadingVehicle(ego_actor_id); // 获取前车的安全距离
54 float collision_radius_square = SQUARE(COLLISION_RADIUS_RATE * velocity + COLLISION_RADIUS_MIN); // 碰撞半径平方
55 if (velocity < 2.0f) { // 如果车辆速度较低
56 const float length = simulation_state.GetDimensions(ego_actor_id).x; // 获取车辆长度
57 const float collision_radius_stop = COLLISION_RADIUS_STOP + length; // 设置静止时的碰撞半径
58 collision_radius_square = SQUARE(collision_radius_stop);
59 }
60 if (distance_to_leading > collision_radius_square) { // 如果前车距离更大
61 collision_radius_square = SQUARE(distance_to_leading);
62 }
63
64 // 遍历重叠路径上的其他车辆,筛选碰撞候选车辆
65 for (ActorId overlapping_actor_id : overlapping_actors) {
66 // 如果其他车辆在最大碰撞避免范围内,并且垂直方向有重叠
67 const cg::Location &overlapping_actor_location = simulation_state.GetLocation(overlapping_actor_id); // 获取重叠车辆的位置
68 if (overlapping_actor_id != ego_actor_id // 排除自身
69 && cg::Math::DistanceSquared(overlapping_actor_location, ego_location) < collision_radius_square // 检测是否在碰撞半径范围内
70 && std::abs(ego_location.z - overlapping_actor_location.z) < VERTICAL_OVERLAP_THRESHOLD) { // 检测垂直方向的重叠
71 collision_candidate_ids.push_back(overlapping_actor_id); // 添加到碰撞候选列表
72 }
73 }
74
75 // 按与自车的距离对潜在碰撞对象进行升序排序
76 std::sort(collision_candidate_ids.begin(), collision_candidate_ids.end(),
77 [this, &ego_location](const ActorId &a_id_1, const ActorId &a_id_2) {
78 const cg::Location &e_loc = ego_location; // 自车位置
79 const cg::Location &loc_1 = simulation_state.GetLocation(a_id_1); // 对象1的位置
80 const cg::Location &loc_2 = simulation_state.GetLocation(a_id_2); // 对象2的位置
81 // 按距离平方排序,避免使用平方根计算,提升性能
82 return (cg::Math::DistanceSquared(e_loc, loc_1) < cg::Math::DistanceSquared(e_loc, loc_2));
83 });
84
85 // 遍历排序后的对象,检查每个对象是否构成碰撞威胁
86 for (auto iter = collision_candidate_ids.begin();
87 iter != collision_candidate_ids.end() && !collision_hazard;
88 ++iter) {
89 const ActorId other_actor_id = *iter; // 当前检查的对象ID
90 const ActorType other_actor_type = simulation_state.GetType(other_actor_id); // 对象的类型(车辆/行人)
91 // 检查碰撞检测条件是否满足
92 if (parameters.GetCollisionDetection(ego_actor_id, other_actor_id) // 检查自车与目标车之间的碰撞检测设置
93 && buffer_map.find(ego_actor_id) != buffer_map.end() // 检查缓冲区是否存在自车
94 && simulation_state.ContainsActor(other_actor_id)) { // 检查目标对象是否仍在场景中
95 // 通过协商函数计算碰撞威胁
96 std::pair<bool, float> negotiation_result = NegotiateCollision(ego_actor_id,
97 other_actor_id,
98 look_ahead_index);
99 if (negotiation_result.first) { // 如果存在碰撞威胁
100 // 根据对象类型和随机概率,决定是否忽略此威胁
101 if ((other_actor_type == ActorType::Vehicle
103 || (other_actor_type == ActorType::Pedestrian
105 collision_hazard = true; // 标记碰撞威胁
106 obstacle_id = other_actor_id; // 记录威胁对象ID
107 available_distance_margin = negotiation_result.second; // 记录距离裕度
108 }
109 }
110 }
111 }
112 }
113
114 // 更新输出碰撞数据
115 CollisionHazardData &output_element = output_array.at(index);
116 output_element.hazard_actor_id = obstacle_id; // 威胁对象ID
117 output_element.hazard = collision_hazard; // 是否存在碰撞威胁
118 output_element.available_distance_margin = available_distance_margin; // 距离裕度
119}
120
122 // 移除特定对象的碰撞锁定
123 collision_locks.erase(actor_id);
124}
125
127 // 清空所有碰撞锁定
128 collision_locks.clear();
129}
130
132 // 根据速度计算对象的碰撞边界延伸
133 const float velocity = cg::Math::Dot(simulation_state.GetVelocity(actor_id), simulation_state.GetHeading(actor_id)); // 计算对象的速度
134 float bbox_extension;
135 // 使用函数来计算边界长度
136 float velocity_extension = VEL_EXT_FACTOR * velocity; // 根据速度计算延伸因子
137 bbox_extension = BOUNDARY_EXTENSION_MINIMUM + velocity_extension * velocity_extension; // 基础边界延伸
138 // 如果对象有有效的碰撞锁定,调整边界以保持锁定
139 if (collision_locks.find(actor_id) != collision_locks.end()) {
140 const CollisionLock &lock = collision_locks.at(actor_id);
141 float lock_boundary_length = static_cast<float>(lock.distance_to_lead_vehicle + LOCKING_DISTANCE_PADDING);
142 // 仅当前车辆距离未超过速度相关延伸的最大值时,才延伸边界跟踪车辆
143 if ((lock_boundary_length - lock.initial_lock_distance) < MAX_LOCKING_EXTENSION) {
144 bbox_extension = lock_boundary_length;
145 }
146 }
147
148 return bbox_extension; // 返回最终计算的边界长度
149}
150
152 const ActorType actor_type = simulation_state.GetType(actor_id); // 获取实体类型
153 const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id); // 获取实体的朝向向量
154
155 float forward_extension = 0.0f; // 用于扩展边界框的向前长度
156 if (actor_type == ActorType::Pedestrian) {
157 // 扩展行人的边界框,用于预测行人未来的位置,从而避免碰撞
158 forward_extension = simulation_state.GetVelocity(actor_id).Length() * WALKER_TIME_EXTENSION; // 根据速度扩展
159 }
160
161 cg::Vector3D dimensions = simulation_state.GetDimensions(actor_id); // 获取实体的尺寸
162
163 float bbox_x = dimensions.x; // 边界框的x轴长度(前后方向)
164 float bbox_y = dimensions.y; // 边界框的y轴长度(左右方向)
165
166 const cg::Vector3D x_boundary_vector = heading_vector * (bbox_x + forward_extension); // 计算x方向的边界向量
167 const auto perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f).MakeSafeUnitVector(EPSILON); // 计算垂直于朝向的单位向量
168 const cg::Vector3D y_boundary_vector = perpendicular_vector * (bbox_y + forward_extension); // 计算y方向的边界向量
169
170 // 四个顶点,按照顺时针顺序(左手坐标系下的顶视图)
171 const cg::Location location = simulation_state.GetLocation(actor_id); // 获取实体位置
172 LocationVector bbox_boundary = {
173 location + cg::Location(x_boundary_vector - y_boundary_vector), // 左前角
174 location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector), // 左后角
175 location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector), // 右后角
176 location + cg::Location(x_boundary_vector + y_boundary_vector), // 右前角
177 };
178
179 return bbox_boundary; // 返回边界框
180}
181
183 LocationVector geodesic_boundary;
184
185 if (geodesic_boundary_map.find(actor_id) != geodesic_boundary_map.end()) {
186 // 如果地理边界已经缓存,则直接获取
187 geodesic_boundary = geodesic_boundary_map.at(actor_id);
188 } else {
189 const LocationVector bbox = GetBoundary(actor_id); //获取边界框
190
191 if (buffer_map.find(actor_id) != buffer_map.end()) {
192 float bbox_extension = GetBoundingBoxExtention(actor_id); // 获取边界框扩展值
193 const float specific_lead_distance = parameters.GetDistanceToLeadingVehicle(actor_id); // 获取特定的前车距离
194 bbox_extension = std::max(specific_lead_distance, bbox_extension); // 扩展边界框,使用更大的距离
195 const float bbox_extension_square = SQUARE(bbox_extension); // 计算扩展距离的平方
196
197 LocationVector left_boundary; // 左边界点集合
198 LocationVector right_boundary; // 右边界点集合
199 cg::Vector3D dimensions = simulation_state.GetDimensions(actor_id); // 获取实体的尺寸
200 const float width = dimensions.y; // 宽度
201 const float length = dimensions.x; // 长度
202
203 const Buffer &waypoint_buffer = buffer_map.at(actor_id); // 获取路径缓冲区
204 const TargetWPInfo target_wp_info = GetTargetWaypoint(waypoint_buffer, length); // 获取目标路径点和起点索引
205 const SimpleWaypointPtr boundary_start = target_wp_info.first; // 边界起始路径点
206 const uint64_t boundary_start_index = target_wp_info.second; // 边界起始索引
207
208 // 在无信号交叉口,我们扩展边界穿过交叉口
209 // 在所有其他情况下,边界长度与速度相关
210 SimpleWaypointPtr boundary_end = nullptr;
211 SimpleWaypointPtr current_point = waypoint_buffer.at(boundary_start_index);
212 bool reached_distance = false;
213 for (uint64_t j = boundary_start_index; !reached_distance && (j < waypoint_buffer.size()); ++j) {
214 if (boundary_start->DistanceSquared(current_point) > bbox_extension_square || j == waypoint_buffer.size() - 1) {
215 reached_distance = true;
216 }
217 if (boundary_end == nullptr
218 || cg::Math::Dot(boundary_end->GetForwardVector(), current_point->GetForwardVector()) < COS_10_DEGREES
219 || reached_distance) {
220
221 const cg::Vector3D heading_vector = current_point->GetForwardVector();
222 const cg::Location location = current_point->GetLocation();
223 cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f);
224 perpendicular_vector = perpendicular_vector.MakeSafeUnitVector(EPSILON);
225 // 方向根据左手坐标系确定
226 const cg::Vector3D scaled_perpendicular = perpendicular_vector * width;
227 left_boundary.push_back(location + cg::Location(scaled_perpendicular));
228 right_boundary.push_back(location + cg::Location(-1.0f * scaled_perpendicular));
229
230 boundary_end = current_point;
231 }
232
233 current_point = waypoint_buffer.at(j);
234 }
235
236 // 反向右边界以构建顺时针(左手坐标系)
237 // 边界。这是因为左边界和右边界向量都有
238 // 在右边界的起始索引处与车辆的最近点
239 // 边界
240 // 我们希望从最远的点开始,以获得顺时针轨迹
241 std::reverse(right_boundary.begin(), right_boundary.end());
242 geodesic_boundary.insert(geodesic_boundary.end(), right_boundary.begin(), right_boundary.end());
243 geodesic_boundary.insert(geodesic_boundary.end(), bbox.begin(), bbox.end());
244 geodesic_boundary.insert(geodesic_boundary.end(), left_boundary.begin(), left_boundary.end());
245 } else {
246
247 geodesic_boundary = bbox;
248 }
249
250 geodesic_boundary_map.insert({actor_id, geodesic_boundary});
251 }
252
253 return geodesic_boundary;
254}
255
257
258 traffic_manager::Polygon boundary_polygon; //定义一个多边形对象
259 for (const cg::Location &location : boundary) {
260 // 将边界点逐一添加到多边形外环中
261 bg::append(boundary_polygon.outer(), Point2D(location.x, location.y));
262 }
263 // 将起始点再次添加到外环,闭合多边形
264 bg::append(boundary_polygon.outer(), Point2D(boundary.front().x, boundary.front().y));
265
266 return boundary_polygon; // 返回多边形
267}
268
270 const ActorId other_actor_id) {
271
272
273 std::pair<ActorId, ActorId> key_parts;
274 if (reference_vehicle_id < other_actor_id) {
275 // 确保缓存键的生成始终保持一致,选择较小的 ActorId 为第一个键
276 key_parts = {reference_vehicle_id, other_actor_id};
277 } else {
278 key_parts = {other_actor_id, reference_vehicle_id};
279 }
280
281 uint64_t actor_id_key = 0u; // 用于存储唯一键
282 actor_id_key |= key_parts.first; // 首先存储第一个实体 ID
283 actor_id_key <<= 32; // 左移32位,为第二个实体ID留出空间
284 actor_id_key |= key_parts.second; // 添加第二个实体ID
285
286 GeometryComparison comparision_result{-1.0, -1.0, -1.0, -1.0}; // 默认比较结果,初始化为-1.0
287
288 if (geometry_cache.find(actor_id_key) != geometry_cache.end()) {
289 // 如果几何关系已缓存,则直接获取
290 comparision_result = geometry_cache.at(actor_id_key);
291 double mref_veh_other = comparision_result.reference_vehicle_to_other_geodesic;
292 // 交换参考车辆到其他车辆的距离和相反方向的距离
293 comparision_result.reference_vehicle_to_other_geodesic = comparision_result.other_vehicle_to_reference_geodesic;
294 comparision_result.other_vehicle_to_reference_geodesic = mref_veh_other;
295 } else {
296 // 获取参考车辆的边界多边形
297 const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle_id));
298 // 获取其他实体的边界多边形
299 const Polygon other_polygon = GetPolygon(GetBoundary(other_actor_id));
300 // 获取参考车辆的地理边界多边形
301 const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle_id));
302 //获取其他实体的地理边界多边形
303 const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_actor_id));
304 // 计算参考车辆到其他实体地理边界的距离
305 const double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon);
306 // 计算其他实体到参考车辆地理边界的距离
307 const double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon);
308 // 计算两实体地理边界之间的距离
309 const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon);
310 // 计算两实体边界框之间的距离
311 const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon);
312 // 将计算结果存储到比较结果中
313 comparision_result = {reference_vehicle_to_other_geodesic,
314 other_vehicle_to_reference_geodesic,
315 inter_geodesic_distance,
316 inter_bbox_distance};
317 // 将结果缓存
318 geometry_cache.insert({actor_id_key, comparision_result});
319 }
320
321 return comparision_result; // 返回几何比较结果
322}
323
324std::pair<bool, float> CollisionStage::NegotiateCollision(const ActorId reference_vehicle_id,
325 const ActorId other_actor_id,
326 const uint64_t reference_junction_look_ahead_index) {
327 // 方法的输出变量
328 bool hazard = false;
329 float available_distance_margin = std::numeric_limits<float>::infinity();
330
331 const cg::Location reference_location = simulation_state.GetLocation(reference_vehicle_id);
332 const cg::Location other_location = simulation_state.GetLocation(other_actor_id);
333
334 // 自我和其他车辆的方向
335 const cg::Vector3D reference_heading = simulation_state.GetHeading(reference_vehicle_id);
336 // 从自车位置到其他车辆位置的向量
337 cg::Vector3D reference_to_other = other_location - reference_location;
338 reference_to_other = reference_to_other.MakeSafeUnitVector(EPSILON);
339
340 // 其他车辆的方向
341 const cg::Vector3D other_heading = simulation_state.GetHeading(other_actor_id);
342 // 从其他车辆位置到自我位置的向量
343 cg::Vector3D other_to_reference = reference_location - other_location;
344 other_to_reference = other_to_reference.MakeSafeUnitVector(EPSILON);
345
346 float reference_vehicle_length = simulation_state.GetDimensions(reference_vehicle_id).x * SQUARE_ROOT_OF_TWO;
347 float other_vehicle_length = simulation_state.GetDimensions(other_actor_id).x * SQUARE_ROOT_OF_TWO;
348
349 float inter_vehicle_distance = cg::Math::DistanceSquared(reference_location, other_location);
350 float ego_bounding_box_extension = GetBoundingBoxExtention(reference_vehicle_id);
351 float other_bounding_box_extension = GetBoundingBoxExtention(other_actor_id);
352 // 计算车辆之间考虑碰撞协商的最小距离
353 float inter_vehicle_length = reference_vehicle_length + other_vehicle_length;
354 float ego_detection_range = SQUARE(ego_bounding_box_extension + inter_vehicle_length);
355 float cross_detection_range = SQUARE(ego_bounding_box_extension + inter_vehicle_length + other_bounding_box_extension);
356
357 // 考虑碰撞谈判的条件
358 bool other_vehicle_in_ego_range = inter_vehicle_distance < ego_detection_range;
359 bool other_vehicles_in_cross_detection_range = inter_vehicle_distance < cross_detection_range;
360 float reference_heading_to_other_dot = cg::Math::Dot(reference_heading, reference_to_other);
361 bool other_vehicle_in_front = reference_heading_to_other_dot > 0;
362 const Buffer &reference_vehicle_buffer = buffer_map.at(reference_vehicle_id);
363 SimpleWaypointPtr closest_point = reference_vehicle_buffer.front();
364 bool ego_inside_junction = closest_point->CheckJunction();
365 TrafficLightState reference_tl_state = simulation_state.GetTLS(reference_vehicle_id);
366 bool ego_at_traffic_light = reference_tl_state.at_traffic_light;
367 bool ego_stopped_by_light = reference_tl_state.tl_state != TLS::Green && reference_tl_state.tl_state != TLS::Off;
368 SimpleWaypointPtr look_ahead_point = reference_vehicle_buffer.at(reference_junction_look_ahead_index);
369 bool ego_at_junction_entrance = !closest_point->CheckJunction() && look_ahead_point->CheckJunction();
370
371 // 考虑碰撞谈判的条件
372 if (!(ego_at_junction_entrance && ego_at_traffic_light && ego_stopped_by_light)
373 && ((ego_inside_junction && other_vehicles_in_cross_detection_range)
374 || (!ego_inside_junction && other_vehicle_in_front && other_vehicle_in_ego_range))) {
375 GeometryComparison geometry_comparison = GetGeometryBetweenActors(reference_vehicle_id, other_actor_id);
376
377 // 碰撞谈判的条件
378 bool geodesic_path_bbox_touching = geometry_comparison.inter_geodesic_distance < OVERLAP_THRESHOLD;
379 bool vehicle_bbox_touching = geometry_comparison.inter_bbox_distance < OVERLAP_THRESHOLD;
380 bool ego_path_clear = geometry_comparison.other_vehicle_to_reference_geodesic > OVERLAP_THRESHOLD;
381 bool other_path_clear = geometry_comparison.reference_vehicle_to_other_geodesic > OVERLAP_THRESHOLD;
382 bool ego_path_priority = geometry_comparison.reference_vehicle_to_other_geodesic < geometry_comparison.other_vehicle_to_reference_geodesic;
383 bool other_path_priority = geometry_comparison.reference_vehicle_to_other_geodesic > geometry_comparison.other_vehicle_to_reference_geodesic;
384 bool ego_angular_priority = reference_heading_to_other_dot< cg::Math::Dot(other_heading, other_to_reference);
385
386 // 哪辆车的路径离另一辆车最远,哪辆车就优先通行
387 bool lower_priority = !ego_path_priority && (other_path_priority || !ego_angular_priority);
388 bool blocked_by_other_or_lower_priority = !ego_path_clear || (other_path_clear && lower_priority);
389 bool yield_pre_crash = !vehicle_bbox_touching && blocked_by_other_or_lower_priority;
390 bool yield_post_crash = vehicle_bbox_touching && !ego_angular_priority;
391
392 if (geodesic_path_bbox_touching && (yield_pre_crash || yield_post_crash)) {
393
394 hazard = true;
395
396 const float reference_lead_distance = parameters.GetDistanceToLeadingVehicle(reference_vehicle_id);
397 const float specific_distance_margin = std::max(reference_lead_distance, MIN_REFERENCE_DISTANCE);
398 available_distance_margin = static_cast<float>(std::max(geometry_comparison.reference_vehicle_to_other_geodesic
399 - static_cast<double>(specific_distance_margin), 0.0));
400
401 ///////////////////////////////////// 碰撞锁定机构 /////////////////////////////////
402 // 这个想法是,在遇到可能的碰撞时,
403 // 我们应该确保边界框的扩展不会过快地减小,从而导致碰撞跟踪的丢失
404 // 这使得我们能够平稳地接近前车
405
406 // 当发现可能的碰撞时,检查是否存在碰撞锁的条目
407 if (collision_locks.find(reference_vehicle_id) != collision_locks.end()) {
408 CollisionLock &lock = collision_locks.at(reference_vehicle_id);
409 // 检查同一车辆是否处于锁定状态
410 if (other_actor_id == lock.lead_vehicle_id) {
411 // 如果领头车辆的车身与参考车辆的边界框接触
412 if (geometry_comparison.other_vehicle_to_reference_geodesic < OVERLAP_THRESHOLD) {
413 // 车辆车身之间的距离
414 lock.distance_to_lead_vehicle = geometry_comparison.inter_bbox_distance;
415 } else {
416 // 参考车辆车身与其他车辆路径多边形的距离
418 }
419 } else {
420 // 如果可能与新车辆发生碰撞,请使用新的锁定条目重新初始化
421 lock = {geometry_comparison.inter_bbox_distance, geometry_comparison.inter_bbox_distance, other_actor_id};
422 }
423 } else {
424 // 如果锁条目不存在,则插入并初始化锁条目
425 collision_locks.insert({reference_vehicle_id,
426 {geometry_comparison.inter_bbox_distance,
427 geometry_comparison.inter_bbox_distance,
428 other_actor_id}});
429 }
430 }
431 }
432
433 // 如果没有检测到碰撞危险,则清除车辆持有的碰撞锁定
434 if (!hazard && collision_locks.find(reference_vehicle_id) != collision_locks.end()) {
435 collision_locks.erase(reference_vehicle_id);
436 }
437
438 return {hazard, available_distance_margin};
439}
440
445
446} // namespace traffic_manager
447} // namespace carla
#define SQUARE(a)
This file contains various constants used in traffic manager arranged into sensible namespaces for re...
Definition Constants.h:13
Vector3D MakeSafeUnitVector(const float epsilon) const
CollisionStage(const std::vector< ActorId > &vehicle_id_list, const SimulationState &simulation_state, const BufferMap &buffer_map, const TrackTraffic &track_traffic, const Parameters &parameters, CollisionFrame &output_array, RandomGenerator &random_device)
const std::vector< ActorId > & vehicle_id_list
const SimulationState & simulation_state
GeometryComparison GetGeometryBetweenActors(const ActorId reference_vehicle_id, const ActorId other_actor_id)
LocationVector GetGeodesicBoundary(const ActorId actor_id)
Polygon GetPolygon(const LocationVector &boundary)
void RemoveActor(const ActorId actor_id) override
移除参与者方法。
LocationVector GetBoundary(const ActorId actor_id)
float GetBoundingBoxExtention(const ActorId actor_id)
void Update(const unsigned long index) override
更新方法。
void Reset() override
重置方法。
std::pair< bool, float > NegotiateCollision(const ActorId reference_vehicle_id, const ActorId other_actor_id, const uint64_t reference_junction_look_ahead_index)
float GetPercentageIgnoreWalkers(const ActorId &actor_id) const
获取百分比以忽略任何步行者的方法
float GetDistanceToLeadingVehicle(const ActorId &actor_id) const
查询给定车辆与前方车辆之间距离的方法
bool GetCollisionDetection(const ActorId &reference_actor_id, const ActorId &other_actor_id) const
查询一对车辆之间避碰规则的方法
float GetPercentageIgnoreVehicles(const ActorId &actor_id) const
方法获取百分比以忽略任何车辆
该类保持了仿真中所有车辆的状态。
bool ContainsActor(ActorId actor_id) const
cg::Location GetLocation(const ActorId actor_id) const
cg::Vector3D GetHeading(const ActorId actor_id) const
cg::Vector3D GetVelocity(const ActorId actor_id) const
ActorType GetType(const ActorId actor_id) const
cg::Vector3D GetDimensions(const ActorId actor_id) const
TrafficLightState GetTLS(const ActorId actor_id) const
ActorIdSet GetOverlappingVehicles(ActorId actor_id) const
bg::model::point< double, 2, bg::cs::cartesian > Point2D
std::deque< std::shared_ptr< SimpleWaypoint > > Buffer
std::vector< CollisionHazardData > CollisionFrame
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &target_point_distance)
carla::ActorId ActorId
参与者的智能指针类型
std::unordered_set< ActorId > ActorIdSet
std::vector< cg::Location > LocationVector
std::unordered_map< carla::ActorId, Buffer > BufferMap
bg::model::polygon< bg::model::d2::point_xy< double > > Polygon
std::pair< SimpleWaypointPtr, uint64_t > TargetWPInfo
根据目标点距离从路点缓冲区返回路点信息
CARLA模拟器的主命名空间。
Definition Carla.cpp:139