@@ -657,8 +657,8 @@ bool StartPlannerModule::findPullOutPath(
657
657
658
658
// check collision
659
659
if (utils::checkCollisionBetweenPathFootprintsAndObjects (
660
- vehicle_footprint, extractCollisionCheckSection (*pull_out_path), pull_out_lane_stop_objects ,
661
- collision_check_margin)) {
660
+ vehicle_footprint, extractCollisionCheckSection (*pull_out_path, planner-> getPlannerType ()) ,
661
+ pull_out_lane_stop_objects, collision_check_margin)) {
662
662
return false ;
663
663
}
664
664
@@ -672,8 +672,17 @@ bool StartPlannerModule::findPullOutPath(
672
672
return true ;
673
673
}
674
674
675
- PathWithLaneId StartPlannerModule::extractCollisionCheckSection (const PullOutPath & path)
675
+ PathWithLaneId StartPlannerModule::extractCollisionCheckSection (
676
+ const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type)
676
677
{
678
+ const std::map<PlannerType, double > collision_check_distances = {
679
+ {behavior_path_planner::PlannerType::SHIFT,
680
+ parameters_->shift_collision_check_distance_from_end },
681
+ {behavior_path_planner::PlannerType::GEOMETRIC,
682
+ parameters_->geometric_collision_check_distance_from_end }};
683
+
684
+ const double collision_check_distance_from_end = collision_check_distances.at (planner_type);
685
+
677
686
PathWithLaneId combined_path;
678
687
for (const auto & partial_path : path.partial_paths ) {
679
688
combined_path.points .insert (
@@ -682,7 +691,7 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPat
682
691
// calculate collision check end idx
683
692
const size_t collision_check_end_idx = std::invoke ([&]() {
684
693
const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose (
685
- combined_path.points , path.end_pose .position , parameters_-> collision_check_distance_from_end );
694
+ combined_path.points , path.end_pose .position , collision_check_distance_from_end);
686
695
687
696
if (collision_check_end_pose) {
688
697
return motion_utils::findNearestIndex (
@@ -1371,9 +1380,14 @@ void StartPlannerModule::setDebugData()
1371
1380
// visualize collision_check_end_pose and footprint
1372
1381
{
1373
1382
const auto local_footprint = vehicle_info_.createFootprint ();
1383
+ std::map<PlannerType, double > collision_check_distances = {
1384
+ {PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end },
1385
+ {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end }};
1386
+
1387
+ double collision_check_distance_from_end = collision_check_distances[status_.planner_type ];
1374
1388
const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose (
1375
1389
getFullPath ().points , status_.pull_out_path .end_pose .position ,
1376
- parameters_-> collision_check_distance_from_end );
1390
+ collision_check_distance_from_end);
1377
1391
if (collision_check_end_pose) {
1378
1392
add (createPoseMarkerArray (
1379
1393
*collision_check_end_pose, " static_collision_check_end_pose" , 0 , 1.0 , 0.0 , 0.0 ));
0 commit comments