@@ -1867,6 +1867,51 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData(
1867
1867
goal_planner_data_.ego_predicted_path = ego_predicted_path;
1868
1868
}
1869
1869
1870
+ static std::vector<utils::path_safety_checker::ExtendedPredictedObject> filterObjectsByWithinPolicy (
1871
+ const std::shared_ptr<const PredictedObjects> & objects,
1872
+ const lanelet::ConstLanelets & target_lanes,
1873
+ const std::shared_ptr<behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams> &
1874
+ params)
1875
+ {
1876
+ // implanted part of behavior_path_planner::utils::path_safety_checker::filterObjects() and
1877
+ // createTargetObjectsOnLane()
1878
+
1879
+ // Guard
1880
+ if (objects->objects .empty ()) {
1881
+ return {};
1882
+ }
1883
+
1884
+ const double ignore_object_velocity_threshold = params->ignore_object_velocity_threshold ;
1885
+ const auto & target_object_types = params->object_types_to_check ;
1886
+
1887
+ PredictedObjects filtered_objects = utils::path_safety_checker::filterObjectsByVelocity (
1888
+ *objects, ignore_object_velocity_threshold, false );
1889
+
1890
+ utils::path_safety_checker::filterObjectsByClass (filtered_objects, target_object_types);
1891
+
1892
+ std::vector<PredictedObject> within_filtered_objects;
1893
+ for (const auto & target_lane : target_lanes) {
1894
+ const auto lane_poly = target_lane.polygon2d ().basicPolygon ();
1895
+ for (const auto & filtered_object : filtered_objects.objects ) {
1896
+ const auto object_bbox = tier4_autoware_utils::toPolygon2d (filtered_object);
1897
+ if (boost::geometry::within (object_bbox, lane_poly)) {
1898
+ within_filtered_objects.push_back (filtered_object);
1899
+ break ;
1900
+ }
1901
+ }
1902
+ }
1903
+
1904
+ const double safety_check_time_horizon = params->safety_check_time_horizon ;
1905
+ const double safety_check_time_resolution = params->safety_check_time_resolution ;
1906
+
1907
+ std::vector<utils::path_safety_checker::ExtendedPredictedObject> refined_filtered_objects;
1908
+ for (const auto & within_filtered_object : within_filtered_objects) {
1909
+ refined_filtered_objects.push_back (utils::path_safety_checker::transform (
1910
+ within_filtered_object, safety_check_time_horizon, safety_check_time_resolution));
1911
+ }
1912
+ return refined_filtered_objects;
1913
+ }
1914
+
1870
1915
std::pair<bool , bool > GoalPlannerModule::isSafePath () const
1871
1916
{
1872
1917
if (!thread_safe_data_.get_pull_over_path ()) {
@@ -1904,31 +1949,30 @@ std::pair<bool, bool> GoalPlannerModule::isSafePath() const
1904
1949
ego_predicted_path_params_, pull_over_path.points , current_pose, current_velocity,
1905
1950
ego_seg_idx, is_object_front, limit_to_max_velocity);
1906
1951
1907
- // filtering objects with velocity, position and class
1908
- const auto filtered_objects = utils::path_safety_checker::filterObjects (
1909
- dynamic_object, route_handler, pull_over_lanes, current_pose.position ,
1910
- objects_filtering_params_);
1911
-
1912
1952
// filtering objects based on the current position's lane
1913
- const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane (
1914
- pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_);
1953
+ const auto expanded_pull_over_lanes_between_ego =
1954
+ goal_planner_utils::generateBetweenEgoAndExpandedPullOverLanes (
1955
+ pull_over_lanes, left_side_parking_, current_pose, planner_data_->parameters .vehicle_info ,
1956
+ parameters_->outer_road_detection_offset , parameters_->inner_road_detection_offset );
1957
+ const auto merged_expanded_pull_over_lanes =
1958
+ lanelet::utils::combineLaneletsShape (expanded_pull_over_lanes_between_ego);
1959
+ debug_data_.expanded_pull_over_lane_between_ego = merged_expanded_pull_over_lanes;
1915
1960
1916
- utils::parking_departure::updateSafetyCheckTargetObjectsData (
1917
- goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path );
1961
+ const auto filtered_objects = filterObjectsByWithinPolicy (
1962
+ dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params_ );
1918
1963
1919
1964
const double hysteresis_factor =
1920
1965
prev_data_.safety_status .is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate ;
1921
1966
1922
1967
const bool current_is_safe = std::invoke ([&]() {
1923
1968
if (parameters_->safety_check_params .method == " RSS" ) {
1924
1969
return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS (
1925
- pull_over_path, ego_predicted_path, target_objects_on_lane.on_current_lane ,
1926
- goal_planner_data_.collision_check , planner_data_->parameters ,
1927
- safety_check_params_->rss_params , objects_filtering_params_->use_all_predicted_path ,
1928
- hysteresis_factor);
1970
+ pull_over_path, ego_predicted_path, filtered_objects, goal_planner_data_.collision_check ,
1971
+ planner_data_->parameters , safety_check_params_->rss_params ,
1972
+ objects_filtering_params_->use_all_predicted_path , hysteresis_factor);
1929
1973
} else if (parameters_->safety_check_params .method == " integral_predicted_polygon" ) {
1930
1974
return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon (
1931
- ego_predicted_path, vehicle_info_, target_objects_on_lane. on_current_lane ,
1975
+ ego_predicted_path, vehicle_info_, filtered_objects ,
1932
1976
objects_filtering_params_->check_all_predicted_path ,
1933
1977
parameters_->safety_check_params .integral_predicted_polygon_params ,
1934
1978
goal_planner_data_.collision_check );
@@ -2046,6 +2090,13 @@ void GoalPlannerModule::setDebugData()
2046
2090
}
2047
2091
debug_marker_.markers .push_back (marker);
2048
2092
2093
+ tier4_autoware_utils::appendMarkerArray (
2094
+ goal_planner_utils::createLaneletPolygonMarkerArray (
2095
+ debug_data_.expanded_pull_over_lane_between_ego .polygon3d (), header,
2096
+ " expanded_pull_over_lane_between_ego" ,
2097
+ tier4_autoware_utils::createMarkerColor (1.0 , 0.7 , 0.0 , 0.999 )),
2098
+ &debug_marker_);
2099
+
2049
2100
// Visualize debug poses
2050
2101
const auto & debug_poses = thread_safe_data_.get_pull_over_path ()->debug_poses ;
2051
2102
for (size_t i = 0 ; i < debug_poses.size (); ++i) {
0 commit comments