From dbdf9a16d83267c4d9a5f99b0e442c9f997ab2c7 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 9 Dec 2024 21:26:07 +0900 Subject: [PATCH] fix(lane_change): check obj predicted path when filtering (#9385) * RT1-8537 check object's predicted path when filtering Signed-off-by: Zulfaqar Azmi * use ranges view in get_line_string_paths Signed-off-by: Zulfaqar Azmi * check only vehicle type predicted path Signed-off-by: Zulfaqar Azmi * Refactor naming Signed-off-by: Zulfaqar Azmi * fix grammatical Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * precommit and grammar fix Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- .../utils/utils.hpp | 17 ++++++++++ .../src/scene.cpp | 1 + .../src/utils/utils.cpp | 34 +++++++++++-------- .../path_safety_checker/objects_filtering.hpp | 10 ++++++ .../path_safety_checker/objects_filtering.cpp | 14 ++++++++ 5 files changed, 62 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 7c1c8e2c65abe..5dcd132f16377 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -382,5 +382,22 @@ bool filter_target_lane_objects( const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, const bool before_terminal, TargetLaneLeadingObjects & leading_objects, ExtendedPredictedObjects & trailing_objects); + +/** + * @brief Determines if the object's predicted path overlaps with the given lane polygon. + * + * This function checks whether any of the line string paths derived from the object's predicted + * trajectories intersect or overlap with the specified polygon representing lanes. + * + * @param object The extended predicted object containing predicted trajectories and initial + * polygon. + * @param lanes_polygon A polygon representing the lanes to check for overlaps with the object's + * paths. + * + * @return true if any of the object's predicted paths overlap with the lanes polygon, false + * otherwise. + */ +bool object_path_overlaps_lanes( + const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon); } // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 99e7cc73e59ff..2770831dbc485 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -988,6 +988,7 @@ lane_change::TargetObjects NormalLaneChange::get_target_objects( FilteredLanesObjects NormalLaneChange::filter_objects() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( objects, lane_change_parameters_->safety.target_object_types); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 9ab6ac288dfd4..4d46f8270fac3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -35,6 +35,8 @@ #include #include #include +#include +#include #include #include @@ -1149,11 +1151,8 @@ std::vector get_line_string_paths(const ExtendedPredictedObject & return line_string; }; - const auto paths = object.predicted_paths; - std::vector line_strings; - std::transform(paths.begin(), paths.end(), std::back_inserter(line_strings), to_linestring_2d); - - return line_strings; + return object.predicted_paths | ranges::views::transform(to_linestring_2d) | + ranges::to(); } bool has_overtaking_turn_lane_object( @@ -1164,10 +1163,6 @@ bool has_overtaking_turn_lane_object( return false; } - const auto is_path_overlap_with_target = [&](const LineString2d & path) { - return !boost::geometry::disjoint(path, common_data_ptr->lanes_polygon_ptr->target); - }; - const auto is_object_overlap_with_target = [&](const auto & object) { // to compensate for perception issue, or if object is from behind ego, and tries to overtake, // but stop all of sudden @@ -1176,8 +1171,7 @@ bool has_overtaking_turn_lane_object( return true; } - const auto paths = get_line_string_paths(object); - return std::any_of(paths.begin(), paths.end(), is_path_overlap_with_target); + return object_path_overlaps_lanes(object, common_data_ptr->lanes_polygon_ptr->target); }; return std::any_of( @@ -1190,6 +1184,7 @@ bool filter_target_lane_objects( const bool before_terminal, TargetLaneLeadingObjects & leading_objects, ExtendedPredictedObjects & trailing_objects) { + using behavior_path_planner::utils::path_safety_checker::filter::is_vehicle; using behavior_path_planner::utils::path_safety_checker::filter::velocity_filter; const auto & current_lanes = common_data_ptr->lanes_ptr->current; const auto & vehicle_width = common_data_ptr->bpp_param_ptr->vehicle_info.vehicle_width_m; @@ -1206,9 +1201,12 @@ bool filter_target_lane_objects( const auto is_stopped = velocity_filter( object.initial_twist, -std::numeric_limits::epsilon(), stopped_obj_vel_th); if (is_lateral_far && before_terminal) { - const auto in_target_lanes = - !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target); - if (in_target_lanes) { + const auto overlapping_with_target_lanes = + !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target) || + (!is_stopped && is_vehicle(object.classification) && + object_path_overlaps_lanes(object, lanes_polygon.target)); + + if (overlapping_with_target_lanes) { if (!ahead_of_ego && !is_stopped) { trailing_objects.push_back(object); return true; @@ -1247,4 +1245,12 @@ bool filter_target_lane_objects( return false; } + +bool object_path_overlaps_lanes( + const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon) +{ + return ranges::any_of(get_line_string_paths(object), [&](const auto & path) { + return !boost::geometry::disjoint(path, lanes_polygon); + }); +} } // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 5c29f347ad9ef..2ed9fdd9197df 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -71,6 +71,16 @@ bool is_within_circle( const geometry_msgs::msg::Point & object_pos, const geometry_msgs::msg::Point & reference_point, const double search_radius); +/** + * @brief Checks if the object classification represents a vehicle (CAR, TRUCK, BUS, TRAILER, + * MOTORCYCLE). + * + * @param classification The object classification to check. + * @return true If the classification is a vehicle type. + * @return false Otherwise. + */ +bool is_vehicle(const ObjectClassification & classification); + } // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter namespace autoware::behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 1caf287ee5386..00e9a0f567e7c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -57,6 +57,20 @@ bool is_within_circle( std::hypot(reference_point.x - object_pos.x, reference_point.y - object_pos.y); return dist < search_radius; } + +bool is_vehicle(const ObjectClassification & classification) +{ + switch (classification.label) { + case ObjectClassification::CAR: + case ObjectClassification::TRUCK: + case ObjectClassification::BUS: + case ObjectClassification::TRAILER: + case ObjectClassification::MOTORCYCLE: + return true; + default: + return false; + } +} } // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter namespace autoware::behavior_path_planner::utils::path_safety_checker