From 84c7193e50dd69714b8ea6ae220073f7b85e7394 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 13 Nov 2024 14:46:53 +0900 Subject: [PATCH] feat(goal_planner): safety check with only parking path (#9293) Signed-off-by: kosuke55 --- .../src/goal_planner_module.cpp | 15 ++++++++++----- .../utils/path_safety_checker/safety_check.hpp | 3 +++ .../utils/path_safety_checker/safety_check.cpp | 16 ++++++++++++++++ 3 files changed, 29 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 147fe9f79dba0..ad3e145d89442 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -2158,6 +2158,10 @@ bool GoalPlannerModule::isSafePath( const std::shared_ptr & objects_filtering_params, const std::shared_ptr & safety_check_params) const { + using autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath; + using autoware::behavior_path_planner::utils::path_safety_checker:: + filterPredictedPathAfterTargetPose; + if (!found_pull_over_path || !pull_over_path_opt) { return false; } @@ -2175,7 +2179,6 @@ bool GoalPlannerModule::isSafePath( const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters.backward_goal_search_length, parameters.forward_goal_search_length); - const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(current_pull_over_path.points); const std::pair terminal_velocity_and_accel = pull_over_path.getPairsTerminalVelocityAndAccel(); RCLCPP_DEBUG( @@ -2186,10 +2189,12 @@ bool GoalPlannerModule::isSafePath( // TODO(Sugahara): shoule judge is_object_front properly const bool is_object_front = true; const bool limit_to_max_velocity = true; - const auto ego_predicted_path = - autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, current_pull_over_path.points, current_pose, current_velocity, - ego_seg_idx, is_object_front, limit_to_max_velocity); + const auto ego_seg_idx = planner_data->findEgoIndex(current_pull_over_path.points); + const auto ego_predicted_path_from_current_pose = createPredictedPath( + ego_predicted_path_params, current_pull_over_path.points, current_pose, current_velocity, + ego_seg_idx, is_object_front, limit_to_max_velocity); + const auto ego_predicted_path = filterPredictedPathAfterTargetPose( + ego_predicted_path_from_current_pose, pull_over_path.start_pose()); // ========================================================================================== // if ego is before the entry of pull_over_lanes, the beginning of the safety check area diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index a357d51cc4afb..9d7b316b7550a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -105,6 +105,9 @@ ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon( const ExtendedPredictedObjects & objects, const double time_horizon, const bool check_all_predicted_path); +std::vector filterPredictedPathAfterTargetPose( + const std::vector & path, const Pose & target_pose); + bool checkSafetyWithRSS( const PathWithLaneId & planned_path, const std::vector & ego_predicted_path, diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 59fc726bd10a0..11f3396ad82f1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -489,6 +489,22 @@ ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon( return filtered_objects; } +std::vector filterPredictedPathAfterTargetPose( + const std::vector & path, const Pose & target_pose) +{ + std::vector filtered_path; + + const auto target_idx = + std::min_element(path.begin(), path.end(), [&target_pose](const auto & a, const auto & b) { + return calcDistance2d(a.pose.position, target_pose.position) < + calcDistance2d(b.pose.position, target_pose.position); + }); + + std::copy(target_idx, path.end(), std::back_inserter(filtered_path)); + + return filtered_path; +}; + bool checkSafetyWithRSS( const PathWithLaneId & planned_path, const std::vector & ego_predicted_path,