fix(intersection): fixed stuck vehicle stop line and refactor stop li…

…ne logic (autowarefoundation#2226)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

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

@@ -38,16 +38,16 @@ namespace behavior_velocity_planner
 {
 namespace util
 {
-int insertPoint(
+std::optional<size_t> insertPoint(
   const geometry_msgs::msg::Pose & in_pose,
   autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path);
 
 bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id);
 std::optional<std::pair<size_t, size_t>> findLaneIdInterval(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const int lane_id);
-bool getDuplicatedPointIdx(
+std::optional<size_t> getDuplicatedPointIdx(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const geometry_msgs::msg::Point & point, int * duplicated_point_idx);
+  const geometry_msgs::msg::Point & point);
 
 /**
  * @brief get objective polygons for detection area
@@ -58,11 +58,10 @@ std::tuple<lanelet::ConstLanelets, lanelet::ConstLanelets> getObjectiveLanelets(
 
 struct StopLineIdx
 {
-  // TODO(Mamoru Sobue): replace optional<size_t>
-  int first_idx_inside_lane = -1;
-  int pass_judge_line_idx = -1;
-  int stop_line_idx = -1;
-  int keep_detection_line_idx = -1;
+  size_t first_inside_lane = 0;
+  size_t pass_judge_line = 0;
+  size_t stop_line = 0;
+  size_t keep_detection_line = 0;
 };
 
 /**
@@ -72,32 +71,17 @@ struct StopLineIdx
  * @param original_path   ego-car lane
  * @param target_path     target lane to insert stop point (part of ego-car lane or same to ego-car
  * lane)
- * @param stop_line_idx   generated stop line index
- * @param pass_judge_line_idx  generated stop line index
- * @return false when path is not intersecting with detection area, or stop_line is behind path[0]
+ " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane
+ * @return nullopt if path is not intersecting with detection areas
  */
-bool generateStopLine(
-  const int lane_id, const std::vector<lanelet::CompoundPolygon3d> detection_areas,
+std::pair<std::optional<size_t>, std::optional<StopLineIdx>> generateStopLine(
+  const int lane_id, const std::vector<lanelet::CompoundPolygon3d> & detection_areas,
+  const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
   const std::shared_ptr<const PlannerData> & planner_data, const double stop_line_margin,
-  const double keep_detection_line_margin,
+  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,
-  StopLineIdx * stop_line_idxs, const rclcpp::Logger logger);
-
-/**
- * @brief If use_stuck_stopline is true, a stop line is generated before the intersection.
- * @param input_path      input path
- * @param output_path     output path
- * @param stuck_stop_line_idx   generated stuck stop line index
- * @param pass_judge_line_idx  generated pass judge line index
- * @return false when generation failed
- */
-bool generateStopLineBeforeIntersection(
-  const int lane_id, lanelet::LaneletMapConstPtr lanelet_map_ptr,
-  const std::shared_ptr<const PlannerData> & planner_data,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
-  autoware_auto_planning_msgs::msg::PathWithLaneId * output_path, int * stuck_stop_line_idx,
-  int * pass_judge_line_idx, const rclcpp::Logger logger);
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger,
+  const rclcpp::Clock::SharedPtr clock);
 
 /**
  * @brief Calculate first path index that is in the polygon.
@@ -120,8 +104,8 @@ std::optional<size_t> getFirstPointInsidePolygons(
 bool getStopLineIndexFromMap(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t lane_interval_start,
   const size_t lane_interval_end, const int lane_id,
-  const std::shared_ptr<const PlannerData> & planner_data, int * stop_idx_ip, int dist_thr,
-  const rclcpp::Logger logger);
+  const std::shared_ptr<const PlannerData> & planner_data, size_t * stop_idx_ip,
+  const double dist_thr, const rclcpp::Logger logger);
 
 std::vector<lanelet::CompoundPolygon3d> getPolygon3dFromLaneletsVec(
   const std::vector<lanelet::ConstLanelets> & ll_vec, double clip_length);
@@ -160,6 +144,7 @@ lanelet::ConstLanelets extendedAdjacentDirectionLanes(
 std::optional<Polygon2d> getIntersectionArea(
   lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr);
 
+bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane);
 bool isTrafficLightArrowActivated(
   lanelet::ConstLanelet lane,
   const std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped> & tl_infos);

planning/behavior_velocity_planner/intersection-design.md

@@ -60,6 +60,7 @@ The following process is performed for the attention targets to determine whethe
    2. calculate the passing area of the target object $A_{target}$ at $t_s$ - `collision_start_margin_time` ~ $t_e$ + `collision_end_margin_time` for each predicted path (\*1).
    3. check if $A_{ego}$ and $A_{target}$ regions are overlapped (has collision).
 4. when a collision is detected, the module inserts a stop velocity in front of the intersection. Note that there is a time margin for the stop release (\*2).
+5. If ego is over the `pass_judge_line`, collision checking is not processed to avoid sudden braking. However if ego velocity is lower than the threshold `keep_detection_vel_thr` then this module continues collision checking.
 
 (\*1) The parameters `collision_start_margin_time` and `collision_end_margin_time` can be interpreted as follows:
 
@@ -102,6 +103,7 @@ As a related case, if the object in front of the ego vehicle is turning the same
 | `intersection/min_predicted_path_confidence`  | double | [-] minimum confidence value of predicted path to use for collision detection                  |
 | `merge_from_private_road/stop_duration_sec`   | double | [s] duration to stop                                                                           |
 | `assumed_front_car_decel: 1.0`                | double | [m/s^2] deceleration of front car used to check if it could stop in the stuck area at the exit |
+| `keep_detection_vel_threshold`                | double | [m/s] the threshold for ego vehicle for keeping detection after passing `pass_judge_line`      |
 
 ### How To Tune Parameters
 

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

@@ -70,17 +70,19 @@ bool IntersectionModule::modifyPathVelocity(
   autoware_auto_planning_msgs::msg::PathWithLaneId * path,
   tier4_planning_msgs::msg::StopReason * stop_reason)
 {
+  RCLCPP_DEBUG(logger_, "===== plan start =====");
+
   const bool external_go = isTargetExternalInputStatus(tier4_api_msgs::msg::IntersectionStatus::GO);
   const bool external_stop =
     isTargetExternalInputStatus(tier4_api_msgs::msg::IntersectionStatus::STOP);
-  RCLCPP_DEBUG(logger_, "===== plan start =====");
+  const StateMachine::State current_state = state_machine_.getState();
+
   debug_data_ = DebugData();
+  debug_data_.path_raw = *path;
+
   *stop_reason =
     planning_utils::initializeStopReason(tier4_planning_msgs::msg::StopReason::INTERSECTION);
 
-  debug_data_.path_raw = *path;
-
-  StateMachine::State current_state = state_machine_.getState();
   RCLCPP_DEBUG(
     logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str());
 
@@ -97,6 +99,7 @@ bool IntersectionModule::modifyPathVelocity(
 
   /* get detection area*/
   /* dynamically change detection area based on tl_arrow_solid_on */
+  [[maybe_unused]] const bool has_tl = util::hasAssociatedTrafficLight(assigned_lanelet);
   const bool tl_arrow_solid_on =
     util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map);
   auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets(
@@ -116,31 +119,25 @@ bool IntersectionModule::modifyPathVelocity(
   }
 
   /* get adjacent lanelets */
-  auto adjacent_lanelets =
+  const auto adjacent_lanelets =
     util::extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet);
   debug_data_.adjacent_area = util::getPolygon3dFromLanelets(adjacent_lanelets);
 
-  /* set stop-line and stop-judgement-line for base_link */
-  util::StopLineIdx stop_line_idxs;
-  // if straight, need to care stuck vehicle ahead of the lane using conflicting_lane
-  const auto & attention_area = (turn_direction.compare("straight") == 0 && detection_area.empty())
-                                  ? conflicting_area
-                                  : detection_area;
-  if (!util::generateStopLine(
-        lane_id_, attention_area, planner_data_, planner_param_.stop_line_margin,
-        planner_param_.keep_detection_line_margin, path, *path, &stop_line_idxs,
-        logger_.get_child("util"))) {
-    // returns here if path is not intersecting with attention_area
-    RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");
+  /* set stop lines for base_link */
+  const auto [stuck_line_idx_opt, stop_lines_idx_opt] = util::generateStopLine(
+    lane_id_, detection_area, conflicting_area, planner_data_, planner_param_.stop_line_margin,
+    planner_param_.keep_detection_line_margin, planner_param_.use_stuck_stopline, path, *path,
+    logger_.get_child("util"), clock_);
+  if (!stuck_line_idx_opt.has_value()) {
+    // returns here if path is not intersecting with conflicting areas
+    RCLCPP_WARN_SKIPFIRST_THROTTLE(
+      logger_, *clock_, 1000 /* ms */, "setStopLineIdx for stuck line fail");
     RCLCPP_DEBUG(logger_, "===== plan end =====");
     setSafe(true);
     setDistance(std::numeric_limits<double>::lowest());
     return false;
   }
-
-  const int stop_line_idx = stop_line_idxs.stop_line_idx;
-  const int pass_judge_line_idx = stop_line_idxs.pass_judge_line_idx;
-  const int keep_detection_line_idx = stop_line_idxs.keep_detection_line_idx;
+  const auto stuck_line_idx = stuck_line_idx_opt.value();
 
   /* calc closest index */
   const auto closest_idx_opt =
@@ -155,89 +152,112 @@ bool IntersectionModule::modifyPathVelocity(
   }
   const size_t closest_idx = closest_idx_opt.get();
 
-  const bool is_over_pass_judge_line =
-    util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx);
-  if (is_over_pass_judge_line) {
-    /*
-      in case ego could not stop exactly before the stop line, but with some overshoot, keep
-      detection within some margin and low velocity threshold
-     */
+  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 (keep_detection) {
+
+    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_go_out_ && !external_stop) {
-      // TODO(Mamoru Sobue): refactor this block
+    } 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));
-      return true;  // no plan needed.
+      // no plan needed.
+      return true;
     }
   }
 
   /* get dynamic object */
   const auto objects_ptr = planner_data_->predicted_objects;
 
-  /* calculate dynamic collision around detection area */
+  /* calculate final stop lines */
+  bool is_entry_prohibited = false;
   const double detect_length =
     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, stop_line_idx, detect_length,
+    lanelet_map_ptr, *path, closest_idx, stuck_line_idx, detect_length,
     planner_param_.stuck_vehicle_detect_dist);
   const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area);
-  const bool has_collision = checkCollision(
-    lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr,
-    closest_idx, stuck_vehicle_detect_area);
-  // TODO(Mamoru Sobue): process external_go at the top of this function
-  bool is_entry_prohibited = (has_collision || is_stuck);
+  int stop_line_idx_final = stuck_line_idx;
+  int pass_judge_line_idx_final = stuck_line_idx;
   if (external_go) {
     is_entry_prohibited = false;
   } else if (external_stop) {
     is_entry_prohibited = true;
+  } else if (is_stuck) {
+    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;
+      }
+    }
   }
+
   state_machine_.setStateWithMarginTime(
     is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO,
     logger_.get_child("state_machine"), *clock_);
 
-  const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
-
   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).point.pose.position));
+  if (is_entry_prohibited) {
+    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());
+  }
 
   if (!isActivated()) {
     // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block
-    constexpr double v = 0.0;
     is_go_out_ = false;
-    int stop_line_idx_stop = stop_line_idx;
-    int pass_judge_line_idx_stop = pass_judge_line_idx;
-    // TODO(Mamoru Sobue): refactor this block
-    if (planner_param_.use_stuck_stopline && is_stuck) {
-      int stuck_stop_line_idx = -1;
-      int stuck_pass_judge_line_idx = -1;
-      if (util::generateStopLineBeforeIntersection(
-            lane_id_, lanelet_map_ptr, planner_data_, *path, path, &stuck_stop_line_idx,
-            &stuck_pass_judge_line_idx, logger_.get_child("util"))) {
-        stop_line_idx_stop = stuck_stop_line_idx;
-        pass_judge_line_idx_stop = stuck_pass_judge_line_idx;
-      }
-    }
-    planning_utils::setVelocityFromIndex(stop_line_idx_stop, v, path);
+
+    constexpr double v = 0.0;
+    planning_utils::setVelocityFromIndex(stop_line_idx_final, v, path);
     debug_data_.stop_required = true;
+    const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
     debug_data_.stop_wall_pose =
-      planning_utils::getAheadPose(stop_line_idx_stop, base_link2front, *path);
-    debug_data_.stop_point_pose = path->points.at(stop_line_idx_stop).point.pose;
-    debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx_stop).point.pose;
+      planning_utils::getAheadPose(stop_line_idx_final, base_link2front, *path);
+    debug_data_.stop_point_pose = path->points.at(stop_line_idx_final).point.pose;
+    debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx_final).point.pose;
 
     /* get stop point and stop factor */
     tier4_planning_msgs::msg::StopFactor stop_factor;

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

@@ -75,33 +75,34 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
   }
   const auto detection_area =
     util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length);
+  const std::vector<lanelet::CompoundPolygon3d> conflicting_area =
+    util::getPolygon3dFromLanelets(conflicting_lanelets);
   debug_data_.detection_area = detection_area;
 
   /* set stop-line and stop-judgement-line for base_link */
-  util::StopLineIdx stop_line_idxs;
   const auto private_path =
     extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m);
-  if (!util::generateStopLine(
-        lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin,
-        0.0 /* unnecessary in merge_from_private */, path, private_path, &stop_line_idxs,
-        logger_.get_child("util"))) {
+  const auto [stuck_line_idx_opt, stop_lines_idx_opt] = util::generateStopLine(
+    lane_id_, detection_area, conflicting_area, planner_data_, planner_param_.stop_line_margin,
+    0.0 /* unnecessary in merge_from_private */, false /* same */, path, *path,
+    logger_.get_child("util"), clock_);
+  if (!stop_lines_idx_opt.has_value()) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");
     return false;
   }
 
-  const int stop_line_idx = stop_line_idxs.stop_line_idx;
-  if (stop_line_idx <= 0) {
+  const auto & stop_lines_idx = stop_lines_idx_opt.value();
+  const size_t stop_line_idx = stop_lines_idx.stop_line;
+  if (stop_line_idx == 0) {
     RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning.");
     return true;
   }
 
   debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
     stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path);
   debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose;
-  const int first_idx_inside_lane = stop_line_idxs.first_idx_inside_lane;
-  if (first_idx_inside_lane != -1) {
-    debug_data_.first_collision_point = path->points.at(first_idx_inside_lane).point.pose.position;
-  }
+  const size_t first_inside_lane_idx = stop_lines_idx.first_inside_lane;
+  debug_data_.first_collision_point = path->points.at(first_inside_lane_idx).point.pose.position;
 
   /* set stop speed */
   if (state_machine_.getState() == StateMachine::State::STOP) {