Skip to content

Commit d6700a4

Browse files
committed
fix(merge_from_private): fix stop position calculation (autowarefoundation#6286)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 520dbf6 commit d6700a4

File tree

1 file changed

+7
-1
lines changed

1 file changed

+7
-1
lines changed

planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp

+7-1
Original file line numberDiff line numberDiff line change
@@ -129,7 +129,13 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
129129
if (!first_conflicting_idx_opt) {
130130
return false;
131131
}
132-
const auto stopline_idx_ip = first_conflicting_idx_opt.value();
132+
// ==========================================================================================
133+
// first_conflicting_idx is calculated considering baselink2front already, so there is no need
134+
// to subtract baselink2front/ds here
135+
// ==========================================================================================
136+
const auto stopline_idx_ip = static_cast<size_t>(std::max<int>(
137+
0, static_cast<int>(first_conflicting_idx_opt.value()) -
138+
static_cast<int>(planner_param_.stopline_margin / planner_param_.path_interpolation_ds)));
133139

134140
const auto stopline_idx_opt = util::insertPointIndex(
135141
interpolated_path_info.path.points.at(stopline_idx_ip).point.pose, path,

0 commit comments

Comments
 (0)