@@ -615,29 +615,11 @@ void GoalPlannerModule::updateData()
615
615
universe_utils::ScopedTimeTrack st (__func__, *time_keeper_);
616
616
617
617
// extract static and dynamic objects in extraction polygon for path collision check
618
- const auto & p = parameters_;
619
- const auto & rh = *(planner_data_->route_handler );
620
- const auto objects = *(planner_data_->dynamic_object );
621
- const double vehicle_width = planner_data_->parameters .vehicle_width ;
622
- const auto pull_over_lanes = goal_planner_utils::getPullOverLanes (
623
- rh, left_side_parking_, p->backward_goal_search_length , p->forward_goal_search_length );
624
- const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon (
625
- pull_over_lanes, left_side_parking_, p->detection_bound_offset ,
626
- p->margin_from_boundary + p->max_lateral_offset + vehicle_width);
627
- if (objects_extraction_polygon.has_value ()) {
628
- debug_data_.objects_extraction_polygon = objects_extraction_polygon.value ();
629
- }
630
- PredictedObjects dynamic_target_objects{};
631
- for (const auto & object : objects.objects ) {
632
- const auto object_polygon = universe_utils::toPolygon2d (object);
633
- if (
634
- objects_extraction_polygon.has_value () &&
635
- boost::geometry::intersects (object_polygon, objects_extraction_polygon.value ())) {
636
- dynamic_target_objects.objects .push_back (object);
637
- }
638
- }
618
+ const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects (
619
+ *(planner_data_->dynamic_object ), *(planner_data_->route_handler ), *parameters_,
620
+ planner_data_->parameters .vehicle_width );
639
621
const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity (
640
- dynamic_target_objects, p ->th_moving_object_velocity );
622
+ dynamic_target_objects, parameters_ ->th_moving_object_velocity );
641
623
642
624
// update goal searcher and generate goal candidates
643
625
if (goal_candidates_.empty ()) {
@@ -753,47 +735,17 @@ bool GoalPlannerModule::isExecutionRequested() const
753
735
// check if goal_pose is in current_lanes or neighboring road lanes
754
736
const lanelet::ConstLanelets current_lanes =
755
737
utils::getCurrentLanesFromPath (getPreviousModuleOutput ().reference_path , planner_data_);
756
- const auto getNeighboringLane =
757
- [&](const lanelet::ConstLanelet & lane) -> std::optional<lanelet::ConstLanelet> {
758
- return left_side_parking_ ? route_handler->getLeftLanelet (lane, false , true )
759
- : route_handler->getRightLanelet (lane, false , true );
760
- };
761
- lanelet::ConstLanelets goal_check_lanes = current_lanes;
762
- for (const auto & lane : current_lanes) {
763
- auto neighboring_lane = getNeighboringLane (lane);
764
- while (neighboring_lane) {
765
- goal_check_lanes.push_back (neighboring_lane.value ());
766
- neighboring_lane = getNeighboringLane (neighboring_lane.value ());
767
- }
768
- }
769
- const bool goal_is_in_current_segment_lanes = std::any_of (
770
- goal_check_lanes.begin (), goal_check_lanes.end (), [&](const lanelet::ConstLanelet & lane) {
771
- return lanelet::utils::isInLanelet (goal_pose, lane);
772
- });
773
-
774
- // check that goal is in current neighbor shoulder lane
775
- const bool goal_is_in_current_shoulder_lanes = std::invoke ([&]() {
776
- for (const auto & lane : current_lanes) {
777
- const auto shoulder_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet (lane)
778
- : route_handler->getRightShoulderLanelet (lane);
779
- if (shoulder_lane && lanelet::utils::isInLanelet (goal_pose, *shoulder_lane)) {
780
- return true ;
781
- }
782
- }
783
- return false ;
784
- });
785
-
786
- // if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner,
787
- // because goal arc coordinates cannot be calculated.
788
- if (!goal_is_in_current_segment_lanes && !goal_is_in_current_shoulder_lanes) {
738
+ const bool is_goal_reachable = goal_planner_utils::is_goal_reachable_on_path (
739
+ current_lanes, *(planner_data_->route_handler ), left_side_parking_);
740
+ if (!is_goal_reachable) {
789
741
return false ;
790
742
}
791
743
792
744
// if goal modification is not allowed
793
745
// 1) goal_pose is in current_lanes, plan path to the original fixed goal
794
746
// 2) goal_pose is NOT in current_lanes, do not execute goal_planner
795
747
if (!utils::isAllowedGoalModification (route_handler)) {
796
- return goal_is_in_current_segment_lanes ;
748
+ return is_goal_reachable ;
797
749
}
798
750
799
751
// if goal arc coordinates can be calculated, check if goal is in request_length
0 commit comments