fix(merge_from_private): fix stop position calculation

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
autowarefoundation · Feb 2, 2024 · b801d7e · b801d7e
1 parent a3b92d1
commit b801d7e
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp
@@ -126,9 +126,13 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
   if (!first_conflicting_idx_opt) {
     return false;
   }
+  // ==========================================================================================
+  // first_conflicting_idx is calcualted considering baselink2front already, so there is no need
+  // to subtract baselink2front/ds here
+  // ==========================================================================================
   const auto stopline_idx_ip = static_cast<size_t>(std::max<int>(
     0, static_cast<int>(first_conflicting_idx_opt.value()) -
-         static_cast<int>(baselink2front / planner_param_.path_interpolation_ds)));
+         static_cast<int>(planner_param_.stopline_margin / planner_param_.path_interpolation_ds)));
 
   const auto stopline_idx_opt = util::insertPointIndex(
     interpolated_path_info.path.points.at(stopline_idx_ip).point.pose, path,