fix(intersection): fixed stuck vehicle detection area

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml

@@ -6,7 +6,7 @@
       keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr
       keep_detection_vel_thr: 0.833 # == 3.0km/h
       stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length.
-      stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
+      stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
       stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h
       intersection_velocity: 2.778 # 2.778m/s = 10.0km/h
       intersection_max_accel: 0.5 # m/ss

planning/behavior_velocity_planner/config/intersection.param.yaml

@@ -6,7 +6,7 @@
       keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr
       keep_detection_vel_thr: 0.833 # == 3.0km/h
       stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length.
-      stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
+      stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
       stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h
       intersection_velocity: 2.778 # 2.778m/s = 10.0km/h
       intersection_max_accel: 0.5 # m/ss

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -166,7 +166,7 @@ class IntersectionModule : public SceneModuleInterface
   Polygon2d generateEgoIntersectionLanePolygon(
     lanelet::LaneletMapConstPtr lanelet_map_ptr,
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
-    const int start_idx, const double extra_dist, const double ignore_dist) const;
+    const double extra_dist, const double ignore_dist) const;
 
   /**
    * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr.

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -150,100 +150,92 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   }
   const size_t closest_idx = closest_idx_opt.get();
 
-  if (stop_lines_idx_opt.has_value()) {
-    const auto & stop_lines = stop_lines_idx_opt.value();
-    const size_t stop_line_idx = stop_lines.stop_line;
-    const size_t pass_judge_line_idx = stop_lines.pass_judge_line;
-    const size_t keep_detection_line_idx = stop_lines.keep_detection_line;
-
-    const bool is_over_pass_judge_line =
-      util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx);
-    const bool is_before_keep_detection_line =
-      util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx);
-    const bool keep_detection = is_before_keep_detection_line &&
-                                std::fabs(current_vel) < planner_param_.keep_detection_vel_thr;
-
-    if (is_over_pass_judge_line && keep_detection) {
-      // in case ego could not stop exactly before the stop line, but with some overshoot,
-      // keep detection within some margin under low velocity threshold
-      RCLCPP_DEBUG(
-        logger_,
-        "over the pass judge line, but before keep detection line and low speed, "
-        "continue planning");
-      // no return here, keep planning
-    } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) {
-      RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed.");
-      RCLCPP_DEBUG(logger_, "===== plan end =====");
-      setSafe(true);
-      setDistance(motion_utils::calcSignedArcLength(
-        path->points, planner_data_->current_pose.pose.position,
-        path->points.at(stop_line_idx).point.pose.position));
-      // no plan needed.
-      return true;
-    }
-  }
+  /* collision checking */
+  bool is_entry_prohibited = false;
 
   /* get dynamic object */
   const auto objects_ptr = planner_data_->predicted_objects;
 
-  /* calculate final stop lines */
-  bool is_entry_prohibited = false;
-  const double detect_length =
+  /* check stuck vehicle */
+  const double ignore_length =
+    planner_param_.stuck_vehicle_ignore_dist + planner_data_->vehicle_info_.vehicle_length_m;
+  const double detect_dist =
     planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m;
   const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon(
-    lanelet_map_ptr, *path, closest_idx, stuck_line_idx, detect_length,
-    planner_param_.stuck_vehicle_detect_dist);
+    lanelet_map_ptr, *path, closest_idx, detect_dist, ignore_length);
   const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area);
-  int stop_line_idx_final = stuck_line_idx;
-  int pass_judge_line_idx_final = stuck_line_idx;
+
+  /* calculate dynamic collision around detection area */
+  const bool has_collision = checkCollision(
+    lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr,
+    closest_idx, stuck_vehicle_detect_area);
+
+  /* calculate final stop lines */
+  int stop_line_idx_final = -1;
+  int pass_judge_line_idx_final = -1;
   if (external_go) {
     is_entry_prohibited = false;
   } else if (external_stop) {
     is_entry_prohibited = true;
-  } else if (is_stuck) {
+  } else if (is_stuck || has_collision) {
     is_entry_prohibited = true;
-    stop_line_idx_final = stuck_line_idx;
-    pass_judge_line_idx_final = stuck_line_idx;
-  } else {
-    /* calculate dynamic collision around detection area */
-    const bool has_collision = checkCollision(
-      lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr,
-      closest_idx, stuck_vehicle_detect_area);
-    is_entry_prohibited = has_collision;
-    if (stop_lines_idx_opt.has_value()) {
-      const auto & stop_lines_idx = stop_lines_idx_opt.value();
-      stop_line_idx_final = stop_lines_idx.stop_line;
-      pass_judge_line_idx_final = stop_lines_idx.pass_judge_line;
-    } else {
-      if (has_collision) {
-        RCLCPP_ERROR(logger_, "generateStopLine() failed for detected objects");
-        RCLCPP_DEBUG(logger_, "===== plan end =====");
-        setSafe(true);
-        setDistance(std::numeric_limits<double>::lowest());
-        return false;
-      } else {
-        RCLCPP_DEBUG(logger_, "no need to stop");
-        RCLCPP_DEBUG(logger_, "===== plan end =====");
-        setSafe(true);
-        setDistance(std::numeric_limits<double>::lowest());
-        return true;
-      }
+    const double dist_stuck_stopline = motion_utils::calcSignedArcLength(
+      path->points, path->points.at(stuck_line_idx).point.pose.position,
+      path->points.at(closest_idx).point.pose.position);
+    const double eps = 1e-1;  // NOTE: check if sufficiently over the stuck stopline
+    const bool is_over_stuck_stopline =
+      util::isOverTargetIndex(*path, closest_idx, current_pose.pose, stuck_line_idx) &&
+      dist_stuck_stopline > eps;
+    if (is_stuck && !is_over_stuck_stopline) {
+      stop_line_idx_final = stuck_line_idx;
+      pass_judge_line_idx_final = stuck_line_idx;
+    } else if (
+      ((is_stuck && is_over_stuck_stopline) || has_collision) && stop_lines_idx_opt.has_value()) {
+      stop_line_idx_final = stop_lines_idx_opt.value().stop_line;
+      pass_judge_line_idx_final = stop_lines_idx_opt.value().pass_judge_line;
     }
   }
 
-  state_machine_.setStateWithMarginTime(
-    is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO,
-    logger_.get_child("state_machine"), *clock_);
+  if (stop_line_idx_final == -1) {
+    RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed");
+    RCLCPP_DEBUG(logger_, "===== plan end =====");
+    setSafe(true);
+    setDistance(std::numeric_limits<double>::lowest());
+    return false;
+  }
 
-  setSafe(state_machine_.getState() == StateMachine::State::GO);
-  if (is_entry_prohibited) {
+  const bool is_over_pass_judge_line =
+    util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx_final);
+  const bool is_before_keep_detection_line =
+    stop_lines_idx_opt.has_value()
+      ? util::isBeforeTargetIndex(
+          *path, closest_idx, current_pose.pose, stop_lines_idx_opt.value().keep_detection_line)
+      : false;
+  const bool keep_detection =
+    is_before_keep_detection_line && std::fabs(current_vel) < planner_param_.keep_detection_vel_thr;
+
+  if (is_over_pass_judge_line && keep_detection) {
+    // in case ego could not stop exactly before the stop line, but with some overshoot,
+    // keep detection within some margin under low velocity threshold
+  } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) {
+    RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed.");
+    RCLCPP_DEBUG(logger_, "===== plan end =====");
+    setSafe(true);
     setDistance(motion_utils::calcSignedArcLength(
       path->points, planner_data_->current_pose.pose.position,
       path->points.at(stop_line_idx_final).point.pose.position));
-  } else {
-    setDistance(std::numeric_limits<double>::lowest());
+    return true;
   }
 
+  state_machine_.setStateWithMarginTime(
+    is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO,
+    logger_.get_child("state_machine"), *clock_);
+
+  setSafe(state_machine_.getState() == StateMachine::State::GO);
+  setDistance(motion_utils::calcSignedArcLength(
+    path->points, planner_data_->current_pose.pose.position,
+    path->points.at(stop_line_idx_final).point.pose.position));
+
   if (!isActivated()) {
     // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block
     is_go_out_ = false;
@@ -318,8 +310,11 @@ bool IntersectionModule::checkCollision(
   using lanelet::utils::getPolygonFromArcLength;
 
   /* generate ego-lane polygon */
-  const auto ego_poly =
-    generateEgoIntersectionLanePolygon(lanelet_map_ptr, path, closest_idx, closest_idx, 0.0, 0.0);
+  const auto ego_lane_poly = lanelet_map_ptr->laneletLayer.get(module_id_).polygon2d();
+  Polygon2d ego_poly{};
+  for (const auto & p : ego_lane_poly) {
+    ego_poly.outer().emplace_back(p.x(), p.y());
+  }
   lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path);
   lanelet::ConstLanelet closest_lanelet;
   lanelet::utils::query::getClosestLanelet(
@@ -479,7 +474,7 @@ bool IntersectionModule::checkCollision(
 Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon(
   lanelet::LaneletMapConstPtr lanelet_map_ptr,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
-  const int start_idx, const double extra_dist, const double ignore_dist) const
+  const double extra_dist, const double ignore_dist) const
 {
   using lanelet::utils::getArcCoordinates;
   using lanelet::utils::getLaneletLength3d;
@@ -488,15 +483,14 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon(
 
   lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path);
 
-  const auto start_arc_coords = getArcCoordinates(
-    ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(start_idx).point));
+  const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front());
 
   const auto closest_arc_coords = getArcCoordinates(
     ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point));
 
-  const double start_arc_length = start_arc_coords.length + ignore_dist < closest_arc_coords.length
-                                    ? closest_arc_coords.length
-                                    : start_arc_coords.length + ignore_dist;
+  const double start_arc_length = intersection_exit_length - ignore_dist > closest_arc_coords.length
+                                    ? intersection_exit_length - ignore_dist
+                                    : closest_arc_coords.length;
 
   const double end_arc_length = getLaneletLength3d(ego_lane_with_next_lane.front()) + extra_dist;
 

planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp

@@ -148,7 +148,7 @@ std::pair<std::optional<size_t>, std::optional<StopLineIdx>> generateStopLine(
   const double keep_detection_line_margin, const bool use_stuck_stopline,
   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger,
-  const rclcpp::Clock::SharedPtr clock)
+  [[maybe_unused]] const rclcpp::Clock::SharedPtr clock)
 {
   /* set judge line dist */
   const double current_vel = planner_data->current_velocity->twist.linear.x;
@@ -196,13 +196,6 @@ std::pair<std::optional<size_t>, std::optional<StopLineIdx>> generateStopLine(
   if (use_stuck_stopline) {
     // the first point in intersection lane
     stuck_stop_line_idx_ip = lane_interval_ip_start;
-    if (stuck_stop_line_idx_ip == 0) {
-      RCLCPP_WARN_SKIPFIRST_THROTTLE(
-        logger, *clock, 1000 /* ms */,
-        "use_stuck_stopline, but ego is already in the intersection, not generating stuck stop "
-        "line");
-      return {std::nullopt, std::nullopt};
-    }
   } else {
     const auto stuck_stop_line_idx_ip_opt = util::getFirstPointInsidePolygons(
       path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, conflicting_areas);