From 7d609fa0a1e05657078db422a60391e73c488af2 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 7 Nov 2024 15:41:29 +0900 Subject: [PATCH] fix(autonomous_emergency_braking): solve issue with arc length (#9247) (#1627) * solve issue with arc length * fix problem with points one vehicle apart from path --------- 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 =