From 0ff262f797d5b001add5ea8fbdd8b26b6e4d84fb Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 6 Nov 2024 18:35:40 +0900 Subject: [PATCH] fix(autonomous_emergency_braking): solve issue with arc length (#9247) * solve issue with arc length Signed-off-by: Daniel Sanchez * fix problem with points one vehicle apart from path Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- control/autoware_autonomous_emergency_braking/src/node.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 5d12031f9039b..c785ab661060d 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -900,11 +900,15 @@ void AEB::getClosestObjectsOnPath( return autoware::universe_utils::createPoint(p.x, p.y, p.z); }(); + const auto path_length = autoware::motion_utils::calcArcLength(ego_path); for (const auto & p : *points_belonging_to_cluster_hulls) { const auto obj_position = autoware::universe_utils::createPoint(p.x, p.y, p.z); const double obj_arc_length = autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); - if (std::isnan(obj_arc_length)) continue; + if ( + std::isnan(obj_arc_length) || + obj_arc_length > path_length + vehicle_info_.max_longitudinal_offset_m) + continue; // calculate the lateral offset between the ego vehicle and the object const double lateral_offset =