fix(intersection): additional fix for autowarefoundation#2463 (autowa…

…refoundation#2565) * always set RTC distance to default stop line Signed-off-by: Mamoru Sobue <[email protected]> * add code owner Signed-off-by: Mamoru Sobue <[email protected]>
sujithvemi · Dec 22, 2022 · 7ecbbc7 · 7ecbbc7
1 parent 14b8ee7
commit 7ecbbc7
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 25 deletions.
diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml
@@ -7,6 +7,7 @@
 
   <!-- intersection module -->
   <maintainer email="[email protected]">Mamoru Sobue</maintainer>
+  <maintainer email="[email protected]">Takayuki Murooka</maintainer>
   <!-- crosswalk module -->
   <maintainer email="[email protected]">Satoshi Ota</maintainer>
   <!-- detection area module -->

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
@@ -150,6 +150,33 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   }
   const size_t closest_idx = closest_idx_opt.get();
 
+  if (stop_lines_idx_opt.has_value()) {
+    const auto stop_line_idx = stop_lines_idx_opt.value().stop_line;
+    const auto pass_judge_line_idx = stop_lines_idx_opt.value().pass_judge_line;
+    const auto keep_detection_line_idx = stop_lines_idx_opt.value().keep_detection_line;
+
+    const bool is_over_pass_judge_line =
+      util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx);
+    const bool is_before_keep_detection_line =
+      stop_lines_idx_opt.has_value()
+        ? util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx)
+        : false;
+    const bool keep_detection = is_before_keep_detection_line &&
+                                std::fabs(current_vel) < planner_param_.keep_detection_vel_thr;
+
+    if (is_over_pass_judge_line && keep_detection) {
+      // in case ego could not stop exactly before the stop line, but with some overshoot,
+      // keep detection within some margin under low velocity threshold
+    } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) {
+      RCLCPP_INFO(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;
+    }
+  }
   /* collision checking */
   bool is_entry_prohibited = false;
 
@@ -171,8 +198,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     closest_idx, stuck_vehicle_detect_area);
 
   /* calculate final stop lines */
-  int stop_line_idx_final = -1;
-  int pass_judge_line_idx_final = -1;
+  int stop_line_idx_final =
+    stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().stop_line : -1;
+  int pass_judge_line_idx_final =
+    stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().pass_judge_line : -1;
   if (external_go) {
     is_entry_prohibited = false;
   } else if (external_stop) {
@@ -204,29 +233,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     return false;
   }
 
-  const bool is_over_pass_judge_line =
-    util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx_final);
-  const bool is_before_keep_detection_line =
-    stop_lines_idx_opt.has_value()
-      ? util::isBeforeTargetIndex(
-          *path, closest_idx, current_pose.pose, stop_lines_idx_opt.value().keep_detection_line)
-      : false;
-  const bool keep_detection =
-    is_before_keep_detection_line && std::fabs(current_vel) < planner_param_.keep_detection_vel_thr;
-
-  if (is_over_pass_judge_line && keep_detection) {
-    // in case ego could not stop exactly before the stop line, but with some overshoot,
-    // keep detection within some margin under low velocity threshold
-  } else if (is_over_pass_judge_line && 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_final).point.pose.position));
-    return true;
-  }
-
   state_machine_.setStateWithMarginTime(
     is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO,
     logger_.get_child("state_machine"), *clock_);