Revert "Preempt PR3711(2) (#13)"

This reverts commit 7fd41ab.

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

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -183,12 +183,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
       planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0));
   }
 
-  const std::optional<size_t> stuck_line_idx_opt =
-    first_conflicting_area
-      ? util::generateStuckStopLine(
-          first_conflicting_area.value(), planner_data_, planner_param_.common.stop_line_margin,
-          planner_param_.stuck_vehicle.use_stuck_stopline, path, path_ip, interval,
-          lane_interval_ip, logger_.get_child("util"))
+  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 =
@@ -198,21 +196,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
                              lane_interval_ip, logger_.get_child("util"))
                          : std::nullopt;
 
-  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_,
-          planner_param_.collision_detection.keep_detection_vel_thr)
-      : std::nullopt;
-
-  const auto occlusion_peeking_line_idx_opt =
-    first_detection_area
-      ? util::generatePeekingLimitLine(
-          first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_,
-          planner_param_.occlusion.peeking_offset)
-      : std::nullopt;
-
-  // TODO(Mamoru Sobue): check the ordering of these stop lines and refactor
   /* calc closest index */
   const auto closest_idx_opt =
     motion_utils::findNearestIndex(path->points, current_pose, 3.0, M_PI_4);
@@ -237,7 +220,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     // if ego is over the pass judge line and not stopped
     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 && !keep_detection) {
+    } 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;
@@ -259,6 +242,13 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     generateStuckVehicleDetectAreaPolygon(*path, ego_lane_with_next_lane, closest_idx);
   const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area);
   debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area);
+  const std::optional<size_t> stuck_line_idx_opt =
+    first_conflicting_area
+      ? util::generateStuckStopLine(
+          first_conflicting_area.value(), planner_data_, planner_param_.common.stop_line_margin,
+          planner_param_.stuck_vehicle.use_stuck_stopline, path, path_ip, interval,
+          lane_interval_ip, logger_.get_child("util"))
+      : std::nullopt;
 
   /* calculate dynamic collision around detection area */
   /* set stop lines for base_link */
@@ -281,6 +271,12 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
           first_detection_area.value(), path_ip, lane_interval_ip, detection_divisions_.value(),
           occlusion_dist_thr)
       : true;
+  const auto occlusion_peeking_line_idx_opt =
+    first_detection_area
+      ? util::generatePeekingLimitLine(
+          first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_,
+          planner_param_.occlusion.peeking_offset)
+      : std::nullopt;
 
   /* calculate final stop lines */
   std::optional<size_t> stop_line_idx = default_stop_line_idx_opt;
@@ -430,13 +426,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     setSafe(collision_state_machine_.getState() == StateMachine::State::GO);
   }
 
-  RCLCPP_DEBUG(
-    logger_,
-    "has_collision = %d, is_occlusion_cleared = %d, collision_stop_required = %d, "
-    "first_phase_stop_required = %d, occlusion_stop_required = %d",
-    has_collision, is_occlusion_cleared, collision_stop_required, first_phase_stop_required,
-    occlusion_stop_required);
-
   /* make decision */
   if (!occlusion_activated_) {
     is_go_out_ = false;

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -134,15 +134,15 @@ std::optional<size_t> generateStaticPassJudgeLine(
   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval,
   const std::pair<size_t, size_t> lane_interval,
-  const std::shared_ptr<const PlannerData> & planner_data, const double velocity)
+  const std::shared_ptr<const PlannerData> & planner_data)
 {
-  // const double velocity = planner_data->current_velocity->twist.linear.x;
-  // const double acceleration = planner_data->current_acceleration->accel.accel.linear.x;
+  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 max_stop_jerk = planner_data->max_stop_jerk_threshold;
   // const double delay_response_time = planner_data->delay_response_time;
-  const double offset =
-    -planning_utils::calcJudgeLineDistWithAccLimit(velocity, max_stop_acceleration, 0.0);
+  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) {

planning/behavior_velocity_intersection_module/src/util.hpp

@@ -108,7 +108,7 @@ std::optional<size_t> generateStaticPassJudgeLine(
   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval,
   const std::pair<size_t, size_t> lane_interval,
-  const std::shared_ptr<const PlannerData> & planner_data, const double velocity);
+  const std::shared_ptr<const PlannerData> & planner_data);
 
 std::optional<size_t> generatePeekingLimitLine(
   const lanelet::CompoundPolygon3d & first_detection_area,