@@ -683,27 +683,36 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(
683
683
684
684
const double collision_check_distance_from_end = collision_check_distances.at (planner_type);
685
685
686
- PathWithLaneId combined_path ;
686
+ PathWithLaneId full_path ;
687
687
for (const auto & partial_path : path.partial_paths ) {
688
- combined_path .points .insert (
689
- combined_path .points .end (), partial_path.points .begin (), partial_path.points .end ());
688
+ full_path .points .insert (
689
+ full_path .points .end (), partial_path.points .begin (), partial_path.points .end ());
690
690
}
691
- // calculate collision check end idx
692
- const size_t collision_check_end_idx = std::invoke ([&]() {
693
- const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose (
694
- combined_path.points , path.end_pose .position , collision_check_distance_from_end);
695
691
696
- if (collision_check_end_pose) {
697
- return motion_utils::findNearestIndex (
698
- combined_path.points , collision_check_end_pose->position );
699
- } else {
700
- return combined_path.points .size () - 1 ;
701
- }
702
- });
703
- // remove the point behind of collision check end pose
704
- combined_path.points .erase (
705
- combined_path.points .begin () + collision_check_end_idx + 1 , combined_path.points .end ());
706
- return combined_path;
692
+ // Find the start index for collision check section based on the shift start pose
693
+ const auto shift_start_idx =
694
+ motion_utils::findNearestIndex (full_path.points , path.start_pose .position );
695
+
696
+ // Find the end index for collision check section based on the end pose and collision check
697
+ // distance
698
+ const auto collision_check_end_idx = [&]() -> size_t {
699
+ const auto end_pose_offset = motion_utils::calcLongitudinalOffsetPose (
700
+ full_path.points , path.end_pose .position , collision_check_distance_from_end);
701
+
702
+ return end_pose_offset
703
+ ? motion_utils::findNearestIndex (full_path.points , end_pose_offset->position )
704
+ : full_path.points .size () - 1 ; // Use the last point if offset pose is not calculable
705
+ }();
706
+
707
+ // Extract the collision check section from the full path
708
+ PathWithLaneId collision_check_section;
709
+ if (shift_start_idx < collision_check_end_idx) {
710
+ collision_check_section.points .assign (
711
+ full_path.points .begin () + shift_start_idx,
712
+ full_path.points .begin () + collision_check_end_idx + 1 );
713
+ }
714
+
715
+ return collision_check_section;
707
716
}
708
717
709
718
void StartPlannerModule::updateStatusWithCurrentPath (
0 commit comments