From 9e7dc27ad802a69477bd7b2676ef2ef22bdd12a3 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 14 Jun 2023 15:47:34 +0900 Subject: [PATCH] refactor(behavior_path_planner): generalize safety check function (#3968) * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * update Signed-off-by: kyoichi-sugahara * fix(compare_map_segmentation): initialize timer_callback_group_ (#3914) Signed-off-by: tomoya.kimura * fix(behavior_path_planner): small refactoring for resamplePathWithSpline (#3922) Signed-off-by: Zulfaqar Azmi * fix(behavior_path_planner): avoid invalid access (#3923) Signed-off-by: tomoya.kimura * temo Signed-off-by: kyoichi-sugahara * update Signed-off-by: kyoichi-sugahara * update Signed-off-by: kyoichi-sugahara * update Signed-off-by: kyoichi-sugahara * update Signed-off-by: kyoichi-sugahara * update Signed-off-by: kyoichi-sugahara * create PredictedPath debug marker Signed-off-by: kyoichi-sugahara * update Signed-off-by: kyoichi-sugahara * update Signed-off-by: kyoichi-sugahara * update Signed-off-by: kyoichi-sugahara * delete debug print Signed-off-by: kyoichi-sugahara * refactoring Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * refactoring Signed-off-by: kyoichi-sugahara * refactoring Signed-off-by: kyoichi-sugahara * update Signed-off-by: kyoichi-sugahara * update Signed-off-by: kyoichi-sugahara * add param for predicted path generation Signed-off-by: kyoichi-sugahara * refactoring Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * generalize safety check function Signed-off-by: kyoichi-sugahara * delete unnecessary difference Signed-off-by: kyoichi-sugahara * modify call refactored function Signed-off-by: kyoichi-sugahara * Update planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_planner/src/utils/safety_check.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --------- Signed-off-by: kyoichi-sugahara Signed-off-by: tomoya.kimura Signed-off-by: Zulfaqar Azmi Co-authored-by: Tomoya Kimura Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../utils/safety_check.hpp | 29 ++++++++++++----- .../src/utils/lane_change/utils.cpp | 8 ++--- .../src/utils/safety_check.cpp | 31 ++++++++++--------- 3 files changed, 43 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp index ed7faa953cf0a..858d742be867b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp @@ -78,16 +78,31 @@ double calcMinimumLongitudinalLength( /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. + * @param planned_path The predicted path of the ego vehicle. + * @param interpolated_ego A vector of pairs of ego vehicle's pose and its polygon at each moment in + * the future. + * @param ego_current_velocity Current velocity of the ego vehicle. + * @param check_duration The vector of times in the future at which safety check is + * performed.(relative time in sec from the current time) + * @param target_object The predicted object to check collision with. + * @param target_object_path The predicted path of the target object. + * @param common_parameters The common parameters used in behavior path planner. + * @param front_object_deceleration The deceleration of the object in the front.(used in RSS) + * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS) + * @param debug The debug information for collision checking. + * @param prepare_duration The duration to prepare before shifting lane. + * @param velocity_threshold_for_prepare_duration The threshold for the target velocity to + * ignore during preparation phase. * @return true if distance is safe. */ bool isSafeInLaneletCollisionCheck( - const PathWithLaneId & path, - const std::vector> & interpolated_ego, - const Twist & ego_current_twist, const std::vector & check_duration, - const double prepare_duration, const PredictedObject & target_object, - const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, - const double prepare_phase_ignore_target_speed_thresh, const double front_decel, - const double rear_decel, CollisionCheckDebug & debug); + const PathWithLaneId & planned_path, + const std::vector> & predicted_ego_poses, + const double ego_current_velocity, const std::vector & check_duration, + const PredictedObject & target_object, const PredictedPath & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, + const double rear_object_deceleration, CollisionCheckDebug & debug, + const double prepare_duration = 0.0, const double velocity_threshold_for_prepare_duration = 0.0); /** * @brief Iterate the points in the ego and target's predicted path and diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index aeaee91159fa6..99c7b9d415597 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -433,10 +433,10 @@ PathSafetyStatus isLaneChangePathSafe( utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { if (!utils::safety_check::isSafeInLaneletCollisionCheck( - path, interpolated_ego, current_twist, check_durations, - lane_change_path.duration.prepare, obj, obj_path, common_parameter, - lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel, - rear_decel, current_debug_data.second)) { + path, interpolated_ego, current_twist.linear.x, check_durations, obj, obj_path, + common_parameter, front_decel, rear_decel, current_debug_data.second, + lane_change_path.duration.prepare, + lane_change_parameter.prepare_segment_ignore_object_velocity_thresh)) { path_safety_status.is_safe = false; appendDebugInfo(current_debug_data, false); if (isObjectIndexIncluded(i, dynamic_objects_indices.target_lane)) { diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp index 8fc4878c9d8f4..72bba383a22f7 100644 --- a/planning/behavior_path_planner/src/utils/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/safety_check.cpp @@ -149,26 +149,28 @@ double calcMinimumLongitudinalLength( } bool isSafeInLaneletCollisionCheck( - const PathWithLaneId & path, - const std::vector> & interpolated_ego, - const Twist & ego_current_twist, const std::vector & check_duration, - const double prepare_duration, const PredictedObject & target_object, - const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, - const double prepare_phase_ignore_target_velocity_thresh, const double front_object_deceleration, - const double rear_object_deceleration, CollisionCheckDebug & debug) + const PathWithLaneId & planned_path, + const std::vector> & predicted_ego_poses, + const double ego_current_velocity, const std::vector & check_duration, + const PredictedObject & target_object, const PredictedPath & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, + const double rear_object_deceleration, CollisionCheckDebug & debug, const double prepare_duration, + const double velocity_threshold_for_prepare_duration) { debug.lerped_path.reserve(check_duration.size()); - const auto & ego_velocity = ego_current_twist.linear.x; + const auto & ego_velocity = ego_current_velocity; const auto & object_velocity = target_object.kinematics.initial_twist_with_covariance.twist.linear.x; for (size_t i = 0; i < check_duration.size(); ++i) { const auto current_time = check_duration.at(i); - if ( - current_time < prepare_duration && - object_velocity < prepare_phase_ignore_target_velocity_thresh) { + // ignore low velocity object during prepare duration + const bool prepare_phase = current_time < prepare_duration; + const bool ignore_target_velocity_during_prepare_phase = + object_velocity < velocity_threshold_for_prepare_duration; + if (prepare_phase && ignore_target_velocity_during_prepare_phase) { continue; } @@ -178,8 +180,8 @@ bool isSafeInLaneletCollisionCheck( } const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, target_object.shape); - const auto & ego_pose = interpolated_ego.at(i).first; - const auto & ego_polygon = interpolated_ego.at(i).second; + const auto & ego_pose = predicted_ego_poses.at(i).first; + const auto & ego_polygon = predicted_ego_poses.at(i).second; // check overlap debug.ego_polygon = ego_polygon; @@ -191,7 +193,7 @@ bool isSafeInLaneletCollisionCheck( // compute which one is at the front of the other const bool is_object_front = - isTargetObjectFront(path, ego_pose, common_parameters.vehicle_info, obj_polygon); + isTargetObjectFront(planned_path, ego_pose, common_parameters.vehicle_info, obj_polygon); const auto & [front_object_velocity, rear_object_velocity] = is_object_front ? std::make_pair(object_velocity, ego_velocity) : std::make_pair(ego_velocity, object_velocity); @@ -221,6 +223,7 @@ bool isSafeInLaneletCollisionCheck( debug.expected_obj_pose = *obj_pose; debug.is_front = is_object_front; + // check overlap with extended polygon if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { debug.failed_reason = "overlap_extended_polygon"; return false;