check the collision position at collision detection

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_velocity_intersection_module/src/object_manager.hpp

@@ -126,12 +126,15 @@ class ObjectInfo
     return predicted_object_;
   };
 
-  bool is_safe() const
+  std::optional<CollisionInterval> is_unsafe() const
   {
     if (safe_under_traffic_control_) {
-      return true;
+      return std::nullopt;
     }
-    return !unsafe_interval_.has_value();
+    if (!unsafe_interval_) {
+      return std::nullopt;
+    }
+    return unsafe_interval_;
   }
 
   /**

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -224,20 +224,20 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   updateObjectInfoManagerArea();
 
   // ==========================================================================================
-  // yield stuck detection
+  // occlusion_status is type of occlusion,
+  // is_occlusion_cleared_with_margin indicates if occlusion is physically detected
+  // is_occlusion_state indicates if occlusion is detected. OR occlusion is not detected but RTC for
+  // intersection_occlusion is disapproved, which means ego is virtually occluded
+  //
+  // so is_occlusion_cleared_with_margin should be sent to RTC as module decision
+  // and is_occlusion_status should be only used to decide ego action
+  // !is_occlusion_state == !physically_occluded && !externally_occluded, so even if occlusion is
+  // not detected but not approved, SAFE is not sent.
   // ==========================================================================================
-  const auto is_yield_stuck_status =
-    isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines);
-  if (is_yield_stuck_status) {
-    return is_yield_stuck_status.value();
-  }
-
   const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] =
     getOcclusionStatus(traffic_prioritized_level, interpolated_path_info);
 
-  // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd
-  // pass judge line respectively, ego should go
-  [[maybe_unused]] const auto
+  const auto
     [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line, safely_passed_1st_judge_line,
      safely_passed_2nd_judge_line] =
       isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines);
@@ -254,13 +254,28 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line,
     safely_passed_2nd_judge_line);
 
-  const bool has_collision = hasCollision();
+  const auto [has_collision, collision_position] = detectCollision();
   if (is_permanent_go_) {
-    // TODO(Mamoru Sobue): diagnosis if has_collision
-    return intersection::Indecisive{"over pass judge lines"};
+    if (has_collision) {
+      // TODO(Mamoru Sobue): diagnosis
+      return intersection::Indecisive{
+        "ego is over the pass judge lines and collision is detected. need acceleration to keep "
+        "safe."};
+    }
+    return intersection::Indecisive{"over pass judge lines and collision is not detected"};
+  }
+  /*
+  const bool collision_on_1st_attention_lane =
+    has_collision && collision_position == intersection::CollisionInterval::LanePosition::FIRST;
+  if (
+    is_over_1st_pass_judge_line && !is_over_2nd_pass_judge_line &&
+    collision_on_1st_attention_lane) {
+    // TODO(Mamoru Sobue): diagnosis
+    return intersection::Indecisive{
+      "ego is already over the 1st pass judge line although still before the 2nd pass judge line, "
+      "but collision is detected on the first attention lane"};
   }
-  // if (is_over_1st_pass_judge_line && !is_over_2nd_pass_judge_line && collision_on_1st_lane)
-  // TODO(Mamoru Sobue): diagnosis
+  */
 
   const bool is_over_default_stopline = util::isOverTargetIndex(
     *path, closest_idx, planner_data_->current_odometry->pose, default_stopline_idx);
@@ -275,6 +290,15 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     return is_green_pseudo_collision_status.value();
   }
 
+  // ==========================================================================================
+  // yield stuck detection
+  // ==========================================================================================
+  const auto is_yield_stuck_status =
+    isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines);
+  if (is_yield_stuck_status) {
+    return is_yield_stuck_status.value();
+  }
+
   collision_state_machine_.setStateWithMarginTime(
     has_collision ? StateMachine::State::STOP : StateMachine::State::GO,
     logger_.get_child("collision state_machine"), *clock_);

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -678,9 +678,9 @@ class IntersectionModule : public SceneModuleInterface
     const intersection::IntersectionStopLines & intersection_stoplines);
 
   /**
-   * @brief check collision
+   * @brief return if collision is detected and the collision position
    */
-  bool hasCollision();
+  std::pair<bool, intersection::CollisionInterval::LanePosition> detectCollision();
 
   std::optional<size_t> checkAngleForTargetLanelets(
     const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,

planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp

@@ -387,14 +387,28 @@ IntersectionModule::isGreenPseudoCollisionStatus(
   return std::nullopt;
 }
 
-bool IntersectionModule::hasCollision()
+std::pair<bool, intersection::CollisionInterval::LanePosition> IntersectionModule::detectCollision()
 {
+  // ==========================================================================================
+  // if collision is detected for multiple objects, we prioritize collision on the first attention
+  // lanelet
+  // ==========================================================================================
+  std::optional<intersection::CollisionInterval::LanePosition> collision_at_non_first_lane_opt{
+    std::nullopt};
   for (const auto & object_info : object_info_manager_.attentionObjects()) {
-    if (!object_info->is_safe()) {
-      return true;
+    if (const auto unsafe_info = object_info->is_unsafe(); unsafe_info) {
+      if (
+        unsafe_info.value().lane_position == intersection::CollisionInterval::LanePosition::FIRST) {
+        return {true, unsafe_info.value().lane_position};
+      } else {
+        collision_at_non_first_lane_opt = unsafe_info.value().lane_position;
+      }
     }
   }
-  return false;
+  if (collision_at_non_first_lane_opt) {
+    return {true, collision_at_non_first_lane_opt.value()};
+  }
+  return {false, intersection::CollisionInterval::ELSE};
 }
 
 /*