Skip to content

Commit bb3bb3f

Browse files
Refactor collision check section extraction in StartPlannerModule
Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent b87e88c commit bb3bb3f

File tree

1 file changed

+28
-25
lines changed

1 file changed

+28
-25
lines changed

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+28-25
Original file line numberDiff line numberDiff line change
@@ -720,33 +720,36 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(
720720

721721
const double collision_check_distance_from_end = collision_check_distances.at(planner_type);
722722

723-
PathWithLaneId combined_path;
723+
PathWithLaneId full_path;
724724
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+
}
738728

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;
750753
}
751754

752755
void StartPlannerModule::updateStatusWithCurrentPath(

0 commit comments

Comments
 (0)