feat(intersection): continue detection after pass judge (#1719)

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

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

@@ -3,6 +3,8 @@
     intersection:
       state_transit_margin_time: 0.5
       stop_line_margin: 3.0
+      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_vel_thr: 0.833 # 0.833m/s = 3.0km/h

planning/behavior_velocity_planner/config/intersection.param.yaml

@@ -3,6 +3,8 @@
     intersection:
       state_transit_margin_time: 0.5
       stop_line_margin: 3.0
+      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_vel_thr: 0.833 # 0.833m/s = 3.0km/h

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

@@ -105,6 +105,10 @@ class IntersectionModule : public SceneModuleInterface
   {
     double state_transit_margin_time;
     double stop_line_margin;  //! distance from auto-generated stopline to detection_area boundary
+    double keep_detection_line_margin;  //! distance (toward path end) from generated stop line.
+                                        //! keep detection if ego is before this line and ego.vel <
+                                        //! keep_detection_vel_thr
+    double keep_detection_vel_thr;
     double stuck_vehicle_detect_dist;  //! distance from end point to finish stuck vehicle check
     double
       stuck_vehicle_ignore_dist;   //! distance from intersection start to start stuck vehicle check

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

@@ -39,7 +39,7 @@ int insertPoint(
   autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path);
 
 bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id);
-bool hasDuplicatedPoint(
+bool getDuplicatedPointIdx(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const geometry_msgs::msg::Point & point, int * duplicated_point_idx);
 
@@ -54,6 +54,14 @@ bool getObjectiveLanelets(
   std::vector<lanelet::ConstLanelets> * objective_lanelets_with_margin_result,
   const rclcpp::Logger logger);
 
+struct StopLineIdx
+{
+  int first_idx_inside_lane = -1;
+  int pass_judge_line_idx = -1;
+  int stop_line_idx = -1;
+  int keep_detection_line_idx = -1;
+};
+
 /**
  * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map,
  * read it from the map; otherwise, generate a stop line at a position where it will not collide.
@@ -68,9 +76,10 @@ bool getObjectiveLanelets(
 bool generateStopLine(
   const int lane_id, const std::vector<lanelet::CompoundPolygon3d> detection_areas,
   const std::shared_ptr<const PlannerData> & planner_data, const double stop_line_margin,
+  const double keep_detection_line_margin,
   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx,
-  int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger);
+  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.
@@ -118,6 +127,22 @@ std::vector<int> getLaneletIdsFromLaneletsVec(const std::vector<lanelet::ConstLa
 lanelet::ConstLanelet generateOffsetLanelet(
   const lanelet::ConstLanelet lanelet, double right_margin, double left_margin);
 geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p);
+
+/**
+ * @brief check if ego is over the target_idx. If the index is same, compare the exact pose
+ * @param path path
+ * @param closest_idx ego's closest index on the path
+ * @param current_pose ego's exact pose
+ * @return true if ego is over the target_idx
+ */
+bool isOverTargetIndex(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
+  const geometry_msgs::msg::Pose & current_pose, const int target_idx);
+
+bool isBeforeTargetIndex(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
+  const geometry_msgs::msg::Pose & current_pose, const int target_idx);
+
 }  // namespace util
 }  // namespace behavior_velocity_planner
 

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

@@ -34,6 +34,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
   ip.state_transit_margin_time = node.declare_parameter(ns + ".state_transit_margin_time", 2.0);
   ip.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0);
+  ip.keep_detection_line_margin = node.declare_parameter(ns + ".keep_detection_line_margin", 1.0);
+  ip.keep_detection_vel_thr = node.declare_parameter(ns + ".keep_detection_vel_thr", 0.833);
   ip.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle_detect_dist", 3.0);
   ip.stuck_vehicle_ignore_dist = node.declare_parameter(ns + ".stuck_vehicle_ignore_dist", 5.0) +
                                  vehicle_info.max_longitudinal_offset_m;

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

@@ -122,17 +122,19 @@ bool IntersectionModule::modifyPathVelocity(
   debug_data_.detection_area_with_margin = detection_areas_with_margin;
 
   /* set stop-line and stop-judgement-line for base_link */
-  int stop_line_idx = -1;
-  int pass_judge_line_idx = -1;
-  int first_idx_inside_lane = -1;
+  util::StopLineIdx stop_line_idxs;
   if (!util::generateStopLine(
-        lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, *path,
-        &stop_line_idx, &pass_judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) {
+        lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin,
+        planner_param_.keep_detection_line_margin, path, *path, &stop_line_idxs,
+        logger_.get_child("util"))) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");
     RCLCPP_DEBUG(logger_, "===== plan end =====");
     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;
   if (stop_line_idx <= 0) {
     RCLCPP_DEBUG(logger_, "stop line line is at path[0], ignore planning.");
     RCLCPP_DEBUG(logger_, "===== plan end =====");
@@ -152,21 +154,32 @@ bool IntersectionModule::modifyPathVelocity(
   }
   const size_t closest_idx = closest_idx_opt.get();
 
-  /* if current_state = GO, and current_pose is in front of stop_line, ignore planning. */
-  bool is_over_pass_judge_line =
-    static_cast<bool>(static_cast<int>(closest_idx) > pass_judge_line_idx);
-  if (static_cast<int>(closest_idx) == pass_judge_line_idx) {
-    geometry_msgs::msg::Pose pass_judge_line = path->points.at(pass_judge_line_idx).point.pose;
-    is_over_pass_judge_line = planning_utils::isAheadOf(current_pose.pose, pass_judge_line);
-  }
-  if (is_go_out_ && is_over_pass_judge_line && !external_stop) {
-    RCLCPP_DEBUG(logger_, "over the pass judge line. 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.
+  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
+     */
+    const bool is_before_keep_detection_line =
+      util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx);
+    if (
+      is_before_keep_detection_line && std::fabs(planner_data_->current_velocity->twist.linear.x) <
+                                         planner_param_.keep_detection_vel_thr) {
+      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) {
+      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.
+    }
   }
 
   /* get dynamic object */
@@ -201,22 +214,24 @@ bool IntersectionModule::modifyPathVelocity(
   if (!isActivated()) {
     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;
     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 = stuck_stop_line_idx;
-        pass_judge_line_idx = stuck_pass_judge_line_idx;
+        stop_line_idx_stop = stuck_stop_line_idx;
+        pass_judge_line_idx_stop = stuck_pass_judge_line_idx;
       }
     }
-    planning_utils::setVelocityFromIndex(stop_line_idx, v, path);
+    planning_utils::setVelocityFromIndex(stop_line_idx_stop, v, path);
     debug_data_.stop_required = true;
     debug_data_.stop_wall_pose =
-      planning_utils::getAheadPose(stop_line_idx, base_link2front, *path);
-    debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose;
-    debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx).point.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;
 
     /* 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

@@ -83,27 +83,27 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
   debug_data_.detection_area = conflicting_areas;
 
   /* set stop-line and stop-judgement-line for base_link */
-  int stop_line_idx = -1;
-  int judge_line_idx = -1;
-  int first_idx_inside_lane = -1;
+  util::StopLineIdx stop_line_idxs;
   const auto private_path =
     extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m);
   if (!util::generateStopLine(
-        lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path,
-        private_path, &stop_line_idx, &judge_line_idx, &first_idx_inside_lane,
+        lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin,
+        0.0 /* unnecessary in merge_from_private */, path, private_path, &stop_line_idxs,
         logger_.get_child("util"))) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");
     return false;
   }
 
-  if (stop_line_idx <= 0 || judge_line_idx <= 0) {
-    RCLCPP_DEBUG(logger_, "stop line or judge line is at path[0], ignore planning.");
+  const int stop_line_idx = stop_line_idxs.stop_line_idx;
+  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;
   }