From dabcd4d62e2af7e86bb9c22ab2ce391f85db98cf Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 19 Nov 2024 18:04:24 +0900 Subject: [PATCH 1/7] RT1-8537 check object's predicted path when filtering Signed-off-by: Zulfaqar Azmi --- .../utils/utils.hpp | 3 ++ .../src/utils/utils.cpp | 28 +++++++++++++------ 2 files changed, 22 insertions(+), 9 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..d9b14e06c1dcd 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,8 @@ 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); + +bool has_path_overlapped_target_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/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 9ab6ac288dfd4..3555fa784c9be 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 @@ -1164,10 +1164,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 +1172,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 has_path_overlapped_target_lanes(object, common_data_ptr->lanes_polygon_ptr->target); }; return std::any_of( @@ -1206,9 +1201,15 @@ 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 overlapped_target_lanes = std::invoke([&]() { + if (!boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target)) { + return true; + } + + return !is_stopped && has_path_overlapped_target_lanes(object, lanes_polygon.target); + }); + + if (overlapped_target_lanes) { if (!ahead_of_ego && !is_stopped) { trailing_objects.push_back(object); return true; @@ -1247,4 +1248,13 @@ bool filter_target_lane_objects( return false; } + +bool has_path_overlapped_target_lanes( + const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon) +{ + const auto paths = get_line_string_paths(object); + return ranges::any_of(paths, [&](const LineString2d & path) { + return !boost::geometry::disjoint(path, lanes_polygon); + }); +} } // namespace autoware::behavior_path_planner::utils::lane_change From 26913f69ade40ef10348061310aa25a9c3d04802 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 22 Nov 2024 11:47:52 +0900 Subject: [PATCH 2/7] use ranges view in get_line_string_paths Signed-off-by: Zulfaqar Azmi --- .../src/utils/utils.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) 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 3555fa784c9be..839fe9ea58156 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( @@ -1252,8 +1251,7 @@ bool filter_target_lane_objects( bool has_path_overlapped_target_lanes( const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon) { - const auto paths = get_line_string_paths(object); - return ranges::any_of(paths, [&](const LineString2d & path) { + return ranges::any_of(get_line_string_paths(object), [&](const auto & path) { return !boost::geometry::disjoint(path, lanes_polygon); }); } From e496e661883d64576a30191ffae8b15eb2f1799f Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 22 Nov 2024 16:54:10 +0900 Subject: [PATCH 3/7] check only vehicle type predicted path Signed-off-by: Zulfaqar Azmi --- .../src/scene.cpp | 1 + .../src/utils/utils.cpp | 4 +++- .../path_safety_checker/objects_filtering.hpp | 10 ++++++++++ .../path_safety_checker/objects_filtering.cpp | 14 ++++++++++++++ 4 files changed, 28 insertions(+), 1 deletion(-) 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 839fe9ea58156..b122dbb8fc5e5 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 @@ -1184,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; @@ -1205,7 +1206,8 @@ bool filter_target_lane_objects( return true; } - return !is_stopped && has_path_overlapped_target_lanes(object, lanes_polygon.target); + return !is_stopped && is_vehicle(object.classification) && + has_path_overlapped_target_lanes(object, lanes_polygon.target); }); if (overlapped_target_lanes) { 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 From 027f2bbe0e6928d05ce5fb47170964822ba748df Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Thu, 28 Nov 2024 14:33:55 +0900 Subject: [PATCH 4/7] Refactor naming Signed-off-by: Zulfaqar Azmi --- .../utils/utils.hpp | 16 +++++++++++++++- .../src/utils/utils.cpp | 6 +++--- 2 files changed, 18 insertions(+), 4 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 d9b14e06c1dcd..fc91ee9556ba2 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 @@ -383,7 +383,21 @@ bool filter_target_lane_objects( const bool before_terminal, TargetLaneLeadingObjects & leading_objects, ExtendedPredictedObjects & trailing_objects); -bool has_path_overlapped_target_lanes( +/** + * @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 is_object_path_overlapped_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/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index b122dbb8fc5e5..6966ff335a444 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 @@ -1171,7 +1171,7 @@ bool has_overtaking_turn_lane_object( return true; } - return has_path_overlapped_target_lanes(object, common_data_ptr->lanes_polygon_ptr->target); + return is_object_path_overlapped_lanes(object, common_data_ptr->lanes_polygon_ptr->target); }; return std::any_of( @@ -1207,7 +1207,7 @@ bool filter_target_lane_objects( } return !is_stopped && is_vehicle(object.classification) && - has_path_overlapped_target_lanes(object, lanes_polygon.target); + is_object_path_overlapped_lanes(object, lanes_polygon.target); }); if (overlapped_target_lanes) { @@ -1250,7 +1250,7 @@ bool filter_target_lane_objects( return false; } -bool has_path_overlapped_target_lanes( +bool is_object_path_overlapped_lanes( const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon) { return ranges::any_of(get_line_string_paths(object), [&](const auto & path) { From df978cb08d1ab32faca0c9ea91bf0f75a5bf20d8 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 29 Nov 2024 15:58:40 +0900 Subject: [PATCH 5/7] fix grammatical Signed-off-by: Zulfaqar Azmi --- .../behavior_path_lane_change_module/utils/utils.hpp | 2 +- .../src/utils/utils.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 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 fc91ee9556ba2..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 @@ -397,7 +397,7 @@ bool filter_target_lane_objects( * @return true if any of the object's predicted paths overlap with the lanes polygon, false * otherwise. */ -bool is_object_path_overlapped_lanes( +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/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 6966ff335a444..094176b5ffa18 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 @@ -1171,7 +1171,7 @@ bool has_overtaking_turn_lane_object( return true; } - return is_object_path_overlapped_lanes(object, common_data_ptr->lanes_polygon_ptr->target); + return object_path_overlaps_lanes(object, common_data_ptr->lanes_polygon_ptr->target); }; return std::any_of( @@ -1207,7 +1207,7 @@ bool filter_target_lane_objects( } return !is_stopped && is_vehicle(object.classification) && - is_object_path_overlapped_lanes(object, lanes_polygon.target); + object_path_overlaps_lanes(object, lanes_polygon.target); }); if (overlapped_target_lanes) { @@ -1250,7 +1250,7 @@ bool filter_target_lane_objects( return false; } -bool is_object_path_overlapped_lanes( +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) { From e3ae83594f8c78659a228e012c824ae323e5fb20 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 9 Dec 2024 19:50:53 +0900 Subject: [PATCH 6/7] 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> --- .../src/utils/utils.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) 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 094176b5ffa18..4b1391542c4c3 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 @@ -1201,14 +1201,7 @@ 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 overlapped_target_lanes = std::invoke([&]() { - if (!boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target)) { - return true; - } - - return !is_stopped && is_vehicle(object.classification) && - object_path_overlaps_lanes(object, lanes_polygon.target); - }); + const auto overlapped_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 (overlapped_target_lanes) { if (!ahead_of_ego && !is_stopped) { From 567959a54ae75c0eeded2cd211ac7159bbee9a4a Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Mon, 9 Dec 2024 19:57:11 +0900 Subject: [PATCH 7/7] precommit and grammar fix Signed-off-by: Zulfaqar Azmi --- .../src/utils/utils.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) 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 4b1391542c4c3..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 @@ -1201,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 overlapped_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)); + 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 (overlapped_target_lanes) { + if (overlapping_with_target_lanes) { if (!ahead_of_ego && !is_stopped) { trailing_objects.push_back(object); return true;