@@ -720,33 +720,36 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(
720
720
721
721
const double collision_check_distance_from_end = collision_check_distances.at (planner_type);
722
722
723
- PathWithLaneId combined_path ;
723
+ PathWithLaneId full_path ;
724
724
for (const auto & partial_path : path.partial_paths ) {
725
- combined_path.points .insert (
726
- combined_path.points .end (), partial_path.points .begin (), partial_path.points .end ());
727
- }
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);
734
- // calculate collision check end idx
735
- const size_t collision_check_end_idx = std::invoke ([&]() {
736
- const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose (
737
- combined_path.points , path.end_pose .position , collision_check_distance_from_end);
725
+ full_path.points .insert (
726
+ full_path.points .end (), partial_path.points .begin (), partial_path.points .end ());
727
+ }
738
728
739
- if (collision_check_end_pose) {
740
- return motion_utils::findNearestIndex (
741
- combined_path.points , collision_check_end_pose->position );
742
- } else {
743
- return combined_path.points .size () - 1 ;
744
- }
745
- });
746
- // remove the point behind of collision check end pose
747
- combined_path.points .erase (
748
- combined_path.points .begin () + collision_check_end_idx + 1 , combined_path.points .end ());
749
- return combined_path;
729
+ // Find the start index for collision check section based on the shift start pose
730
+ const auto shift_start_idx =
731
+ motion_utils::findNearestIndex (full_path.points , path.start_pose .position );
732
+
733
+ // Find the end index for collision check section based on the end pose and collision check
734
+ // distance
735
+ const auto collision_check_end_idx = [&]() -> size_t {
736
+ const auto end_pose_offset = motion_utils::calcLongitudinalOffsetPose (
737
+ full_path.points , path.end_pose .position , collision_check_distance_from_end);
738
+
739
+ return end_pose_offset
740
+ ? motion_utils::findNearestIndex (full_path.points , end_pose_offset->position )
741
+ : full_path.points .size () - 1 ; // Use the last point if offset pose is not calculable
742
+ }();
743
+
744
+ // Extract the collision check section from the full path
745
+ PathWithLaneId collision_check_section;
746
+ if (shift_start_idx < collision_check_end_idx) {
747
+ collision_check_section.points .assign (
748
+ full_path.points .begin () + shift_start_idx,
749
+ full_path.points .begin () + collision_check_end_idx + 1 );
750
+ }
751
+
752
+ return collision_check_section;
750
753
}
751
754
752
755
void StartPlannerModule::updateStatusWithCurrentPath (
0 commit comments