Preempt PR3711(2) (#13)

* fix(intersection): use 0 offset peeking line Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * use braking distance Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * add pass_judge_wall_pose marker Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * keep detection if ego is between stopline and pass judge line and velocity is below threshold Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * modify condition Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * use 0.0s for delay response time Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * right turn scenario passes Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * find all stop line candidates at first in order Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * use keep_detection_vel for staticPassJudgeLine Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> --------- Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
autowarefoundation · May 22, 2023 · 7fd41ab · 7fd41ab
1 parent c13a37b
commit 7fd41ab
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 25 deletions.
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -181,10 +181,12 @@ 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_)
+  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;
 
   const auto default_stop_line_idx_opt =
@@ -194,6 +196,21 @@ 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);
@@ -218,7 +235,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) {
+    } else if (is_over_default_stop_line && is_over_pass_judge_line && !keep_detection) {
       RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed.");
       RCLCPP_DEBUG(logger_, "===== plan end =====");
       return true;
@@ -240,13 +257,6 @@ 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 */
@@ -269,12 +279,6 @@ 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;
@@ -414,6 +418,13 @@ 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;

diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/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 std::shared_ptr<const PlannerData> & planner_data, const double velocity)
 {
-  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::calcJudgeLineDistWithJerkLimit(
-    velocity, acceleration, max_stop_acceleration, max_stop_jerk, 0.0);
+  const double offset =
+    -planning_utils::calcJudgeLineDistWithAccLimit(velocity, max_stop_acceleration, 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) {

diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/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 std::shared_ptr<const PlannerData> & planner_data, const double velocity);
 
 std::optional<size_t> generatePeekingLimitLine(
   const lanelet::CompoundPolygon3d & first_detection_area,