Skip to content

Commit c831fcf

Browse files
kyoichi-sugaharakarishma1911
authored andcommitted
fix(start_planner): expand lane departure check for shift path (#6055)
Refactor lane departure check in ShiftPullOut::plan() Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent f0deba8 commit c831fcf

File tree

1 file changed

+7
-17
lines changed

1 file changed

+7
-17
lines changed

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

+7-17
Original file line numberDiff line numberDiff line change
@@ -69,25 +69,15 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
6969
pull_out_path.partial_paths.front(); // shift path is not separate but only one.
7070

7171
// check lane_departure with path between pull_out_start to pull_out_end
72-
PathWithLaneId path_start_to_end{};
72+
PathWithLaneId path_shift_start_to_end{};
7373
{
7474
const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position);
75+
const size_t pull_out_end_idx =
76+
findNearestIndex(shift_path.points, pull_out_path.end_pose.position);
7577

76-
// calculate collision check end idx
77-
const size_t collision_check_end_idx = std::invoke([&]() {
78-
const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
79-
shift_path.points, pull_out_path.end_pose.position,
80-
parameters_.collision_check_distance_from_end);
81-
82-
if (collision_check_end_pose) {
83-
return findNearestIndex(shift_path.points, collision_check_end_pose->position);
84-
} else {
85-
return shift_path.points.size() - 1;
86-
}
87-
});
88-
path_start_to_end.points.insert(
89-
path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx,
90-
shift_path.points.begin() + collision_check_end_idx + 1);
78+
path_shift_start_to_end.points.insert(
79+
path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx,
80+
shift_path.points.begin() + pull_out_end_idx + 1);
9181
}
9282

9383
// extract shoulder lanes from pull out lanes
@@ -131,7 +121,7 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
131121
// check lane departure
132122
if (
133123
parameters_.check_shift_path_lane_departure &&
134-
lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) {
124+
lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_shift_start_to_end)) {
135125
continue;
136126
}
137127

0 commit comments

Comments
 (0)