10namespace traffic_manager {
12using Point2D = bg::model::point<double, 2, bg::cs::cartesian>;
15using namespace constants::Collision;
20 const std::vector<ActorId> &vehicle_id_list,
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) {}
38 bool collision_hazard =
false;
39 float available_distance_margin = std::numeric_limits<float>::infinity();
46 const unsigned long look_ahead_index =
GetTargetWaypoint(ego_buffer, JUNCTION_LOOK_AHEAD).second;
51 std::vector<ActorId> collision_candidate_ids;
55 if (velocity < 2.0f) {
58 collision_radius_square =
SQUARE(collision_radius_stop);
60 if (distance_to_leading > collision_radius_square) {
61 collision_radius_square =
SQUARE(distance_to_leading);
65 for (
ActorId overlapping_actor_id : overlapping_actors) {
68 if (overlapping_actor_id != ego_actor_id
69 && cg::Math::DistanceSquared(overlapping_actor_location, ego_location) < collision_radius_square
71 collision_candidate_ids.push_back(overlapping_actor_id);
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);
80 const cg::Location &loc_2 = simulation_state.GetLocation(a_id_2);
82 return (cg::Math::DistanceSquared(e_loc, loc_1) < cg::Math::DistanceSquared(e_loc, loc_2));
86 for (
auto iter = collision_candidate_ids.begin();
87 iter != collision_candidate_ids.end() && !collision_hazard;
89 const ActorId other_actor_id = *iter;
99 if (negotiation_result.first) {
105 collision_hazard =
true;
106 obstacle_id = other_actor_id;
107 available_distance_margin = negotiation_result.second;
117 output_element.
hazard = collision_hazard;
134 float bbox_extension;
144 bbox_extension = lock_boundary_length;
148 return bbox_extension;
155 float forward_extension = 0.0f;
163 float bbox_x = dimensions.
x;
164 float bbox_y = dimensions.
y;
166 const cg::Vector3D x_boundary_vector = heading_vector * (bbox_x + forward_extension);
168 const cg::Vector3D y_boundary_vector = perpendicular_vector * (bbox_y + forward_extension);
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),
179 return bbox_boundary;
194 bbox_extension = std::max(specific_lead_distance, bbox_extension);
195 const float bbox_extension_square =
SQUARE(bbox_extension);
200 const float width = dimensions.
y;
201 const float length = dimensions.
x;
206 const uint64_t boundary_start_index = target_wp_info.second;
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;
217 if (boundary_end ==
nullptr
218 || cg::Math::Dot(boundary_end->GetForwardVector(), current_point->GetForwardVector()) <
COS_10_DEGREES
219 || reached_distance) {
221 const cg::Vector3D heading_vector = current_point->GetForwardVector();
222 const cg::Location location = current_point->GetLocation();
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));
230 boundary_end = current_point;
233 current_point = waypoint_buffer.at(j);
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());
247 geodesic_boundary = bbox;
253 return geodesic_boundary;
261 bg::append(boundary_polygon.outer(),
Point2D(location.x, location.y));
264 bg::append(boundary_polygon.outer(),
Point2D(boundary.front().x, boundary.front().y));
266 return boundary_polygon;
270 const ActorId other_actor_id) {
273 std::pair<ActorId, ActorId> key_parts;
274 if (reference_vehicle_id < other_actor_id) {
276 key_parts = {reference_vehicle_id, other_actor_id};
278 key_parts = {other_actor_id, reference_vehicle_id};
281 uint64_t actor_id_key = 0u;
282 actor_id_key |= key_parts.first;
284 actor_id_key |= key_parts.second;
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;
305 const double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon);
307 const double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon);
309 const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon);
311 const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon);
313 comparision_result = {reference_vehicle_to_other_geodesic,
314 other_vehicle_to_reference_geodesic,
315 inter_geodesic_distance,
316 inter_bbox_distance};
321 return comparision_result;
326 const uint64_t reference_junction_look_ahead_index) {
329 float available_distance_margin = std::numeric_limits<float>::infinity();
337 cg::Vector3D reference_to_other = other_location - reference_location;
343 cg::Vector3D other_to_reference = reference_location - other_location;
349 float inter_vehicle_distance = cg::Math::DistanceSquared(reference_location, other_location);
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);
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);
364 bool ego_inside_junction = closest_point->CheckJunction();
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();
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))) {
384 bool ego_angular_priority = reference_heading_to_other_dot< cg::Math::Dot(other_heading, other_to_reference);
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;
392 if (geodesic_path_bbox_touching && (yield_pre_crash || yield_post_crash)) {
399 -
static_cast<double>(specific_distance_margin), 0.0));
438 return {hazard, available_distance_margin};
#define SQUARE(a)
This file contains various constants used in traffic manager arranged into sensible namespaces for re...
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 ¶meters, CollisionFrame &output_array, RandomGenerator &random_device)
GeodesicBoundaryMap geodesic_boundary_map
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
移除参与者方法。
CollisionFrame & output_array
LocationVector GetBoundary(const ActorId actor_id)
float GetBoundingBoxExtention(const ActorId actor_id)
RandomGenerator & random_device
const TrackTraffic & track_traffic
const BufferMap & buffer_map
void Update(const unsigned long index) override
更新方法。
CollisionLockMap collision_locks
GeometryComparisonMap geometry_cache
const Parameters & parameters
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
static const float LOCKING_DISTANCE_PADDING
static const float VEL_EXT_FACTOR
static const float OVERLAP_THRESHOLD
static const float BOUNDARY_EXTENSION_MINIMUM
static const float COLLISION_RADIUS_RATE
static const float COS_10_DEGREES
static const float VERTICAL_OVERLAP_THRESHOLD
static const float SQUARE_ROOT_OF_TWO
static const float WALKER_TIME_EXTENSION
static const float MIN_REFERENCE_DISTANCE
static const float COLLISION_RADIUS_MIN
static const float COLLISION_RADIUS_STOP
static const float MAX_LOCKING_EXTENSION
static const float JUNCTION_LOOK_AHEAD
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
根据目标点距离从路点缓冲区返回路点信息
float available_distance_margin
double distance_to_lead_vehicle
double initial_lock_distance
double reference_vehicle_to_other_geodesic
double inter_geodesic_distance
double other_vehicle_to_reference_geodesic
double inter_bbox_distance