From eb9e1e99b9b3a694f01c9ad88fee2f4964c32c72 Mon Sep 17 00:00:00 2001 From: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com> Date: Fri, 22 Dec 2023 04:35:54 +0300 Subject: [PATCH] fix(AEB): ego to object distance calculation logic with predicted path (#5930) init commit Signed-off-by: ismetatabay --- .../autonomous_emergency_braking/src/node.cpp | 28 +++++++++++-------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index e88a5ee833612..960aadf226b70 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -358,25 +358,29 @@ bool AEB::hasCollision( const double current_v, const Path & ego_path, const std::vector & objects) { // calculate RSS - const auto current_p = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + const auto current_p = tier4_autoware_utils::createPoint( + ego_path[0].position.x, ego_path[0].position.y, ego_path[0].position.z); const double & t = t_response_; for (const auto & obj : objects) { const double & obj_v = obj.velocity; const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; - const double dist_ego_to_object = - motion_utils::calcSignedArcLength(ego_path, current_p, obj.position) - - vehicle_info_.max_longitudinal_offset_m; - if (dist_ego_to_object < rss_dist) { - // collision happens - ObjectData collision_data = obj; - collision_data.rss = rss_dist; - collision_data.distance_to_object = dist_ego_to_object; - collision_data_keeper_.update(collision_data); - return true; + + // check the object is front the ego or not + if ((obj.position.x - ego_path[0].position.x) > 0) { + const double dist_ego_to_object = + motion_utils::calcSignedArcLength(ego_path, current_p, obj.position) - + vehicle_info_.max_longitudinal_offset_m; + if (dist_ego_to_object < rss_dist) { + // collision happens + ObjectData collision_data = obj; + collision_data.rss = rss_dist; + collision_data.distance_to_object = dist_ego_to_object; + collision_data_keeper_.update(collision_data); + return true; + } } } - return false; }