From 5eacb6fd391a509a9a36733aca7d700820d68b2d Mon Sep 17 00:00:00 2001
From: danielsanchezaran <daniel.sanchez@tier4.jp>
Date: Tue, 1 Oct 2024 18:19:49 +0900
Subject: [PATCH] feat(autonomous_emergency_braking): add sanity chackes
 (#8998)

add sanity chackes

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
 .../src/node.cpp                                | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp
index 82d9de811b444..32f48b337efbf 100644
--- a/control/autoware_autonomous_emergency_braking/src/node.cpp
+++ b/control/autoware_autonomous_emergency_braking/src/node.cpp
@@ -446,7 +446,9 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
     const auto ego_polys = generatePathFootprint(path, expand_width_);
     std::vector<ObjectData> objects;
     // Crop out Pointcloud using an extra wide ego path
-    if (use_pointcloud_data_ && !points_belonging_to_cluster_hulls->empty()) {
+    if (
+      use_pointcloud_data_ && points_belonging_to_cluster_hulls &&
+      !points_belonging_to_cluster_hulls->empty()) {
       const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp;
       getClosestObjectsOnPath(
         path, ego_polys, current_time, points_belonging_to_cluster_hulls, objects);
@@ -667,6 +669,9 @@ void AEB::generatePathFootprint(
   const Path & path, const double extra_width_margin, std::vector<Polygon2d> & polygons)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  if (path.empty()) {
+    return;
+  }
   for (size_t i = 0; i < path.size() - 1; ++i) {
     polygons.push_back(
       createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin));
@@ -676,8 +681,11 @@ void AEB::generatePathFootprint(
 std::vector<Polygon2d> AEB::generatePathFootprint(
   const Path & path, const double extra_width_margin)
 {
-  std::vector<Polygon2d> polygons;
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  if (path.empty()) {
+    return {};
+  }
+  std::vector<Polygon2d> polygons;
   for (size_t i = 0; i < path.size() - 1; ++i) {
     polygons.push_back(
       createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin));
@@ -690,7 +698,7 @@ void AEB::createObjectDataUsingPredictedObjects(
   std::vector<ObjectData> & object_data_vector)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
-  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;
@@ -863,6 +871,9 @@ void AEB::cropPointCloudWithEgoFootprintPath(
   const std::vector<Polygon2d> & ego_polys, PointCloud::Ptr filtered_objects)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  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