use the centerline of first attention lane for second pass judge line

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

planning/behavior_velocity_intersection_module/README.md

@@ -298,7 +298,7 @@ $$
 
 is called pass_judge_line, and safety decision must be made before ego passes this position because ego does not stop anymore.
 
-1st_pass_judge_line is before the first upcoming lane, and at intersections with multiple upcoming lanes, 2nd_pass_judge_line is defined as the position which is before the boundary of second attention lane by the braking distance plus `common.second_pass_judge_line_margin`. 1st/2nd_pass_judge_line are illustrated in the following figure.
+1st_pass_judge_line is before the first upcoming lane, and at intersections with multiple upcoming lanes, 2nd_pass_judge_line is defined as the position which is before the centerline of the first attention lane by the braking distance. 1st/2nd_pass_judge_line are illustrated in the following figure.
 
 ![pass-judge-line](./docs/pass-judge-line.drawio.svg)
 
@@ -414,7 +414,6 @@ entity TargetObject {
 | `.use_intersection_area`                     | bool   | [-] flag to use intersection_area for collision detection                        |
 | `.default_stopline_margin`                   | double | [m] margin before_stop_line                                                      |
 | `.stopline_overshoot_margin`                 | double | [m] margin for the overshoot from stopline                                       |
-| `.second_pass_judge_line_margin`             | double | [m] extra margin for the second pass judge line from the second attention lane   |
 | `.max_accel`                                 | double | [m/ss] max acceleration for stop                                                 |
 | `.max_jerk`                                  | double | [m/sss] max jerk for stop                                                        |
 | `.delay_response_time`                       | double | [s] action delay before stop                                                     |

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -7,7 +7,6 @@
         attention_area_angle_threshold: 0.785
         use_intersection_area: false
         default_stopline_margin: 3.0
-        second_pass_judge_line_margin: 1.0
         stopline_overshoot_margin: 0.5
         path_interpolation_ds: 0.1
         max_accel: -2.8

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -53,8 +53,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<bool>(node, ns + ".common.use_intersection_area");
   ip.common.default_stopline_margin =
     getOrDeclareParameter<double>(node, ns + ".common.default_stopline_margin");
-  ip.common.second_pass_judge_line_margin =
-    getOrDeclareParameter<double>(node, ns + ".common.second_pass_judge_line_margin");
   ip.common.stopline_overshoot_margin =
     getOrDeclareParameter<double>(node, ns + ".common.stopline_overshoot_margin");
   ip.common.path_interpolation_ds =

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1714,7 +1714,6 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
   const double max_jerk = planner_param_.common.max_jerk;
   const double delay_response_time = planner_param_.common.delay_response_time;
   const double peeking_offset = planner_param_.occlusion.peeking_offset;
-  const double second_pass_judge_line_margin = planner_param_.common.second_pass_judge_line_margin;
 
   const auto first_attention_area = first_attention_lane.polygon3d();
   const auto first_attention_lane_centerline = first_attention_lane.centerline2d();
@@ -1855,8 +1854,7 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
 
   // (8) second pass judge line position on interpolated path. It is the same as first pass judge
   // line if second_attention_lane is null
-  int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds) -
-                                 std::ceil(second_pass_judge_line_margin / ds);
+  int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip;
   const auto second_pass_judge_line_ip =
     second_attention_area_opt ? static_cast<size_t>(std::max<int>(second_pass_judge_ip_int, 0))
                               : first_pass_judge_line_ip;

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -342,7 +342,6 @@ class IntersectionModule : public SceneModuleInterface
       double attention_area_angle_threshold;
       bool use_intersection_area;
       double default_stopline_margin;
-      double second_pass_judge_line_margin;
       double stopline_overshoot_margin;
       double path_interpolation_ds;
       double max_accel;