From d67d132196fda249e254877c80b9293394524766 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Thu, 1 Feb 2024 19:07:59 +0900
Subject: [PATCH] fix(merge_from_private): fix stop position calculation
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
.../src/scene_merge_from_private_road.cpp | 6 +++++-
1 file changed, 5 insertions(+), 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
index 8b0511c741b4e..67da3c7a759fe 100644
--- 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 calculated 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,