Skip to content

Commit f878a81

Browse files
kyoichi-sugaharashmpwk
authored andcommitted
fix(start_planner): remove front of shift start pose from collision check section (autowarefoundation#6374)
* remove front of shift start pose Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * Refactor collision check section extraction in StartPlannerModule Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent a1d36f1 commit f878a81

File tree

1 file changed

+27
-18
lines changed

1 file changed

+27
-18
lines changed

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+27-18
Original file line numberDiff line numberDiff line change
@@ -683,27 +683,36 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(
683683

684684
const double collision_check_distance_from_end = collision_check_distances.at(planner_type);
685685

686-
PathWithLaneId combined_path;
686+
PathWithLaneId full_path;
687687
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());
690690
}
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);
695691

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;
707716
}
708717

709718
void StartPlannerModule::updateStatusWithCurrentPath(

0 commit comments

Comments
 (0)