save the CollisionKnowledge at passage of 1st/2nd judge line

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

planning/behavior_velocity_intersection_module/src/object_manager.cpp

@@ -86,10 +86,11 @@ void ObjectInfo::initialize(
 
 void ObjectInfo::update_safety(
   const std::optional<CollisionInterval> & unsafe_interval,
-  const std::optional<CollisionInterval> & safe_interval)
+  const std::optional<CollisionInterval> & safe_interval, const bool safe_under_traffic_control)
 {
   unsafe_interval_ = unsafe_interval;
   safe_interval_ = safe_interval;
+  safe_under_traffic_control_ = safe_under_traffic_control;
 }
 
 void ObjectInfo::calc_dist_to_stopline()

planning/behavior_velocity_intersection_module/src/object_manager.hpp

@@ -80,9 +80,19 @@ struct CollisionInterval
 
 struct CollisionKnowledge
 {
+  //! the time when the expected collision is judged
   rclcpp::Time stamp;
-  CollisionInterval interval;
+
+  //! if judged as SAFE/UNSAFE
   bool safe{false};
+
+  //! if judged as SAFE given traffic control
+  bool safe_under_traffic_control{false};
+
+  //! if !safe, this has value, and it safe, this maybe null if the predicted path does not
+  //! intersect with ego path
+  std::optional<CollisionInterval> interval{std::nullopt};
+
   double observed_velocity;
   /**
    * diag format:
@@ -116,7 +126,13 @@ class ObjectInfo
     return predicted_object_;
   };
 
-  const std::optional<CollisionInterval> & unsafe_interval() const { return unsafe_interval_; }
+  bool is_safe() const
+  {
+    if (safe_under_traffic_control_) {
+      return true;
+    }
+    return !unsafe_interval_.has_value();
+  }
 
   /**
    * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline
@@ -131,7 +147,8 @@ class ObjectInfo
    */
   void update_safety(
     const std::optional<CollisionInterval> & unsafe_interval_opt,
-    const std::optional<CollisionInterval> & safe_interval_opt);
+    const std::optional<CollisionInterval> & safe_interval_opt,
+    const bool safe_under_traffic_control);
 
   /**
    * @brief find the estimated position of the object in the past
@@ -157,6 +174,16 @@ class ObjectInfo
    */
   bool before_stopline_by(const double margin) const;
 
+  void setDecisionAt1stPassJudgeLinePassage(const CollisionKnowledge & knowledge)
+  {
+    decision_at_1st_pass_judge_line_passage_ = knowledge;
+  }
+
+  void setDecisionAt2ndPassJudgeLinePassage(const CollisionKnowledge & knowledge)
+  {
+    decision_at_2nd_pass_judge_line_passage_ = knowledge;
+  }
+
 private:
   const std::string uuid_str;
   autoware_auto_perception_msgs::msg::PredictedObject predicted_object_;
@@ -176,8 +203,11 @@ class ObjectInfo
   //! store the information if judged as SAFE
   std::optional<CollisionInterval> safe_interval_{std::nullopt};
 
-  std::optional<CollisionKnowledge> decision_at_1st_pass_judge_line_passage{std::nullopt};
-  std::optional<CollisionKnowledge> decision_at_2nd_pass_judge_line_passage{std::nullopt};
+  //! true if the object is judged as negligible given traffic light color
+  bool safe_under_traffic_control_{false};
+
+  std::optional<CollisionKnowledge> decision_at_1st_pass_judge_line_passage_{std::nullopt};
+  std::optional<CollisionKnowledge> decision_at_2nd_pass_judge_line_passage_{std::nullopt};
 
   /**
    * @brief calculate/update the distance to corresponding stopline
@@ -217,6 +247,15 @@ class ObjectInfoManager
     return objects_info_;
   }
 
+  void setPassed1stPassJudgeLineFirstTime(const rclcpp::Time & time)
+  {
+    passed_1st_judge_line_first_time_ = time;
+  }
+  void setPassed2ndPassJudgeLineFirstTime(const rclcpp::Time & time)
+  {
+    passed_2nd_judge_line_first_time_ = time;
+  }
+
 private:
   std::unordered_map<unique_identifier_msgs::msg::UUID, std::shared_ptr<ObjectInfo>> objects_info_;
 
@@ -228,6 +267,9 @@ class ObjectInfoManager
 
   //! parked objects on attention_area/intersection_area
   std::vector<std::shared_ptr<ObjectInfo>> parked_objects_;
+
+  std::optional<rclcpp::Time> passed_1st_judge_line_first_time_{std::nullopt};
+  std::optional<rclcpp::Time> passed_2nd_judge_line_first_time_{std::nullopt};
 };
 
 /**

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -254,7 +254,7 @@ 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 = checkCollision();
+  const bool has_collision = hasCollision();
   if (is_permanent_go_) {
     // TODO(Mamoru Sobue): diagnosis if has_collision
     return intersection::Indecisive{"over pass judge lines"};
@@ -1189,10 +1189,10 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat
 
   const bool is_over_1st_pass_judge_line =
     util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx);
-  bool safely_passed_1st_judge_line = false;
+  bool safely_passed_1st_judge_line_first_time = false;
   if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) {
     safely_passed_1st_judge_line_time_ = clock_->now();
-    safely_passed_1st_judge_line = true;
+    safely_passed_1st_judge_line_first_time = true;
   }
   const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
   debug_data_.first_pass_judge_wall_pose =
@@ -1201,10 +1201,10 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat
   const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line;
   const bool is_over_2nd_pass_judge_line =
     util::isOverTargetIndex(path, closest_idx, current_pose, second_pass_judge_line_idx);
-  bool safely_passed_2nd_judge_line = false;
+  bool safely_passed_2nd_judge_line_first_time = false;
   if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) {
     safely_passed_2nd_judge_line_time_ = clock_->now();
-    safely_passed_2nd_judge_line = true;
+    safely_passed_2nd_judge_line_first_time = true;
   }
   debug_data_.second_pass_judge_wall_pose =
     planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, path);
@@ -1238,8 +1238,8 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat
     is_permanent_go_ = true;
   }
   return {
-    is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line, safely_passed_1st_judge_line,
-    safely_passed_2nd_judge_line};
+    is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line,
+    safely_passed_1st_judge_line_first_time, safely_passed_2nd_judge_line_first_time};
 }
 
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -680,7 +680,7 @@ class IntersectionModule : public SceneModuleInterface
   /**
    * @brief check collision
    */
-  bool checkCollision();
+  bool hasCollision();
 
   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

@@ -132,11 +132,17 @@ void IntersectionModule::updateObjectInfoManagerCollision(
   const intersection::PathLanelets & path_lanelets,
   const IntersectionModule::TimeDistanceArray & time_distance_array,
   const IntersectionModule::TrafficPrioritizedLevel & traffic_prioritized_level,
-  [[maybe_unused]] const bool passed_1st_judge_line_first_time,
-  [[maybe_unused]] const bool passed_2nd_judge_line_first_time)
+  const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time)
 {
   const auto & intersection_lanelets = intersection_lanelets_.value();
 
+  if (passed_1st_judge_line_first_time) {
+    object_info_manager_.setPassed1stPassJudgeLineFirstTime(clock_->now());
+  }
+  if (passed_2nd_judge_line_first_time) {
+    object_info_manager_.setPassed2ndPassJudgeLineFirstTime(clock_->now());
+  }
+
   const double passing_time = time_distance_array.back().first;
   const auto & concat_lanelets = path_lanelets.all;
   const auto closest_arc_coords =
@@ -163,13 +169,14 @@ void IntersectionModule::updateObjectInfoManagerCollision(
 
   for (auto & object_info : object_info_manager_.attentionObjects()) {
     const auto & predicted_object = object_info->predicted_object();
+    bool safe_under_traffic_control = false;
     if (
       traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED &&
       object_info->can_stop_before_stopline(
         planner_param_.collision_detection.ignore_on_amber_traffic_light
           .object_expected_deceleration)) {
       debug_data_.amber_ignore_targets.objects.push_back(predicted_object);
-      continue;
+      safe_under_traffic_control = true;
     }
     if (
       traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED &&
@@ -178,7 +185,7 @@ void IntersectionModule::updateObjectInfoManagerCollision(
           .object_expected_deceleration,
         planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path,
         ego_lane)) {
-      continue;
+      safe_under_traffic_control = true;
     }
 
     // ==========================================================================================
@@ -191,8 +198,15 @@ void IntersectionModule::updateObjectInfoManagerCollision(
     }
     sorted_predicted_paths.sort(
       [](const auto path1, const auto path2) { return path1->confidence > path2->confidence; });
+
+    // ==========================================================================================
+    // if all of the predicted path is lower confidence/geometrically does not intersect with ego
+    // path, both will be null, which is interpreted as SAFE. if any of the path is "normal", either
+    // of them has value, not both
+    // ==========================================================================================
     std::optional<intersection::CollisionInterval> unsafe_interval{std::nullopt};
     std::optional<intersection::CollisionInterval> safe_interval{std::nullopt};
+
     for (const auto & predicted_path_ptr : sorted_predicted_paths) {
       auto predicted_path = *predicted_path_ptr;
       if (
@@ -277,7 +291,27 @@ void IntersectionModule::updateObjectInfoManagerCollision(
         safe_interval = object_passage_interval;
       }
     }
-    object_info->update_safety(unsafe_interval, safe_interval);
+    object_info->update_safety(unsafe_interval, safe_interval, safe_under_traffic_control);
+    if (passed_1st_judge_line_first_time) {
+      object_info->setDecisionAt1stPassJudgeLinePassage(intersection::CollisionKnowledge{
+        clock_->now(),                                      // stamp
+        unsafe_interval ? false : true,                     // safe
+        safe_under_traffic_control,                         // safe_under_traffic_control
+        unsafe_interval ? unsafe_interval : safe_interval,  // interval
+        predicted_object.kinematics.initial_twist_with_covariance.twist.linear
+          .x  // observed_velocity
+      });
+    }
+    if (passed_2nd_judge_line_first_time) {
+      object_info->setDecisionAt2ndPassJudgeLinePassage(intersection::CollisionKnowledge{
+        clock_->now(),                                      // stamp
+        unsafe_interval ? false : true,                     // safe
+        safe_under_traffic_control,                         // safe_under_traffic_control
+        unsafe_interval ? unsafe_interval : safe_interval,  // interval
+        predicted_object.kinematics.initial_twist_with_covariance.twist.linear
+          .x  // observed_velocity
+      });
+    }
   }
 }
 
@@ -353,10 +387,10 @@ IntersectionModule::isGreenPseudoCollisionStatus(
   return std::nullopt;
 }
 
-bool IntersectionModule::checkCollision()
+bool IntersectionModule::hasCollision()
 {
   for (const auto & object_info : object_info_manager_.attentionObjects()) {
-    if (object_info->unsafe_interval().has_value()) {
+    if (!object_info->is_safe()) {
       return true;
     }
   }