preempt PR3711 (#11)

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -195,6 +195,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       debug_data_.predicted_obj_pose, "predicted_obj_pose", module_id_, 0.7, 0.85, 0.9),
     &debug_marker_array, now);
 
+  appendMarkerArray(
+    createPoseMarkerArray(
+      debug_data_.pass_judge_wall_pose, "pass_judge_wall_pose", module_id_, 0.7, 0.85, 0.9),
+    &debug_marker_array, now);
+
   return debug_marker_array;
 }
 

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -112,6 +112,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   occlusion_stop_distance_ = std::numeric_limits<double>::lowest();
   occlusion_first_stop_required_ = false;
 
+  const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
   /* get current pose */
   const geometry_msgs::msg::Pose current_pose = planner_data_->current_odometry->pose;
 
@@ -180,6 +181,19 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
       planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0));
   }
 
+  const auto static_pass_judge_line_opt =
+    first_detection_area
+      ? util::generateStaticPassJudgeLine(
+          first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_)
+      : std::nullopt;
+
+  const auto default_stop_line_idx_opt =
+    first_detection_area ? util::generateCollisionStopLine(
+                             lane_id_, first_detection_area.value(), planner_data_,
+                             planner_param_.common.stop_line_margin, path, path_ip, interval,
+                             lane_interval_ip, logger_.get_child("util"))
+                         : std::nullopt;
+
   /* calc closest index */
   const auto closest_idx_opt =
     motion_utils::findNearestIndex(path->points, current_pose, 3.0, M_PI_4);
@@ -191,20 +205,20 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   }
   const size_t closest_idx = closest_idx_opt.get();
 
-  const auto static_pass_judge_line_opt =
-    first_detection_area
-      ? util::generateStaticPassJudgeLine(
-          first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_)
-      : std::nullopt;
-
-  if (static_pass_judge_line_opt) {
+  if (static_pass_judge_line_opt && default_stop_line_idx_opt) {
     const auto pass_judge_line_idx = static_pass_judge_line_opt.value();
+    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 is_over_default_stop_line =
+      util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx_opt.value());
     const double vel = std::fabs(planner_data_->current_velocity->twist.linear.x);
     const bool keep_detection = (vel < planner_param_.collision_detection.keep_detection_vel_thr);
     // if ego is over the pass judge line and not stopped
-    if (is_over_pass_judge_line && is_go_out_ && !keep_detection) {
+    if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) {
+      /* do nothing */
+    } else if (is_over_default_stop_line && is_over_pass_judge_line) {
       RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed.");
       RCLCPP_DEBUG(logger_, "===== plan end =====");
       return true;
@@ -236,12 +250,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 
   /* calculate dynamic collision around detection area */
   /* set stop lines for base_link */
-  const auto default_stop_line_idx_opt =
-    first_detection_area ? util::generateCollisionStopLine(
-                             lane_id_, first_detection_area.value(), planner_data_,
-                             planner_param_.common.stop_line_margin, path, path_ip, interval,
-                             lane_interval_ip, logger_.get_child("util"))
-                         : std::nullopt;
   const double time_delay = is_go_out_
                               ? 0.0
                               : (planner_param_.collision_detection.state_transit_margin_time -
@@ -407,7 +415,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   }
 
   /* make decision */
-  const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
   if (!occlusion_activated_) {
     is_go_out_ = false;
 

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -54,6 +54,7 @@ class IntersectionModule : public SceneModuleInterface
     geometry_msgs::msg::Pose collision_stop_wall_pose;
     geometry_msgs::msg::Pose occlusion_stop_wall_pose;
     geometry_msgs::msg::Pose occlusion_first_stop_wall_pose;
+    geometry_msgs::msg::Pose pass_judge_wall_pose;
     geometry_msgs::msg::Polygon stuck_vehicle_detect_area;
     geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon;
     std::vector<geometry_msgs::msg::Polygon> candidate_collision_object_polygons;

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -136,28 +136,19 @@ std::optional<size_t> generateStaticPassJudgeLine(
   const std::pair<size_t, size_t> lane_interval,
   const std::shared_ptr<const PlannerData> & planner_data)
 {
-  const auto pass_judge_line_idx_ip =
-    util::getFirstPointInsidePolygon(path_ip, lane_interval, first_detection_area);
-  if (!pass_judge_line_idx_ip) {
-    return std::nullopt;
-  }
-  const int base2front_idx_dist =
-    std::ceil(planner_data->vehicle_info_.vehicle_length_m / ip_interval);
-  const int idx = static_cast<int>(pass_judge_line_idx_ip.value()) - base2front_idx_dist;
-  if (idx < 0) {
-    return std::nullopt;
-  }
-  const auto & insert_point = path_ip.points.at(static_cast<size_t>(idx)).point.pose;
-  const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position);
-  if (duplicate_idx_opt) {
-    return duplicate_idx_opt;
-  } else {
-    const auto insert_idx_opt = util::insertPoint(insert_point, original_path);
-    if (!insert_idx_opt) {
-      return std::nullopt;
-    }
-    return insert_idx_opt;
-  }
+  const double velocity = planner_data->current_velocity->twist.linear.x;
+  const double acceleration = planner_data->current_acceleration->accel.accel.linear.x;
+  const double max_stop_acceleration = planner_data->max_stop_acceleration_threshold;
+  const double max_stop_jerk = planner_data->max_stop_jerk_threshold;
+  // const double delay_response_time = planner_data->delay_response_time;
+  const double offset = -planning_utils::calcJudgeLineDistWithJerkLimit(
+    velocity, acceleration, max_stop_acceleration, max_stop_jerk, 0.0);
+  const auto pass_judge_line_idx = generatePeekingLimitLine(
+    first_detection_area, original_path, path_ip, ip_interval, lane_interval, planner_data, offset);
+  if (pass_judge_line_idx) {
+    return pass_judge_line_idx;
+  }
+  return 0;
 }
 
 std::optional<size_t> generatePeekingLimitLine(