From 379516151dfa44780f83a9536964ab9614324801 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Fri, 24 May 2024 20:57:31 +0900
Subject: [PATCH] update for checkConsistency()

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 planning/obstacle_cruise_planner/src/node.cpp | 83 +++++++++----------
 1 file changed, 40 insertions(+), 43 deletions(-)

diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
index 7c201e1d4e4b4..b8f345b6cee7f 100644
--- a/planning/obstacle_cruise_planner/src/node.cpp
+++ b/planning/obstacle_cruise_planner/src/node.cpp
@@ -815,12 +815,6 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
   }
   slow_down_condition_counter_.removeCounterUnlessUpdated();
 
-  std::sort(
-    stop_obstacles.begin(), stop_obstacles.end(),
-    [](const StopObstacle & o1, const StopObstacle & o2) {
-      return o1.dist_to_collide_on_decimated_traj < o2.dist_to_collide_on_decimated_traj;
-    });
-    
   // Check target obstacles' consistency
   checkConsistency(objects.header.stamp, objects, stop_obstacles);
 
@@ -1381,50 +1375,53 @@ void ObstacleCruisePlannerNode::checkConsistency(
   const rclcpp::Time & current_time, const PredictedObjects & predicted_objects,
   std::vector<StopObstacle> & stop_obstacles)
 {
-  const auto current_closest_stop_obstacle =
-    obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles);
-
-  if (!prev_closest_stop_obstacle_ptr_) {
-    if (current_closest_stop_obstacle) {
-      prev_closest_stop_obstacle_ptr_ =
-        std::make_shared<StopObstacle>(*current_closest_stop_obstacle);
+  std::vector<StopObstacle> current_closest_stop_obstacles{};
+  std::sort(
+    stop_obstacles.begin(), stop_obstacles.end(),
+    [](const StopObstacle & o1, const StopObstacle & o2) {
+      return o1.dist_to_collide_on_decimated_traj < o2.dist_to_collide_on_decimated_traj;
+    });
+  for (const auto & stop_obstacle : stop_obstacles) {
+    if (std::none_of(
+          current_closest_stop_obstacles.begin(), current_closest_stop_obstacles.end(),
+          [&stop_obstacle](StopObstacle & cco) {
+            return cco.classification.label == stop_obstacle.classification.label;
+          })) {
+      current_closest_stop_obstacles.push_back(stop_obstacle);
     }
-    return;
   }
 
-  const auto predicted_object_itr = std::find_if(
-    predicted_objects.objects.begin(), predicted_objects.objects.end(),
-    [&](PredictedObject predicted_object) {
-      return tier4_autoware_utils::toHexString(predicted_object.object_id) ==
-             prev_closest_stop_obstacle_ptr_->uuid;
-    });
-  // If previous closest obstacle disappear from the perception result, do nothing anymore.
-  if (predicted_object_itr == predicted_objects.objects.end()) {
-    return;
-  }
+  static std::vector<StopObstacle> prev_closest_stop_obstacles{};
+  for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles) {
+    const auto predicted_object_itr = std::find_if(
+      predicted_objects.objects.begin(), predicted_objects.objects.end(),
+      [&](PredictedObject predicted_object) {
+        return tier4_autoware_utils::toHexString(predicted_object.object_id) ==
+               prev_closest_stop_obstacle_ptr_->uuid;
+      });
+    // If previous closest obstacle disappear from the perception result, do nothing anymore.
+    if (predicted_object_itr == predicted_objects.objects.end()) {
+      continue;
+    }
 
-  const auto is_disappeared_from_stop_obstacle = std::none_of(
-    stop_obstacles.begin(), stop_obstacles.end(),
-    [&](const auto & obstacle) { return obstacle.uuid == prev_closest_stop_obstacle_ptr_->uuid; });
-  if (is_disappeared_from_stop_obstacle) {
-    // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop"
-    // condition is satisfied
-    const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds();
-    if (
-      predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x <
-        behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise &&
-      elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) {
-      stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_);
-      return;
+    const auto is_disappeared_from_stop_obstacle = std::none_of(
+      stop_obstacles.begin(), stop_obstacles.end(),
+      [&](const auto & obstacle) { return obstacle.uuid == prev_closest_stop_obstacle.uuid; });
+    if (is_disappeared_from_stop_obstacle) {
+      // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop"
+      // condition is satisfied
+      const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds();
+      if (
+        predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x <
+          behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise &&
+        elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) {
+        stop_obstacles.push_back(prev_closest_stop_obstacle);
+        current_closest_stop_obstacles.push_back(prev_closest_stop_obstacle);
+      }
     }
   }
 
-  if (current_closest_stop_obstacle) {
-    prev_closest_stop_obstacle_ptr_ =
-      std::make_shared<StopObstacle>(*current_closest_stop_obstacle);
-  } else {
-    prev_closest_stop_obstacle_ptr_ = nullptr;
-  }
+  prev_closest_stop_obstacles = std::move(current_closest_stop_obstacles);
 }
 
 bool ObstacleCruisePlannerNode::isObstacleCrossing(