@@ -69,25 +69,15 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
69
69
pull_out_path.partial_paths .front (); // shift path is not separate but only one.
70
70
71
71
// 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 {};
73
73
{
74
74
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 );
75
77
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 );
91
81
}
92
82
93
83
// extract shoulder lanes from pull out lanes
@@ -131,7 +121,7 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
131
121
// check lane departure
132
122
if (
133
123
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 )) {
135
125
continue ;
136
126
}
137
127
0 commit comments