From 723ad9bfe828d933293241f8682cbd1eaed3b915 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 18 Jun 2024 12:04:20 +0900 Subject: [PATCH] introduce early break to reduce computation time Signed-off-by: Daniel Sanchez --- control/autoware_autonomous_emergency_braking/src/node.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 8351daa5cce0c..b12cb2f81da87 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -665,6 +665,7 @@ void AEB::createObjectDataUsingPredictedObjects( if (collision_points_bg.empty()) continue; // Create an object for each intersection point + bool collision_points_added{false}; for (const auto & collision_point : collision_points_bg) { const auto obj_position = tier4_autoware_utils::createPoint(collision_point.x(), collision_point.y(), 0.0); @@ -685,7 +686,11 @@ void AEB::createObjectDataUsingPredictedObjects( obj.velocity = (obj_tangent_velocity > 0.0) ? obj_tangent_velocity : 0.0; obj.distance_to_object = std::abs(dist_ego_to_object); object_data_vector.push_back(obj); + collision_points_added = true; } + // The ego polygons are in order, so the first intersection points found are the closest + // points. It is not necessary to continue iterating the ego polys for the same object. + if (collision_points_added) break; } }); }