fix(autonomous_emergency_braking): solve issue with arc length (autow…

…arefoundation#9247) * solve issue with arc length Signed-off-by: Daniel Sanchez <[email protected]> * fix problem with points one vehicle apart from path Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]>
tier4 · Nov 14, 2024 · 32808d4 · 32808d4
1 parent 25aeac1
commit 32808d4
Showing 1 changed file with 5 additions and 1 deletion.
diff --git 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 =