feat(goal_planner): add diciding status considering recognition stabi…

…lity time

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -230,6 +230,13 @@ struct LastApprovalData
   Pose pose{};
 };
 
+enum class DecidingPathStatus {
+  NOT_DECIDED,
+  DECIDING,
+  DECIDED,
+};
+using DecidingPathStatusWithStamp = std::pair<DecidingPathStatus, rclcpp::Time>;
+
 struct PreviousPullOverData
 {
   struct SafetyStatus
@@ -242,12 +249,13 @@ struct PreviousPullOverData
   {
     found_path = false;
     safety_status = SafetyStatus{};
-    has_decided_path = false;
+    deciding_path_status = DecidingPathStatusWithStamp{};
   }
 
   bool found_path{false};
   SafetyStatus safety_status{};
-  bool has_decided_path{false};
+  // bool has_decided_path{false};
+  DecidingPathStatusWithStamp deciding_path_status{};
 };
 
 // store stop_pose_ pointer with reason string
@@ -439,6 +447,7 @@ class GoalPlannerModule : public SceneModuleInterface
   bool needPathUpdate(const double path_update_duration) const;
   bool isStuck();
   bool hasDecidedPath() const;
+  DecidingPathStatus checkDecidingPathStatus() const;
   void decideVelocity();
   bool foundPullOverPath() const;
   void updateStatus(const BehaviorModuleOutput & output);

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -902,22 +902,44 @@ void GoalPlannerModule::updateSteeringFactor(
 }
 
 bool GoalPlannerModule::hasDecidedPath() const
+{
+  return checkDecidingPathStatus() == DecidingPathStatus::DECIDED;
+}
+
+DecidingPathStatus GoalPlannerModule::checkDecidingPathStatus() const
 {
   // Once this function returns true, it will continue to return true thereafter
-  if (prev_data_.has_decided_path) {
-    return true;
+  if (prev_data_.deciding_path_status.first == DecidingPathStatus::DECIDED) {
+    return DecidingPathStatus::DECIDED;
   }
 
   // if path is not safe, not decided
   if (!thread_safe_data_.foundPullOverPath()) {
-    return false;
+    return DecidingPathStatus::NOT_DECIDED;
   }
 
   // If it is dangerous before approval, do not determine the path.
   // This eliminates a unsafe path to be approved
   if (
     parameters_->safety_check_params.enable_safety_check && !isSafePath().first && !isActivated()) {
-    return false;
+    return DecidingPathStatus::NOT_DECIDED;
+  }
+
+  const auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath();
+  if (prev_data_.deciding_path_status.first == DecidingPathStatus::DECIDING) {
+    if (
+      parameters_->use_object_recognition &&
+      checkObjectsCollision(
+        current_path, parameters_->object_recognition_collision_check_margin,
+        true /*update_debug_data*/)) {
+      return DecidingPathStatus::NOT_DECIDED;
+    }
+
+    if ((clock_->now() - prev_data_.deciding_path_status.second).seconds() > 1.0) {
+      return DecidingPathStatus::DECIDED;
+    }
+
+    return DecidingPathStatus::DECIDING;
   }
 
   // if ego is sufficiently close to the start of the nearest candidate path, the path is decided
@@ -932,7 +954,11 @@ bool GoalPlannerModule::hasDecidedPath() const
     thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position,
     ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position,
     start_pose_segment_idx);
-  return dist_to_parking_start_pose < parameters_->decide_path_distance;
+  if (dist_to_parking_start_pose > parameters_->decide_path_distance) {
+    return DecidingPathStatus::NOT_DECIDED;
+  }
+
+  return DecidingPathStatus::DECIDING;
 }
 
 void GoalPlannerModule::decideVelocity()
@@ -993,7 +1019,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
     return getPreviousModuleOutput();
   }
 
-  if (!hasDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) {
+  if (
+    checkDecidingPathStatus() == DecidingPathStatus::NOT_DECIDED &&
+    needPathUpdate(1.0 /*path_update_duration*/)) {
     // if the final path is not decided and enough time has passed since last path update,
     // select safe path from lane parking pull over path candidates
     // and set it to thread_safe_data_
@@ -1083,7 +1111,8 @@ void GoalPlannerModule::updatePreviousData()
   // this is used to determine whether to generate a new stop path or keep the current stop path.
   prev_data_.found_path = thread_safe_data_.foundPullOverPath();
 
-  prev_data_.has_decided_path = hasDecidedPath();
+  prev_data_.deciding_path_status.first = checkDecidingPathStatus();
+  prev_data_.deciding_path_status.second = clock_->now();
 
   // This is related to safety check, so if it is disabled, return here.
   if (!parameters_->safety_check_params.enable_safety_check) {