measure the overshoot into unprotected area and check the overshoot

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

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -121,6 +121,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     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<double>(
+      node, ns + ".collision_detection.pass_judge.allow_overshoot_to_unprotected_area");
+  ip.collision_detection.pass_judge.tolerable_overshoot_to_unprotected_area =
+    getOrDeclareParameter<bool>(
+      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

@@ -99,7 +99,6 @@ IntersectionModule::IntersectionModule(
   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 @@ IntersectionModule::IntersectionModule(
   }
 
   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 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     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;
@@ -978,15 +977,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   const auto intersection_stop_lines_opt = util::generateIntersectionStopLines(
     first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info,
     planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin,
-    peeking_offset, planner_param_.occlusion.enable, path);
+    peeking_offset, path);
   if (!intersection_stop_lines_opt) {
     return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"};
   }
   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();
@@ -1060,48 +1059,49 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
 
   // 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.pass_judge.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 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   }
 
   // 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

@@ -107,6 +107,8 @@ class IntersectionModule : public SceneModuleInterface
         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;
@@ -153,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};
@@ -221,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
@@ -258,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

@@ -269,7 +269,7 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
   const lanelet::CompoundPolygon3d & first_detection_area,
   const std::shared_ptr<const PlannerData> & planner_data,
   const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline,
-  const double stop_line_margin, const double peeking_offset, const double enable_occlusion,
+  const double stop_line_margin, const double peeking_offset,
   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path)
 {
   const auto & path_ip = interpolated_path_info.path;
@@ -360,6 +360,13 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
   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;
@@ -391,6 +398,7 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
     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;
@@ -401,7 +409,7 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
     {&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) {
@@ -412,19 +420,6 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
     }
     *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 (
-    enable_occlusion && 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) {
@@ -442,6 +437,8 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
       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.hpp

@@ -70,7 +70,7 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
   const lanelet::CompoundPolygon3d & first_detection_area,
   const std::shared_ptr<const PlannerData> & planner_data,
   const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline,
-  const double stop_line_margin, const double peeking_offset, const double enable_occlusion,
+  const double stop_line_margin, const double peeking_offset,
   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path);
 
 std::optional<size_t> getFirstPointInsidePolygon(

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