classify too_late_detect_objects and misjudge_objects

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

planning/behavior_velocity_intersection_module/src/object_manager.cpp

@@ -233,7 +233,7 @@ std::optional<intersection::CollisionInterval> findPassageInterval(
   const autoware_auto_perception_msgs::msg::Shape & shape,
   const lanelet::BasicPolygon2d & ego_lane_poly,
   const std::optional<lanelet::ConstLanelet> & first_attention_lane_opt,
-  const std::optional<lanelet::ConstLanelet> & second_attention_lane_opt)
+  [[maybe_unused]] const std::optional<lanelet::ConstLanelet> & second_attention_lane_opt)
 {
   const auto first_itr = std::adjacent_find(
     predicted_path.path.cbegin(), predicted_path.path.cend(), [&](const auto & a, const auto & b) {
@@ -261,15 +261,6 @@ std::optional<intersection::CollisionInterval> findPassageInterval(
     static_cast<double>(exit_idx) * rclcpp::Duration(predicted_path.time_step).seconds();
   const auto [lane_position, lane_id] =
     [&]() -> std::pair<intersection::CollisionInterval::LanePosition, lanelet::Id> {
-    if (second_attention_lane_opt) {
-      if (lanelet::geometry::inside(
-            second_attention_lane_opt.value(),
-            lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) {
-        return std::make_pair(
-          intersection::CollisionInterval::LanePosition::SECOND,
-          second_attention_lane_opt.value().id());
-      }
-    }
     if (first_attention_lane_opt) {
       if (lanelet::geometry::inside(
             first_attention_lane_opt.value(),

planning/behavior_velocity_intersection_module/src/object_manager.hpp

@@ -62,7 +62,6 @@ struct CollisionInterval
 {
   enum LanePosition {
     FIRST,
-    SECOND,
     ELSE,
   };
   LanePosition lane_position{LanePosition::ELSE};
@@ -187,6 +186,16 @@ class ObjectInfo
     decision_at_2nd_pass_judge_line_passage_ = knowledge;
   }
 
+  const std::optional<CollisionKnowledge> & decision_at_1st_pass_judge_line_passage() const
+  {
+    return decision_at_1st_pass_judge_line_passage_;
+  }
+
+  const std::optional<CollisionKnowledge> & decision_at_2nd_pass_judge_line_passage() const
+  {
+    return decision_at_2nd_pass_judge_line_passage_;
+  }
+
 private:
   const std::string uuid_str;
   autoware_auto_perception_msgs::msg::PredictedObject predicted_object_;

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -224,7 +224,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line,
     safely_passed_2nd_judge_line);
 
-  const auto [has_collision, collision_position] = detectCollision();
+  const auto [has_collision, collision_position] =
+    detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line);
   if (is_permanent_go_) {
     if (has_collision) {
       // TODO(Mamoru Sobue): diagnosis

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -680,7 +680,8 @@ class IntersectionModule : public SceneModuleInterface
   /**
    * @brief return if collision is detected and the collision position
    */
-  std::pair<bool, intersection::CollisionInterval::LanePosition> detectCollision();
+  std::pair<bool, intersection::CollisionInterval::LanePosition> detectCollision(
+    const bool is_over_1st_pass_judge_line, const bool is_over_2nd_pass_judge_line);
 
   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

@@ -151,6 +151,10 @@ void IntersectionModule::updateObjectInfoManagerCollision(
   debug_data_.ego_lane = ego_lane.polygon3d();
   const auto ego_poly = ego_lane.polygon2d().basicPolygon();
 
+  // ==========================================================================================
+  // dynamically change TTC margin according to traffic light color to gradually relax from green to
+  // red
+  // ==========================================================================================
   const auto [collision_start_margin_time, collision_end_margin_time] = [&]() {
     if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) {
       return std::make_pair(
@@ -365,26 +369,71 @@ IntersectionModule::isGreenPseudoCollisionStatus(
   return std::nullopt;
 }
 
-std::pair<bool, intersection::CollisionInterval::LanePosition> IntersectionModule::detectCollision()
+std::pair<bool, intersection::CollisionInterval::LanePosition> IntersectionModule::detectCollision(
+  const bool is_over_1st_pass_judge_line, const bool is_over_2nd_pass_judge_line)
 {
   // ==========================================================================================
   // 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};
+  bool collision_at_first_lane = false;
+  bool collision_at_non_first_lane = false;
+
+  // ==========================================================================================
+  // find the objects which is judges as UNSAFE after ego passed pass judge lines.
+  //
+  // misjudge_objects are those that were once judged as safe when ego passed the pass judge line
+  //
+  // too_late_detect objects are those that (1) were not detected when ego passed the pass judge
+  // line (2) were judged as dangerous at the same time when ego passed the pass judge, which are
+  // expected to have been detected in the prior iteration because ego could have judged as UNSAFE
+  // in the prior iteration
+  // ==========================================================================================
+  std::vector<std::shared_ptr<intersection::ObjectInfo>> misjudge_objects;
+  std::vector<std::shared_ptr<intersection::ObjectInfo>> too_late_detect_objects;
   for (const auto & object_info : object_info_manager_.attentionObjects()) {
-    if (const auto unsafe_info = object_info->is_unsafe(); unsafe_info) {
+    if (!object_info->is_unsafe()) {
+      continue;
+    }
+    const auto & unsafe_info = object_info->is_unsafe().value();
+    if (unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) {
+      collision_at_first_lane = true;
+    } else {
+      collision_at_non_first_lane = true;
+    }
+    if (is_over_1st_pass_judge_line) {
+      const auto & decision_at_1st_pass_judge_opt =
+        object_info->decision_at_1st_pass_judge_line_passage();
+      if (!decision_at_1st_pass_judge_opt) {
+        too_late_detect_objects.push_back(object_info);
+      }
+      const auto & decision_at_1st_pass_judge = decision_at_1st_pass_judge_opt.value();
+      if (
+        decision_at_1st_pass_judge.safe || decision_at_1st_pass_judge.safe_under_traffic_control) {
+        misjudge_objects.push_back(object_info);
+      } else {
+        too_late_detect_objects.push_back(object_info);
+      }
+    }
+    if (is_over_2nd_pass_judge_line) {
+      const auto & decision_at_2nd_pass_judge_opt =
+        object_info->decision_at_2nd_pass_judge_line_passage();
+      if (!decision_at_2nd_pass_judge_opt) {
+        too_late_detect_objects.push_back(object_info);
+      }
+      const auto & decision_at_2nd_pass_judge = decision_at_2nd_pass_judge_opt.value();
       if (
-        unsafe_info.value().lane_position == intersection::CollisionInterval::LanePosition::FIRST) {
-        return {true, intersection::CollisionInterval::LanePosition::FIRST};
+        decision_at_2nd_pass_judge.safe || decision_at_2nd_pass_judge.safe_under_traffic_control) {
+        misjudge_objects.push_back(object_info);
       } else {
-        collision_at_non_first_lane_opt = unsafe_info.value().lane_position;
+        too_late_detect_objects.push_back(object_info);
       }
     }
   }
-  if (collision_at_non_first_lane_opt) {
-    return {true, collision_at_non_first_lane_opt.value()};
+  if (collision_at_first_lane) {
+    return {true, intersection::CollisionInterval::FIRST};
+  } else if (collision_at_non_first_lane) {
+    return {true, intersection::CollisionInterval::ELSE};
   }
   return {false, intersection::CollisionInterval::ELSE};
 }