From abb2305fe44569d46a302e917fb7b777ac552a8e Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 25 Nov 2024 10:49:50 +0900
Subject: [PATCH] fix: limit recursive search

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

refactor: improve efficiency of recursiveSearch function
---
 .../scan_ground_filter/grid_ground_filter.cpp    | 16 ++++++++++++++--
 .../scan_ground_filter/grid_ground_filter.hpp    |  2 ++
 2 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.cpp
index 83e50fa64a8eb..46a2c5d3b0500 100644
--- a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.cpp
+++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.cpp
@@ -50,6 +50,18 @@ void GridGroundFilter::preprocess()
 bool GridGroundFilter::recursiveSearch(
   const int check_idx, const int search_cnt, std::vector<int> & idx) const
 {
+  // set the maximum search count
+  constexpr size_t count_limit = 1023;
+  return recursiveSearch(check_idx, search_cnt, idx, count_limit);
+}
+
+bool GridGroundFilter::recursiveSearch(
+  const int check_idx, const int search_cnt, std::vector<int> & idx, size_t & count) const
+{
+  if (count == 0) {
+    return false;
+  }
+  count -= 1;
   // recursive search
   if (check_idx < 0) {
     return false;
@@ -61,10 +73,10 @@ bool GridGroundFilter::recursiveSearch(
   if (check_cell.has_ground_) {
     // the cell has ground, add the index to the list, and search previous cell
     idx.push_back(check_idx);
-    return recursiveSearch(check_cell.scan_grid_root_idx_, search_cnt - 1, idx);
+    return recursiveSearch(check_cell.scan_grid_root_idx_, search_cnt - 1, idx, count);
   }
   // if the cell does not have ground, search previous cell
-  return recursiveSearch(check_cell.scan_grid_root_idx_, search_cnt, idx);
+  return recursiveSearch(check_cell.scan_grid_root_idx_, search_cnt, idx, count);
 }
 
 void GridGroundFilter::fitLineFromGndGrid(const std::vector<int> & idx, float & a, float & b) const
diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.hpp
index 766af3e5528c9..88a94790adbb3 100644
--- a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.hpp
+++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.hpp
@@ -199,6 +199,8 @@ class GridGroundFilter
   std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_;
 
   bool recursiveSearch(const int check_idx, const int search_cnt, std::vector<int> & idx) const;
+  bool recursiveSearch(
+    const int check_idx, const int search_cnt, std::vector<int> & idx, size_t & count) const;
   void fitLineFromGndGrid(const std::vector<int> & idx, float & a, float & b) const;
 
   void convert();