From 6c85af4382e264c39b96069e3b99edef69fb8bd8 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 30 Nov 2023 19:53:57 +0900 Subject: [PATCH] refactor(start_planner): add rss safety check function (#5623) * Refactor safety check method in StartPlannerModule Signed-off-by: kyoichi-sugahara * Remove unused function checkSafetyWithRSS Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../goal_planner/goal_planner_module.hpp | 6 ---- .../path_safety_checker/safety_check.hpp | 8 +++++ .../goal_planner/goal_planner_module.cpp | 32 ++--------------- .../start_planner/start_planner_module.cpp | 34 +++---------------- .../path_safety_checker/safety_check.cpp | 30 ++++++++++++++++ 5 files changed, 46 insertions(+), 64 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 25f0a815dee8c..a25b407623089 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -62,7 +62,6 @@ using freespace_planning_algorithms::RRTStar; using freespace_planning_algorithms::RRTStarParam; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; -using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; @@ -489,11 +488,6 @@ class GoalPlannerModule : public SceneModuleInterface */ std::pair isSafePath() const; - bool checkSafetyWithRSS( - const PathWithLaneId & planned_path, - const std::vector & ego_predicted_path, - const std::vector & objects, const double hysteresis_factor) const; - // debug void setDebugData(); void printParkingPositionError() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index a4851e97d5008..133375f6117d9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -99,6 +99,14 @@ ExtendedPredictedObject filterObjectPredictedPathByTimeHorizon( ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon( const ExtendedPredictedObjects & objects, const double time_horizon, const bool check_all_predicted_path); + +bool checkSafetyWithRSS( + const PathWithLaneId & planned_path, + const std::vector & ego_predicted_path, + const std::vector & objects, CollisionCheckDebugMap & debug_map, + const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params, + const bool check_all_predicted_path, const double hysteresis_factor); + /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 647487d8c799b..4ca1281abdbc6 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1742,34 +1742,6 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData( goal_planner_data_.ego_predicted_path = ego_predicted_path; } -bool GoalPlannerModule::checkSafetyWithRSS( - const PathWithLaneId & planned_path, - const std::vector & ego_predicted_path, - const std::vector & objects, const double hysteresis_factor) const -{ - // Check for collisions with each predicted path of the object - const bool is_safe = !std::any_of(objects.begin(), objects.end(), [&](const auto & object) { - auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); - - const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( - object, objects_filtering_params_->check_all_predicted_path); - - return std::any_of( - obj_predicted_paths.begin(), obj_predicted_paths.end(), [&](const auto & obj_path) { - const bool has_collision = !utils::path_safety_checker::checkCollision( - planned_path, ego_predicted_path, object, obj_path, planner_data_->parameters, - safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second); - - utils::path_safety_checker::updateCollisionCheckDebugMap( - goal_planner_data_.collision_check, current_debug_data, !has_collision); - - return has_collision; - }); - }); - - return is_safe; -} - std::pair GoalPlannerModule::isSafePath() const { const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); @@ -1821,8 +1793,10 @@ std::pair GoalPlannerModule::isSafePath() const const bool current_is_safe = std::invoke([&]() { if (parameters_->safety_check_params.method == "RSS") { - return checkSafetyWithRSS( + return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_over_path, ego_predicted_path, target_objects_on_lane.on_current_lane, + goal_planner_data_.collision_check, planner_data_->parameters, + safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path, hysteresis_factor); } else if (parameters_->safety_check_params.method == "integral_predicted_polygon") { return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index fb63a3c2cf47f..2bf7ac86bed1e 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -1091,35 +1091,11 @@ bool StartPlannerModule::isSafePath() const utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); - bool is_safe_dynamic_objects = true; - // Check for collisions with each predicted path of the object - for (const auto & object : target_objects_on_lane.on_current_lane) { - auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); - - bool is_safe_dynamic_object = true; - - const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( - object, objects_filtering_params_->check_all_predicted_path); - - // If a collision is detected, mark the object as unsafe and break the loop - for (const auto & obj_path : obj_predicted_paths) { - if (!utils::path_safety_checker::checkCollision( - pull_out_path, ego_predicted_path, object, obj_path, planner_data_->parameters, - safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) { - utils::path_safety_checker::updateCollisionCheckDebugMap( - start_planner_data_.collision_check, current_debug_data, false); - is_safe_dynamic_objects = false; - is_safe_dynamic_object = false; - break; - } - } - if (is_safe_dynamic_object) { - utils::path_safety_checker::updateCollisionCheckDebugMap( - start_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object); - } - } - - return is_safe_dynamic_objects; + return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( + pull_out_path, ego_predicted_path, target_objects_on_lane.on_current_lane, + start_planner_data_.collision_check, planner_data_->parameters, + safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path, + hysteresis_factor); } bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 473a0489a14a9..b2d9eddeaddbd 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -392,6 +392,36 @@ ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon( return filtered_objects; } +bool checkSafetyWithRSS( + const PathWithLaneId & planned_path, + const std::vector & ego_predicted_path, + const std::vector & objects, CollisionCheckDebugMap & debug_map, + const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params, + const bool check_all_predicted_path, const double hysteresis_factor) +{ + // Check for collisions with each predicted path of the object + const bool is_safe = !std::any_of(objects.begin(), objects.end(), [&](const auto & object) { + auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); + + const auto obj_predicted_paths = + utils::path_safety_checker::getPredictedPathFromObj(object, check_all_predicted_path); + + return std::any_of( + obj_predicted_paths.begin(), obj_predicted_paths.end(), [&](const auto & obj_path) { + const bool has_collision = !utils::path_safety_checker::checkCollision( + planned_path, ego_predicted_path, object, obj_path, parameters, rss_params, + hysteresis_factor, current_debug_data.second); + + utils::path_safety_checker::updateCollisionCheckDebugMap( + debug_map, current_debug_data, !has_collision); + + return has_collision; + }); + }); + + return is_safe; +} + bool checkSafetyWithIntegralPredictedPolygon( const std::vector & ego_predicted_path, const VehicleInfo & vehicle_info, const ExtendedPredictedObjects & objects, const bool check_all_predicted_path,