From a0c4ab45d78f64ef9c7c1022af2c8f61682ae32a Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Mon, 18 Nov 2024 09:39:46 +0900 Subject: [PATCH] refactor(lane_change): refactor extended object safety check (#9322) * refactor LC extended object collision check code Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --------- Signed-off-by: mohammad alqudah Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../scene.hpp | 7 ++-- .../utils/utils.hpp | 5 +-- .../src/scene.cpp | 42 +++++++++++++------ .../src/utils/utils.cpp | 15 ++----- 4 files changed, 40 insertions(+), 29 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 4c8cbe05f5535..dd21233c716f1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -169,12 +169,13 @@ class NormalLaneChange : public LaneChangeBase bool has_collision_with_decel_patterns( const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, const size_t deceleration_sampling_num, const RSSparams & rss_param, - CollisionCheckDebugMap & debug_data) const; + const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const; bool is_collided( - const PathWithLaneId & lane_change_path, const ExtendedPredictedObject & obj, + const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj, const std::vector & ego_predicted_path, - const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const; + const RSSparams & selected_rss_param, const bool check_prepare_phase, + CollisionCheckDebugMap & debug_data) const; double get_max_velocity_for_safety_check() const; 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 747a991b038e9..edf64ecdc49ee 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 @@ -145,7 +145,7 @@ lanelet::BasicPolygon2d create_polygon( ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase); + const LaneChangeParameters & lane_change_parameters); bool is_collided_polygons_in_lanelet( const std::vector & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon); @@ -249,8 +249,7 @@ bool is_before_terminal( double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose); ExtendedPredictedObjects transform_to_extended_objects( - const CommonDataPtr & common_data_ptr, const std::vector & objects, - const bool check_prepare_phase); + const CommonDataPtr & common_data_ptr, const std::vector & objects); double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false, 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 1e102a8b7542d..3f06ff0481c7e 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 @@ -1006,17 +1006,16 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return is_within_vel_th(object) && ahead_of_ego; }); - const auto is_check_prepare_phase = check_prepare_phase(); const auto target_lane_leading_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.target_lane_leading, is_check_prepare_phase); + common_data_ptr_, filtered_by_lanes_objects.target_lane_leading); const auto target_lane_trailing_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing, is_check_prepare_phase); + common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing); const auto current_lane_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.current_lane, is_check_prepare_phase); + common_data_ptr_, filtered_by_lanes_objects.current_lane); const auto other_lane_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.other_lane, is_check_prepare_phase); + common_data_ptr_, filtered_by_lanes_objects.other_lane); FilteredByLanesExtendedObjects lane_change_target_objects( current_lane_extended_objects, target_lane_leading_extended_objects, @@ -1856,10 +1855,13 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( return {is_safe, !is_object_behind_ego}; } + const auto is_check_prepare_phase = check_prepare_phase(); + const auto all_decel_pattern_has_collision = [&](const utils::path_safety_checker::ExtendedPredictedObjects & objects) -> bool { return has_collision_with_decel_patterns( - lane_change_path, objects, deceleration_sampling_num, rss_params, debug_data); + lane_change_path, objects, deceleration_sampling_num, rss_params, is_check_prepare_phase, + debug_data); }; if (all_decel_pattern_has_collision(collision_check_objects.trailing)) { @@ -1876,7 +1878,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( bool NormalLaneChange::has_collision_with_decel_patterns( const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, const size_t deceleration_sampling_num, const RSSparams & rss_param, - CollisionCheckDebugMap & debug_data) const + const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const { if (objects.empty()) { return false; @@ -1922,7 +1924,8 @@ bool NormalLaneChange::has_collision_with_decel_patterns( ? lane_change_parameters_->rss_params_for_parked : rss_param; return is_collided( - lane_change_path.path, obj, ego_predicted_path, selected_rss_param, debug_data); + lane_change_path, obj, ego_predicted_path, selected_rss_param, check_prepare_phase, + debug_data); }); }); @@ -1930,13 +1933,14 @@ bool NormalLaneChange::has_collision_with_decel_patterns( } bool NormalLaneChange::is_collided( - const PathWithLaneId & lane_change_path, const ExtendedPredictedObject & obj, + const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj, const std::vector & ego_predicted_path, - const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const + const RSSparams & selected_rss_param, const bool check_prepare_phase, + CollisionCheckDebugMap & debug_data) const { constexpr auto is_collided{true}; - if (lane_change_path.points.empty()) { + if (lane_change_path.path.points.empty()) { return !is_collided; } @@ -1961,9 +1965,23 @@ bool NormalLaneChange::is_collided( const auto safety_check_max_vel = get_max_velocity_for_safety_check(); const auto & bpp_param = *common_data_ptr_->bpp_param_ptr; + const double velocity_threshold = lane_change_parameters_->stopped_object_velocity_threshold; + const double prepare_duration = lane_change_path.info.duration.prepare; + const double start_time = check_prepare_phase ? 0.0 : prepare_duration; + for (const auto & obj_path : obj_predicted_paths) { + utils::path_safety_checker::PredictedPathWithPolygon predicted_obj_path; + predicted_obj_path.confidence = obj_path.confidence; + std::copy_if( + obj_path.path.begin(), obj_path.path.end(), std::back_inserter(predicted_obj_path.path), + [&](const auto & entry) { + return !( + entry.time < start_time || + (entry.time < prepare_duration && entry.velocity < velocity_threshold)); + }); + const auto collided_polygons = utils::path_safety_checker::get_collided_polygons( - lane_change_path, ego_predicted_path, obj, obj_path, bpp_param.vehicle_info, + lane_change_path.path, ego_predicted_path, obj, predicted_obj_path, bpp_param.vehicle_info, selected_rss_param, hysteresis_factor, safety_check_max_vel, collision_check_yaw_diff_threshold, current_debug_data.second); 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 4d92b7440d1ec..69cf6441ef766 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 @@ -914,14 +914,11 @@ lanelet::BasicPolygon2d create_polygon( ExtendedPredictedObject transform( const PredictedObject & object, [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase) + const LaneChangeParameters & lane_change_parameters) { ExtendedPredictedObject extended_object(object); const auto & time_resolution = lane_change_parameters.prediction_time_resolution; - const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; - const auto & velocity_threshold = lane_change_parameters.stopped_object_velocity_threshold; - const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; const double obj_vel_norm = std::hypot(extended_object.initial_twist.linear.x, extended_object.initial_twist.linear.y); @@ -933,11 +930,8 @@ ExtendedPredictedObject transform( extended_object.predicted_paths.at(i).confidence = path.confidence; // create path - for (double t = start_time; t < end_time + std::numeric_limits::epsilon(); + for (double t = 0.0; t < end_time + std::numeric_limits::epsilon(); t += time_resolution) { - if (t < prepare_duration && obj_vel_norm < velocity_threshold) { - continue; - } const auto obj_pose = autoware::object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape); @@ -1163,8 +1157,7 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co } ExtendedPredictedObjects transform_to_extended_objects( - const CommonDataPtr & common_data_ptr, const std::vector & objects, - const bool check_prepare_phase) + const CommonDataPtr & common_data_ptr, const std::vector & objects) { ExtendedPredictedObjects extended_objects; extended_objects.reserve(objects.size()); @@ -1173,7 +1166,7 @@ ExtendedPredictedObjects transform_to_extended_objects( const auto & lc_param = *common_data_ptr->lc_param_ptr; std::transform( objects.begin(), objects.end(), std::back_inserter(extended_objects), [&](const auto & object) { - return utils::lane_change::transform(object, bpp_param, lc_param, check_prepare_phase); + return utils::lane_change::transform(object, bpp_param, lc_param); }); return extended_objects;