@@ -862,38 +862,26 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
862
862
target_objects.other_lane .reserve (target_obj_index.other_lane .size ());
863
863
864
864
// objects in current lane
865
-
866
- lanelet::ConstLanelet current_lane;
867
- if (getRouteHandler ()->getClosestLaneletWithinRoute (getEgoPose (), ¤t_lane)) {
868
- }
869
- const auto ego_footprint =
870
- utils::lane_change::getEgoCurrentFootprint (getEgoPose (), common_parameters.vehicle_info );
871
- const auto is_ego_within_intersection =
872
- utils::lane_change::isWithinIntersection (getRouteHandler (), current_lane, ego_footprint);
873
-
874
- const auto check_prepare_phase =
875
- is_ego_within_intersection || lane_change_parameters_->enable_prepare_segment_collision_check ;
876
-
877
865
for (const auto & obj_idx : target_obj_index.current_lane ) {
878
866
const auto extended_object = utils::lane_change::transform (
879
867
objects.objects .at (obj_idx), common_parameters, *lane_change_parameters_,
880
- check_prepare_phase);
868
+ check_prepare_phase () );
881
869
target_objects.current_lane .push_back (extended_object);
882
870
}
883
871
884
872
// objects in target lane
885
873
for (const auto & obj_idx : target_obj_index.target_lane ) {
886
874
const auto extended_object = utils::lane_change::transform (
887
875
objects.objects .at (obj_idx), common_parameters, *lane_change_parameters_,
888
- check_prepare_phase);
876
+ check_prepare_phase () );
889
877
target_objects.target_lane .push_back (extended_object);
890
878
}
891
879
892
880
// objects in other lane
893
881
for (const auto & obj_idx : target_obj_index.other_lane ) {
894
882
const auto extended_object = utils::lane_change::transform (
895
883
objects.objects .at (obj_idx), common_parameters, *lane_change_parameters_,
896
- check_prepare_phase);
884
+ check_prepare_phase () );
897
885
target_objects.other_lane .push_back (extended_object);
898
886
}
899
887
@@ -951,25 +939,14 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
951
939
target_backward_polygons.push_back (lane_polygon);
952
940
}
953
941
954
- lanelet::ConstLanelet current_lane;
955
- if (!route_handler.getClosestLaneletWithinRoute (getEgoPose (), ¤t_lane)) {
956
- }
957
- const auto ego_footprint =
958
- utils::lane_change::getEgoCurrentFootprint (getEgoPose (), common_parameters.vehicle_info );
959
- const auto is_ego_within_intersection =
960
- utils::lane_change::isWithinIntersection (getRouteHandler (), current_lane, ego_footprint);
961
-
962
- const auto check_prepare_phase =
963
- is_ego_within_intersection || lane_change_parameters_->enable_prepare_segment_collision_check ;
964
-
965
942
LaneChangeTargetObjectIndices filtered_obj_indices;
966
943
for (size_t i = 0 ; i < objects.objects .size (); ++i) {
967
944
const auto & object = objects.objects .at (i);
968
945
const auto obj_velocity_norm = std::hypot (
969
946
object.kinematics .initial_twist_with_covariance .twist .linear .x ,
970
947
object.kinematics .initial_twist_with_covariance .twist .linear .y );
971
948
const auto extended_object = utils::lane_change::transform (
972
- object, common_parameters, *lane_change_parameters_, check_prepare_phase);
949
+ object, common_parameters, *lane_change_parameters_, check_prepare_phase () );
973
950
974
951
const auto obj_polygon = tier4_autoware_utils::toPolygon2d (object);
975
952
@@ -2108,4 +2085,29 @@ void NormalLaneChange::updateStopTime()
2108
2085
stop_watch_.tic (" stop_time" );
2109
2086
}
2110
2087
2088
+ bool NormalLaneChange::check_prepare_phase () const
2089
+ {
2090
+ const auto & route_handler = getRouteHandler ();
2091
+ const auto & vehicle_info = getCommonParam ().vehicle_info ;
2092
+
2093
+ const auto check_prepare_phase_in_intersection = std::invoke ([&]() {
2094
+ if (!lane_change_parameters_->enable_prepare_segment_collision_check_in_intersection ) {
2095
+ return false ;
2096
+ }
2097
+
2098
+ lanelet::ConstLanelet current_lane;
2099
+ if (!route_handler->getClosestLaneletWithinRoute (getEgoPose (), ¤t_lane)) {
2100
+ RCLCPP_DEBUG (logger_, " unable to retreive current lane" );
2101
+ return false ;
2102
+ }
2103
+
2104
+ const auto ego_footprint =
2105
+ utils::lane_change::getEgoCurrentFootprint (getEgoPose (), vehicle_info);
2106
+ return utils::lane_change::isWithinIntersection (getRouteHandler (), current_lane, ego_footprint);
2107
+ });
2108
+
2109
+ return check_prepare_phase_in_intersection ||
2110
+ lane_change_parameters_->enable_prepare_segment_collision_check_in_general_areas ;
2111
+ }
2112
+
2111
2113
} // namespace behavior_path_planner
0 commit comments