@@ -624,7 +624,9 @@ bool GoalPlannerModule::canReturnToLaneParking()
624
624
625
625
if (
626
626
parameters_->use_object_recognition &&
627
- checkObjectsCollision (path, parameters_->object_recognition_collision_check_margin )) {
627
+ checkObjectsCollision (
628
+ path, parameters_->object_recognition_collision_check_margin ,
629
+ /* extract_static_objects=*/ false )) {
628
630
return false ;
629
631
}
630
632
@@ -711,7 +713,9 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
711
713
const auto resampled_path =
712
714
utils::resamplePathWithSpline (pull_over_path.getParkingPath (), 0.5 );
713
715
for (const auto & scale_factor : scale_factors) {
714
- if (!checkObjectsCollision (resampled_path, margin * scale_factor)) {
716
+ if (!checkObjectsCollision (
717
+ resampled_path, margin * scale_factor,
718
+ /* extract_static_objects=*/ true )) {
715
719
return margin * scale_factor;
716
720
}
717
721
}
@@ -771,8 +775,10 @@ std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectP
771
775
772
776
const auto resampled_path = utils::resamplePathWithSpline (pull_over_path.getParkingPath (), 0.5 );
773
777
if (
774
- parameters_->use_object_recognition &&
775
- checkObjectsCollision (resampled_path, collision_check_margin, true /* update_debug_data*/ )) {
778
+ parameters_->use_object_recognition && checkObjectsCollision (
779
+ resampled_path, collision_check_margin,
780
+ /* extract_static_objects=*/ true ,
781
+ /* update_debug_data=*/ true )) {
776
782
continue ;
777
783
}
778
784
@@ -914,6 +920,7 @@ bool GoalPlannerModule::hasNotDecidedPath() const
914
920
DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus () const
915
921
{
916
922
const auto & prev_status = prev_data_.deciding_path_status ;
923
+ const bool enable_safety_check = parameters_->safety_check_params .enable_safety_check ;
917
924
918
925
// Once this function returns true, it will continue to return true thereafter
919
926
if (prev_status.first == DecidingPathStatus::DECIDED) {
@@ -927,8 +934,7 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const
927
934
928
935
// If it is dangerous against dynamic objects before approval, do not determine the path.
929
936
// This eliminates a unsafe path to be approved
930
- if (
931
- parameters_->safety_check_params .enable_safety_check && !isSafePath ().first && !isActivated ()) {
937
+ if (enable_safety_check && !isSafePath ().first && !isActivated ()) {
932
938
RCLCPP_DEBUG (
933
939
getLogger (),
934
940
" [DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before "
@@ -957,13 +963,20 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const
957
963
const auto parking_path = utils::resamplePathWithSpline (pull_over_path->getParkingPath (), 0.5 );
958
964
const double margin =
959
965
parameters_->object_recognition_collision_check_margin * hysteresis_factor;
960
- if (checkObjectsCollision (parking_path, margin)) {
966
+ if (checkObjectsCollision (parking_path, margin, /* extract_static_objects= */ false )) {
961
967
RCLCPP_DEBUG (
962
968
getLogger (),
963
969
" [DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects" );
964
970
return {DecidingPathStatus::NOT_DECIDED, clock_->now ()};
965
971
}
966
972
973
+ if (enable_safety_check && !isSafePath ().first ) {
974
+ RCLCPP_DEBUG (
975
+ getLogger (),
976
+ " [DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects" );
977
+ return {DecidingPathStatus::NOT_DECIDED, clock_->now ()};
978
+ }
979
+
967
980
// if enough time has passed since deciding status starts, transition to DECIDED
968
981
constexpr double check_collision_duration = 1.0 ;
969
982
const double elapsed_time_from_deciding = (clock_->now () - prev_status.second ).seconds ();
@@ -1406,7 +1419,7 @@ bool GoalPlannerModule::isStuck()
1406
1419
parameters_->use_object_recognition &&
1407
1420
checkObjectsCollision (
1408
1421
thread_safe_data_.get_pull_over_path ()->getCurrentPath (),
1409
- parameters_->object_recognition_collision_check_margin )) {
1422
+ /* extract_static_objects= */ false , parameters_->object_recognition_collision_check_margin )) {
1410
1423
return true ;
1411
1424
}
1412
1425
@@ -1519,16 +1532,24 @@ bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path)
1519
1532
1520
1533
bool GoalPlannerModule::checkObjectsCollision (
1521
1534
const PathWithLaneId & path, const double collision_check_margin,
1522
- const bool update_debug_data) const
1523
- {
1524
- const auto pull_over_lane_stop_objects =
1525
- goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes (
1526
- *(planner_data_->route_handler ), left_side_parking_, parameters_->backward_goal_search_length ,
1527
- parameters_->forward_goal_search_length , parameters_->detection_bound_offset ,
1528
- *(planner_data_->dynamic_object ), parameters_->th_moving_object_velocity );
1535
+ const bool extract_static_objects, const bool update_debug_data) const
1536
+ {
1537
+ const auto target_objects = std::invoke ([&]() {
1538
+ const auto & p = parameters_;
1539
+ const auto & rh = *(planner_data_->route_handler );
1540
+ const auto objects = *(planner_data_->dynamic_object );
1541
+ if (extract_static_objects) {
1542
+ return goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes (
1543
+ rh, left_side_parking_, p->backward_goal_search_length , p->forward_goal_search_length ,
1544
+ p->detection_bound_offset , objects, p->th_moving_object_velocity );
1545
+ }
1546
+ return goal_planner_utils::extractObjectsInExpandedPullOverLanes (
1547
+ rh, left_side_parking_, p->backward_goal_search_length , p->forward_goal_search_length ,
1548
+ p->detection_bound_offset , objects);
1549
+ });
1529
1550
1530
1551
std::vector<Polygon2d> obj_polygons;
1531
- for (const auto & object : pull_over_lane_stop_objects .objects ) {
1552
+ for (const auto & object : target_objects .objects ) {
1532
1553
obj_polygons.push_back (tier4_autoware_utils::toPolygon2d (object));
1533
1554
}
1534
1555
0 commit comments