fix(intersection): fix/formalize pass judge behavior

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -44,7 +44,9 @@
         not_prioritized:
           collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
           collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
-        keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr
+        pass_judge:
+          judge_before_default_stop_line: false
+          keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr
         use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module
         minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity
         yield_on_green_traffic_light:

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -117,8 +117,16 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
       node, ns + ".collision_detection.not_prioritized.collision_start_margin_time");
   ip.collision_detection.not_prioritized.collision_end_margin_time = getOrDeclareParameter<double>(
     node, ns + ".collision_detection.not_prioritized.collision_end_margin_time");
-  ip.collision_detection.keep_detection_vel_thr =
-    getOrDeclareParameter<double>(node, ns + ".collision_detection.keep_detection_vel_thr");
+  ip.collision_detection.pass_judge.judge_before_default_stop_line = getOrDeclareParameter<bool>(
+    node, ns + ".collision_detection.pass_judge.judge_before_default_stop_line");
+  ip.collision_detection.pass_judge.keep_detection_vel_thr = getOrDeclareParameter<double>(
+    node, ns + ".collision_detection.pass_judge.keep_detection_vel_thr");
+  ip.collision_detection.pass_judge.allow_overshoot_to_unprotected_area =
+    getOrDeclareParameter<bool>(
+      node, ns + ".collision_detection.pass_judge.allow_overshoot_to_unprotected_area");
+  ip.collision_detection.pass_judge.tolerable_overshoot_to_unprotected_area =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.pass_judge.tolerable_overshoot_to_unprotected_area");
   ip.collision_detection.use_upstream_velocity =
     getOrDeclareParameter<bool>(node, ns + ".collision_detection.use_upstream_velocity");
   ip.collision_detection.minimum_upstream_velocity =

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -99,7 +99,6 @@
   const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node,
   const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
-  node_(node),
   lane_id_(lane_id),
   associative_ids_(associative_ids),
   turn_direction_(turn_direction),
@@ -131,7 +130,7 @@
   }
 
   decision_state_pub_ =
-    node_.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
+    node.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
 }
 
 void IntersectionModule::initializeRTCStatus()
@@ -877,9 +876,9 @@
     stop_reason, &velocity_factor_, &debug_data_);
 
   if (!activated_ || !occlusion_activated_) {
-    is_go_out_ = false;
+    is_trying_stop_previously_ = true;
   } else {
-    is_go_out_ = true;
+    is_trying_stop_previously_ = false;
   }
   RCLCPP_DEBUG(logger_, "===== plan end =====");
   return true;
@@ -985,123 +984,124 @@
   const auto & intersection_stop_lines = intersection_stop_lines_opt.value();
   const auto
     [closest_idx, stuck_stop_line_idx_opt, default_stop_line_idx_opt,
-     first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] =
-      intersection_stop_lines;
+     first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx,
+     sudden_stop_line_idx] = intersection_stop_lines;
 
   // see the doc for struct PathLanelets
   const auto & conflicting_area = intersection_lanelets.conflicting_area();
   const auto path_lanelets_opt = util::generatePathLanelets(
     lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area,
     conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx,
     planner_data_->vehicle_info_.vehicle_width_m);
   if (!path_lanelets_opt.has_value()) {
     return IntersectionModule::Indecisive{"failed to generate PathLanelets"};
   }
   const auto path_lanelets = path_lanelets_opt.value();
 
   // utility functions
   auto fromEgoDist = [&](const size_t index) {
     return motion_utils::calcSignedArcLength(path->points, closest_idx, index);
   };
   auto stoppedForDuration =
     [&](const size_t pos, const double duration, StateMachine & state_machine) {
       const double dist_stopline = fromEgoDist(pos);
       const bool approached_dist_stopline =
         (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin);
       const bool over_stopline = (dist_stopline < 0.0);
       const bool is_stopped_duration = planner_data_->isVehicleStopped(duration);
       if (over_stopline) {
         state_machine.setState(StateMachine::State::GO);
       } else if (is_stopped_duration && approached_dist_stopline) {
         state_machine.setState(StateMachine::State::GO);
       }
       return state_machine.getState() == StateMachine::State::GO;
     };
 
   // stuck vehicle detection is viable even if attention area is empty
   // so this needs to be checked before attention area validation
   const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets);
   if (stuck_detected && stuck_stop_line_idx_opt) {
     auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value();
     if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) {
       if (
         default_stop_line_idx_opt &&
         fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) {
         stuck_stop_line_idx = default_stop_line_idx_opt.value();
       }
     } else {
       return IntersectionModule::StuckStop{
         closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt};
     }
   }
 
   // yield stuck vehicle detection is viable even if attention area is empty
   // so this needs to be checked before attention area validation
   const bool yield_stuck_detected = checkYieldStuckVehicle(
     planner_data_, path_lanelets, intersection_lanelets.first_attention_area());
   if (yield_stuck_detected && stuck_stop_line_idx_opt) {
     auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value();
     if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) {
       if (
         default_stop_line_idx_opt &&
         fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) {
         stuck_stop_line_idx = default_stop_line_idx_opt.value();
       }
     } else {
       return IntersectionModule::YieldStuckStop{closest_idx, stuck_stop_line_idx};
     }
   }
 
   // if attention area is empty, collision/occlusion detection is impossible
   if (!first_attention_area_opt) {
     return IntersectionModule::Indecisive{"attention area is empty"};
   }
   const auto first_attention_area = first_attention_area_opt.value();
 
   // if attention area is not null but default stop line is not available, ego/backward-path has
   // already passed the stop line
-  if (!default_stop_line_idx_opt) {
-    return IntersectionModule::Indecisive{"default stop line is null"};
+  if (!default_stop_line_idx_opt || !first_attention_stop_line_idx_opt) {
+    return IntersectionModule::Indecisive{"default_stop_line/first_attention_stop_line is null"};
   }
   const auto default_stop_line_idx = default_stop_line_idx_opt.value();
+  const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value();
+  // occlusion stop line is generated from the intersection of ego footprint along the path with the
+  // attention area, so if this is null, eog has already passed the intersection
+  if (!occlusion_peeking_stop_line_idx_opt) {
+    return IntersectionModule::Indecisive{"occlusion stop line is null"};
+  }
+  const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value();
 
-  // TODO(Mamoru Sobue): this part needs more formal handling
   const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
   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 bool can_stop_before_unprotected_area =
+    planner_param_.occlusion.enable
+      ? !util::isOverTargetIndex(*path, closest_idx, current_pose, occlusion_stop_line_idx)
+      : !util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx);
   const bool is_over_default_stop_line =
     util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_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_vel_thr);
-  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_stop_line && !is_over_pass_judge_line && keep_detection) {
-    RCLCPP_DEBUG(
-      logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection");
-    // do nothing
-  } else if (
-    (was_safe && is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) ||
-    is_permanent_go_) {
-    // is_go_out_: previous RTC approval
-    // activated_: current RTC approval
-    is_permanent_go_ = true;
-    return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"};
+  if (
+    !can_stop_before_unprotected_area &&
+    planner_param_.collision_detection.pass_judge.judge_before_default_stop_line) {
+    return Indecisive{"cannot stop before unprotected area without overshoot"};
   }
-
-  // occlusion stop line is generated from the intersection of ego footprint along the path with the
-  // attention area, so if this is null, eog has already passed the intersection
-  if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) {
-    return IntersectionModule::Indecisive{"occlusion stop line is null"};
+  if (
+    !can_stop_before_unprotected_area &&
+    !planner_param_.collision_detection.pass_judge.allow_overshoot_to_unprotected_area) {
+    return Indecisive{"cannot stop before unprotected area without overshoot"};
+  }
+  const double expected_overshoot_to_unprotected_area = motion_utils::calcSignedArcLength(
+    path->points, first_attention_stop_line_idx, sudden_stop_line_idx);
+  if (
+    !can_stop_before_unprotected_area &&
+    planner_param_.collision_detection.pass_judge.allow_overshoot_to_unprotected_area &&
+    expected_overshoot_to_unprotected_area >
+      planner_param_.collision_detection.pass_judge.tolerable_overshoot_to_unprotected_area) {
+    return Indecisive{"cannot stop before unprotected area within specified margin"};
   }
+
   const auto collision_stop_line_idx =
     is_over_default_stop_line ? closest_idx : default_stop_line_idx;
-  const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value();
-  const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value();
 
   const auto & adjacent_lanelets = intersection_lanelets.adjacent();
   const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention();
@@ -1161,7 +1161,7 @@
   }
 
   // calculate dynamic collision around attention area
-  const double time_to_restart = (is_go_out_ || is_prioritized)
+  const double time_to_restart = (!is_trying_stop_previously_ || is_prioritized)
                                    ? 0.0
                                    : (planner_param_.collision_detection.state_transit_margin_time -
                                       collision_state_machine_.getDuration());

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -102,7 +102,14 @@ class IntersectionModule : public SceneModuleInterface
         double collision_start_margin_time;  //! start margin time to check collision
         double collision_end_margin_time;    //! end margin time to check collision
       } not_prioritized;
-      double keep_detection_vel_thr;  //! keep detection if ego is ego.vel < keep_detection_vel_thr
+      struct PassJudge
+      {
+        bool judge_before_default_stop_line;
+        double
+          keep_detection_vel_thr;  //! keep detection if ego is ego.vel < keep_detection_vel_thr
+        bool allow_overshoot_to_unprotected_area;
+        double tolerable_overshoot_to_unprotected_area;
+      } pass_judge;
       bool use_upstream_velocity;
       double minimum_upstream_velocity;
       struct YieldOnGreeTrafficLight
@@ -148,6 +155,12 @@ class IntersectionModule : public SceneModuleInterface
   {
     std::string error;
   };
+  struct OverPassJudge
+  {
+    size_t closest_idx{0};
+    size_t collision_stop_line_idx{0};
+    size_t occlusion_stop_line_idx{0};
+  };
   struct StuckStop
   {
     size_t closest_idx{0};
@@ -216,7 +229,8 @@ class IntersectionModule : public SceneModuleInterface
     size_t occlusion_stop_line_idx{0};
   };
   using DecisionResult = std::variant<
-    Indecisive,                   // internal process error, or over the pass judge line
+    Indecisive,  // internal process error
+    // OverPassJudge cannot avoid overshoot to unprotected area if stopped suddenly
     StuckStop,                    // detected stuck vehicle
     YieldStuckStop,               // detected yield stuck vehicle
     NonOccludedCollisionStop,     // detected collision while FOV is clear
@@ -253,14 +267,13 @@ class IntersectionModule : public SceneModuleInterface
   bool isOcclusionFirstStopRequired() { return occlusion_first_stop_required_; }
 
 private:
-  rclcpp::Node & node_;
   const int64_t lane_id_;
   const std::set<int> associative_ids_;
   const std::string turn_direction_;
   const bool has_traffic_light_;
 
-  bool is_go_out_{false};
-  bool is_permanent_go_{false};
+  bool is_trying_stop_previously_{false};
+  bool is_going_permanently_{false};
   DecisionResult prev_decision_result_{Indecisive{""}};
 
   // Parameter

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -360,88 +360,85 @@
   const auto pass_judge_line_ip = static_cast<size_t>(
     std::clamp<int>(pass_judge_ip_int, 0, static_cast<int>(path_ip.points.size()) - 1));
 
+  // (4.1) expected stop position on sudden braking
+  constexpr double emergency_response_time = 0.0;
+  const double emergency_braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit(
+    velocity, acceleration, max_stop_acceleration, max_stop_jerk, emergency_response_time);
+  const auto sudden_stop_position_line_ip = std::min<size_t>(
+    closest_idx_ip + std::ceil(emergency_braking_dist / ds), path_ip.points.size() - 1);
+
   // (5) stuck vehicle stop line
   int stuck_stop_line_ip_int = 0;
   bool stuck_stop_line_valid = true;
   if (use_stuck_stopline) {
     // NOTE: when ego vehicle is approaching detection area and already passed
     // first_conflicting_area, this could be null.
     const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygonByFootprint(
       first_conflicting_area, interpolated_path_info, local_footprint);
     if (!stuck_stop_line_idx_ip_opt) {
       stuck_stop_line_valid = false;
       stuck_stop_line_ip_int = 0;
     } else {
       stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value() - stop_line_margin_idx_dist;
     }
   } else {
     stuck_stop_line_ip_int =
       std::get<0>(lane_interval_ip) - (stop_line_margin_idx_dist + base2front_idx_dist);
   }
   if (stuck_stop_line_ip_int < 0) {
     stuck_stop_line_valid = false;
   }
   const auto stuck_stop_line_ip = static_cast<size_t>(std::max(0, stuck_stop_line_ip_int));
 
   struct IntersectionStopLinesTemp
   {
     size_t closest_idx{0};
     size_t stuck_stop_line{0};
     size_t default_stop_line{0};
     size_t first_attention_stop_line{0};
     size_t occlusion_peeking_stop_line{0};
     size_t pass_judge_line{0};
+    size_t sudden_stop_position_line{0};
   };
 
   IntersectionStopLinesTemp intersection_stop_lines_temp;
   std::list<std::pair<const size_t *, size_t *>> stop_lines = {
     {&closest_idx_ip, &intersection_stop_lines_temp.closest_idx},
     {&stuck_stop_line_ip, &intersection_stop_lines_temp.stuck_stop_line},
     {&default_stop_line_ip, &intersection_stop_lines_temp.default_stop_line},
     {&first_attention_stop_line_ip, &intersection_stop_lines_temp.first_attention_stop_line},
     {&occlusion_peeking_line_ip, &intersection_stop_lines_temp.occlusion_peeking_stop_line},
     {&pass_judge_line_ip, &intersection_stop_lines_temp.pass_judge_line},
-  };
+    {&sudden_stop_position_line_ip, &intersection_stop_lines_temp.sudden_stop_position_line}};
   stop_lines.sort(
     [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); });
   for (const auto & [stop_idx_ip, stop_idx] : stop_lines) {
     const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose;
     const auto insert_idx = insertPointIndex(insert_point, original_path);
     if (!insert_idx) {
       return std::nullopt;
     }
     *stop_idx = insert_idx.value();
   }
-  if (
-    intersection_stop_lines_temp.occlusion_peeking_stop_line <
-    intersection_stop_lines_temp.default_stop_line) {
-    intersection_stop_lines_temp.occlusion_peeking_stop_line =
-      intersection_stop_lines_temp.default_stop_line;
-  }
-  if (
-    intersection_stop_lines_temp.occlusion_peeking_stop_line >
-    intersection_stop_lines_temp.pass_judge_line) {
-    intersection_stop_lines_temp.pass_judge_line =
-      intersection_stop_lines_temp.occlusion_peeking_stop_line;
-  }
-
   IntersectionStopLines intersection_stop_lines;
   intersection_stop_lines.closest_idx = intersection_stop_lines_temp.closest_idx;
   if (stuck_stop_line_valid) {
     intersection_stop_lines.stuck_stop_line = intersection_stop_lines_temp.stuck_stop_line;
   }
   if (default_stop_line_valid) {
     intersection_stop_lines.default_stop_line = intersection_stop_lines_temp.default_stop_line;
   }
   if (first_attention_stop_line_valid) {
     intersection_stop_lines.first_attention_stop_line =
       intersection_stop_lines_temp.first_attention_stop_line;
   }
   if (occlusion_peeking_line_valid) {
     intersection_stop_lines.occlusion_peeking_stop_line =
       intersection_stop_lines_temp.occlusion_peeking_stop_line;
   }
   intersection_stop_lines.pass_judge_line = intersection_stop_lines_temp.pass_judge_line;
+  intersection_stop_lines.sudden_stop_position_line =
+    intersection_stop_lines_temp.sudden_stop_position_line;
   return intersection_stop_lines;
 }
 

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -159,6 +159,8 @@ struct IntersectionStopLines
   std::optional<size_t> occlusion_peeking_stop_line{std::nullopt};
   // if the value is calculated negative, its value is 0
   size_t pass_judge_line{0};
+  // NOTE: this position is calculated with delay_response_time = 0.0
+  size_t sudden_stop_position_line{0};
 };
 
 struct PathLanelets