From 98d4b0bc1f16c3d9253214cda6d28f7bf784b943 Mon Sep 17 00:00:00 2001
From: danielsanchezaran <daniel.sanchez@tier4.jp>
Date: Fri, 4 Oct 2024 11:25:45 +0900
Subject: [PATCH] fix(autonomous_emergency_braking): add sanity checks (#8998)
 (#1573)

feat(autonomous_emergency_braking): add sanity chackes (#8998)

add sanity chackes

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
 .../autoware_autonomous_emergency_braking/src/node.cpp    | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp
index 52b4d22f2e40d..8fad2533bf3ab 100644
--- a/control/autoware_autonomous_emergency_braking/src/node.cpp
+++ b/control/autoware_autonomous_emergency_braking/src/node.cpp
@@ -631,6 +631,9 @@ std::optional<Path> AEB::generateEgoPath(const Trajectory & predicted_traj)
 std::vector<Polygon2d> AEB::generatePathFootprint(
   const Path & path, const double extra_width_margin)
 {
+  if (path.empty()) {
+    return {};
+  }
   std::vector<Polygon2d> polygons;
   for (size_t i = 0; i < path.size() - 1; ++i) {
     polygons.push_back(
@@ -643,7 +646,7 @@ void AEB::createObjectDataUsingPredictedObjects(
   const Path & ego_path, const std::vector<Polygon2d> & ego_polys,
   std::vector<ObjectData> & object_data_vector)
 {
-  if (predicted_objects_ptr_->objects.empty()) return;
+  if (predicted_objects_ptr_->objects.empty() || ego_polys.empty()) return;
 
   const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity;
   const auto & objects = predicted_objects_ptr_->objects;
@@ -814,6 +817,9 @@ void AEB::createObjectDataUsingPointCloudClusters(
 void AEB::cropPointCloudWithEgoFootprintPath(
   const std::vector<Polygon2d> & ego_polys, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects)
 {
+  if (ego_polys.empty()) {
+    return;
+  }
   PointCloud::Ptr full_points_ptr(new PointCloud);
   pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr);
   // Create a Point cloud with the points of the ego footprint