formalize pass judge

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -7,6 +7,7 @@
         attention_area_angle_threshold: 0.785
         use_intersection_area: false
         default_stopline_margin: 3.0
+        second_pass_judge_line_margin: 1.0
         stopline_overshoot_margin: 0.5
         path_interpolation_ds: 0.1
         max_accel: -2.8
@@ -36,7 +37,6 @@
         consider_wrong_direction_vehicle: false
         collision_detection_hold_time: 0.5
         min_predicted_path_confidence: 0.05
-        keep_detection_velocity_threshold: 0.833
         velocity_profile:
           use_upstream: true
           minimum_upstream_velocity: 0.01

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -90,7 +90,7 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray(
   marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP;
   marker_line.action = visualization_msgs::msg::Marker::ADD;
   marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0);
-  marker_line.scale = createMarkerScale(0.1, 0.0, 0.0);
+  marker_line.scale = createMarkerScale(0.2, 0.0, 0.0);
   marker_line.color = createMarkerColor(r, g, b, 0.999);
 
   const double yaw = tf2::getYaw(pose.orientation);
@@ -283,11 +283,23 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
     &debug_marker_array, now);
   */
 
-  if (debug_data_.pass_judge_wall_pose) {
+  if (debug_data_.first_pass_judge_wall_pose) {
+    const double r = debug_data_.passed_first_pass_judge ? 1.0 : 0.0;
+    const double g = debug_data_.passed_first_pass_judge ? 0.0 : 1.0;
     appendMarkerArray(
       createPoseMarkerArray(
-        debug_data_.pass_judge_wall_pose.value(), "pass_judge_wall_pose", module_id_, 0.7, 0.85,
-        0.9),
+        debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r,
+        g, 0.0),
+      &debug_marker_array, now);
+  }
+
+  if (debug_data_.second_pass_judge_wall_pose) {
+    const double r = debug_data_.passed_second_pass_judge ? 1.0 : 0.0;
+    const double g = debug_data_.passed_second_pass_judge ? 0.0 : 1.0;
+    appendMarkerArray(
+      createPoseMarkerArray(
+        debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_,
+        r, g, 0.0),
       &debug_marker_array, now);
   }
 

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -53,6 +53,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<bool>(node, ns + ".common.use_intersection_area");
   ip.common.default_stopline_margin =
     getOrDeclareParameter<double>(node, ns + ".common.default_stopline_margin");
+  ip.common.second_pass_judge_line_margin =
+    getOrDeclareParameter<double>(node, ns + ".common.second_pass_judge_line_margin");
   ip.common.stopline_overshoot_margin =
     getOrDeclareParameter<double>(node, ns + ".common.stopline_overshoot_margin");
   ip.common.path_interpolation_ds =
@@ -92,8 +94,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".collision_detection.collision_detection_hold_time");
   ip.collision_detection.min_predicted_path_confidence =
     getOrDeclareParameter<double>(node, ns + ".collision_detection.min_predicted_path_confidence");
-  ip.collision_detection.keep_detection_velocity_threshold = getOrDeclareParameter<double>(
-    node, ns + ".collision_detection.keep_detection_velocity_threshold");
   ip.collision_detection.velocity_profile.use_upstream =
     getOrDeclareParameter<bool>(node, ns + ".collision_detection.velocity_profile.use_upstream");
   ip.collision_detection.velocity_profile.minimum_upstream_velocity = getOrDeclareParameter<double>(

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1041,7 +1041,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   const auto default_stopline_idx_opt = intersection_stoplines.default_stopline;
   const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline;
   const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline;
-  const auto default_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line;
+  const auto first_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line;
+  const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line;
   const auto occlusion_wo_tl_pass_judge_line_idx =
     intersection_stoplines.occlusion_wo_tl_pass_judge_line;
 
@@ -1221,44 +1222,72 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     prev_occlusion_status_ = occlusion_status;
   }
 
-  // TODO(Mamoru Sobue): this part needs more formal handling
-  const size_t pass_judge_line_idx = [=]() {
+  const size_t pass_judge_line_idx = [&]() {
     if (enable_occlusion_detection_) {
-      // if occlusion detection is enabled, pass_judge position is beyond the boundary of first
-      // attention area
       if (has_traffic_light_) {
-        return occlusion_stopline_idx;
+        // if ego passed the first_pass_judge_line while it is peeking to occlusion, then its
+        // position is occlusion_stopline_idx. Otherwise it is first_pass_judge_line
+        if (passed_1st_judge_line_while_peeking_) {
+          return occlusion_stopline_idx;
+        }
+        const bool is_over_first_pass_judge_line =
+          util::isOverTargetIndex(*path, closest_idx, current_pose, first_pass_judge_line_idx);
+        if (is_occlusion_state && is_over_first_pass_judge_line) {
+          passed_1st_judge_line_while_peeking_ = true;
+          return occlusion_stopline_idx;
+        } else {
+          return first_pass_judge_line_idx;
+        }
       } else if (is_occlusion_state) {
         // if there is no traffic light and occlusion is detected, pass_judge position is beyond
         // the boundary of first attention area
         return occlusion_wo_tl_pass_judge_line_idx;
       } else {
         // if there is no traffic light and occlusion is not detected, pass_judge position is
         // default
-        return default_pass_judge_line_idx;
+        return first_pass_judge_line_idx;
       }
     }
-    return default_pass_judge_line_idx;
+    return first_pass_judge_line_idx;
   }();
-  debug_data_.pass_judge_wall_pose =
-    planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path);
-  const bool is_over_pass_judge_line =
-    util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx);
-  const double vel_norm = std::hypot(
-    planner_data_->current_velocity->twist.linear.x,
-    planner_data_->current_velocity->twist.linear.y);
-  const bool keep_detection =
-    (vel_norm < planner_param_.collision_detection.keep_detection_velocity_threshold);
+
   const bool was_safe = std::holds_alternative<IntersectionModule::Safe>(prev_decision_result_);
-  // if ego is over the pass judge line and not stopped
-  if (is_over_default_stopline && !is_over_pass_judge_line && keep_detection) {
-    RCLCPP_DEBUG(logger_, "is_over_default_stopline && !is_over_pass_judge_line && keep_detection");
-    // do nothing
-  } else if (
-    (was_safe && is_over_default_stopline && is_over_pass_judge_line && is_go_out_) ||
-    is_permanent_go_) {
-    // is_go_out_: previous RTC approval
-    // activated_: current RTC approval
+
+  const bool is_over_1st_pass_judge_line =
+    util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx);
+  if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) {
+    safely_passed_1st_judge_line_time_ = clock_->now();
+  }
+  debug_data_.first_pass_judge_wall_pose =
+    planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path);
+  debug_data_.passed_first_pass_judge = safely_passed_1st_judge_line_time_.has_value();
+  const bool is_over_2nd_pass_judge_line =
+    util::isOverTargetIndex(*path, closest_idx, current_pose, second_pass_judge_line_idx);
+  if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) {
+    safely_passed_2nd_judge_line_time_ = clock_->now();
+  }
+  debug_data_.second_pass_judge_wall_pose =
+    planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, *path);
+  debug_data_.passed_second_pass_judge = safely_passed_2nd_judge_line_time_.has_value();
+
+  if ((is_over_default_stopline && is_over_2nd_pass_judge_line && was_safe) || is_permanent_go_) {
+    /*
+     * This body is active if ego is
+     * - over the default stopline AND
+     * - over the 1st && 2nd pass judge line AND
+     * - previously safe
+     * ,
+     * which means ego can stop even if it is over the 1st pass judge line but
+     * - before default stopline OR
+     * - before the 2nd pass judge line OR
+     * - or previously unsafe
+     * .
+     * In order for ego to continue peeking or collision detection when occlusion is detected after
+     * ego passed the 1st pass judge line, it needs to be
+     * - before the default stopline OR
+     * - before the 2nd pass judge line OR
+     * - previously unsafe
+     */
     is_permanent_go_ = true;
     return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"};
   }
@@ -1309,6 +1338,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
       : (planner_param_.collision_detection.collision_detection_hold_time -
          collision_state_machine_.getDuration());
 
+  // 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
   const bool has_collision = checkCollision(
     *path, &target_objects, path_lanelets, closest_idx,
     std::min<size_t>(occlusion_stopline_idx, path->points.size() - 1), time_to_restart,
@@ -1677,6 +1708,7 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
   const double max_jerk = planner_param_.common.max_jerk;
   const double delay_response_time = planner_param_.common.delay_response_time;
   const double peeking_offset = planner_param_.occlusion.peeking_offset;
+  const double second_pass_judge_line_margin = planner_param_.common.second_pass_judge_line_margin;
 
   const auto first_attention_area = first_attention_lane.polygon3d();
   const auto first_attention_lane_centerline = first_attention_lane.centerline2d();
@@ -1814,10 +1846,13 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
     second_attention_stopline_ip_int >= 0 ? static_cast<size_t>(second_attention_stopline_ip_int)
                                           : 0;
 
-  // (8) second pass judge lie position on interpolated path
-  int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds);
+  // (8) second pass judge line position on interpolated path. It is the same as first pass judge
+  // line if second_attention_lane is null
+  int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds) -
+                                 std::ceil(second_pass_judge_line_margin / ds);
   const auto second_pass_judge_line_ip =
-    static_cast<size_t>(std::max<int>(second_pass_judge_ip_int, 0));
+    second_attention_area_opt ? static_cast<size_t>(std::max<int>(second_pass_judge_ip_int, 0))
+                              : first_pass_judge_line_ip;
 
   struct IntersectionStopLinesTemp
   {

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -48,7 +48,10 @@ struct DebugData
   std::optional<geometry_msgs::msg::Pose> collision_stop_wall_pose{std::nullopt};
   std::optional<geometry_msgs::msg::Pose> occlusion_stop_wall_pose{std::nullopt};
   std::optional<geometry_msgs::msg::Pose> occlusion_first_stop_wall_pose{std::nullopt};
-  std::optional<geometry_msgs::msg::Pose> pass_judge_wall_pose{std::nullopt};
+  std::optional<geometry_msgs::msg::Pose> first_pass_judge_wall_pose{std::nullopt};
+  bool passed_first_pass_judge{false};
+  bool passed_second_pass_judge{false};
+  std::optional<geometry_msgs::msg::Pose> second_pass_judge_wall_pose{std::nullopt};
   std::optional<std::vector<lanelet::CompoundPolygon3d>> attention_area{std::nullopt};
   std::optional<std::vector<lanelet::CompoundPolygon3d>> occlusion_attention_area{std::nullopt};
   std::optional<lanelet::CompoundPolygon3d> ego_lane{std::nullopt};
@@ -256,8 +259,8 @@ struct IntersectionStopLines
   size_t first_pass_judge_line{0};
 
   /**
-   * second_pass_judge_line is before second_attention_stopline by the braking distance. if its
-   * value is calculated negative, it is 0
+   * second_pass_judge_line is before second_attention_stopline by the braking distance. if
+   * second_attention_lane is null, it is same as first_pass_judge_line
    */
   size_t second_pass_judge_line{0};
 
@@ -339,6 +342,7 @@ class IntersectionModule : public SceneModuleInterface
       double attention_area_angle_threshold;
       bool use_intersection_area;
       double default_stopline_margin;
+      double second_pass_judge_line_margin;
       double stopline_overshoot_margin;
       double path_interpolation_ds;
       double max_accel;
@@ -373,7 +377,6 @@ class IntersectionModule : public SceneModuleInterface
       bool consider_wrong_direction_vehicle;
       double collision_detection_hold_time;
       double min_predicted_path_confidence;
-      double keep_detection_velocity_threshold;
       struct VelocityProfile
       {
         bool use_upstream;
@@ -606,7 +609,7 @@ class IntersectionModule : public SceneModuleInterface
 
 private:
   rclcpp::Node & node_;
-  const int64_t lane_id_;
+  const lanelet::Id lane_id_;
   const std::set<lanelet::Id> associative_ids_;
   const std::string turn_direction_;
   const bool has_traffic_light_;
@@ -621,6 +624,9 @@ class IntersectionModule : public SceneModuleInterface
   bool is_permanent_go_{false};
   DecisionResult prev_decision_result_{Indecisive{""}};
   OcclusionType prev_occlusion_status_;
+  bool passed_1st_judge_line_while_peeking_{false};
+  std::optional<rclcpp::Time> safely_passed_1st_judge_line_time_{std::nullopt};
+  std::optional<rclcpp::Time> safely_passed_2nd_judge_line_time_{std::nullopt};
 
   // for occlusion detection
   const bool enable_occlusion_detection_;