Skip to content

Commit b87e88c

Browse files
remove front of shift start pose
Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent 4409d20 commit b87e88c

File tree

1 file changed

+6
-0
lines changed

1 file changed

+6
-0
lines changed

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+6
Original file line numberDiff line numberDiff line change
@@ -725,6 +725,12 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(
725725
combined_path.points.insert(
726726
combined_path.points.end(), partial_path.points.begin(), partial_path.points.end());
727727
}
728+
// calculate shift start pose idx
729+
const size_t shift_start_idx =
730+
motion_utils::findNearestIndex(combined_path.points, path.start_pose.position);
731+
// remove front of start pose
732+
combined_path.points.erase(
733+
combined_path.points.begin(), combined_path.points.begin() + shift_start_idx);
728734
// calculate collision check end idx
729735
const size_t collision_check_end_idx = std::invoke([&]() {
730736
const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(

0 commit comments

Comments
 (0)