From e73d68329b6c940c7c7b3873dfb25e52f7e73ac8 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 13 Nov 2023 22:58:59 +0900 Subject: [PATCH] feat(goal_planner): keep margin against objects as possible Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.hpp | 7 +- .../goal_planner/pull_over_planner_base.hpp | 1 + .../goal_planner/goal_planner_module.cpp | 147 +++++++++++++++--- 3 files changed, 130 insertions(+), 25 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 a0433da92be48..75e544b79a531 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 @@ -347,7 +347,10 @@ class GoalPlannerModule : public SceneModuleInterface // collision check void initializeOccupancyGridMap(); void updateOccupancyGrid(); - bool checkCollision(const PathWithLaneId & path) const; + bool checkOccupancyGridCollision(const PathWithLaneId & path) const; + bool checkObjectsCollision( + const PathWithLaneId & path, const double collision_check_margin, + const bool update_debug_data = false) const; // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; @@ -396,7 +399,7 @@ class GoalPlannerModule : public SceneModuleInterface BehaviorModuleOutput planPullOverAsCandidate(); std::optional> selectPullOverPath( const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates) const; + const GoalCandidates & goal_candidates, const double collision_check_margin) const; std::vector sortPullOverPathCandidatesByGoalPriority( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index acb034a29d9e5..5023401263b8d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -53,6 +53,7 @@ struct PullOverPath Pose end_pose{}; std::vector debug_poses{}; size_t goal_id{}; + size_t id{}; bool decided_velocity{false}; PathWithLaneId getFullPath() const 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 5f1f36bd3a684..dd0b6c6cf7c53 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 @@ -174,6 +174,7 @@ void GoalPlannerModule::onTimer() auto pull_over_path = planner->plan(goal_candidate.goal_pose); if (pull_over_path && isCrossingPossible(*pull_over_path)) { pull_over_path->goal_id = goal_candidate.id; + pull_over_path->id = path_candidates.size(); path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = @@ -533,7 +534,15 @@ bool GoalPlannerModule::canReturnToLaneParking() } const PathWithLaneId path = thread_safe_data_.get_lane_parking_pull_over_path()->getFullPath(); - if (checkCollision(path)) { + + if ( + parameters_->use_object_recognition && + checkObjectsCollision(path, parameters_->object_recognition_collision_check_margin)) { + return false; + } + + if ( + parameters_->use_occupancy_grid_for_path_collision_check && checkOccupancyGridCollision(path)) { return false; } @@ -597,6 +606,48 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; }); + if (parameters_->use_object_recognition) { + // Create a map of PullOverPath pointer to largeest collision check margin + auto calcLargestMargin = [&](const PullOverPath & pull_over_path) { + // check has safe goal + const auto goal_candidate_it = std::find_if( + goal_candidates.begin(), goal_candidates.end(), + [pull_over_path](const auto & goal_candidate) { + return goal_candidate.id == pull_over_path.goal_id; + }); + if (goal_candidate_it != goal_candidates.end() && !goal_candidate_it->is_safe) { + return 0.0; + } + // calc largest margin + std::vector scale_factors{3.0, 2.0, 1.0}; + const double margin = parameters_->object_recognition_collision_check_margin; + const auto resampled_path = + utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); + for (const auto & scale_factor : scale_factors) { + if (!checkObjectsCollision(resampled_path, margin * scale_factor)) { + return margin * scale_factor; + } + } + return 0.0; + }; + + // Create a map of PullOverPath pointer to largest collision check margin + std::map path_id_to_margin_map; + for (const auto & path : sorted_pull_over_path_candidates) { + path_id_to_margin_map[path.id] = calcLargestMargin(path); + } + + // sorts in descending order so the item with larger margin comes first + std::stable_sort( + sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), + [&path_id_to_margin_map](const PullOverPath & a, const PullOverPath & b) { + if (std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01) { + return false; + } + return path_id_to_margin_map[a.id] > path_id_to_margin_map[b.id]; + }); + } + // Sort pull_over_path_candidates based on the order in efficient_path_order if (parameters_->path_priority == "efficient_path") { std::stable_sort( @@ -614,7 +665,7 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri std::optional> GoalPlannerModule::selectPullOverPath( const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates) const + const GoalCandidates & goal_candidates, const double collision_check_margin) const { for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe @@ -627,10 +678,20 @@ std::optional> GoalPlannerModule::selectP continue; } - // check if path is valid and safe + if (!hasEnoughDistance(pull_over_path)) { + continue; + } + + const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); if ( - !hasEnoughDistance(pull_over_path) || - checkCollision(utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5))) { + parameters_->use_object_recognition && + checkObjectsCollision(resampled_path, collision_check_margin, true /*update_debug_data*/)) { + continue; + } + + if ( + parameters_->use_occupancy_grid_for_path_collision_check && + checkOccupancyGridCollision(resampled_path)) { continue; } @@ -890,7 +951,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() thread_safe_data_.get_pull_over_path_candidates(), goal_candidates); // select pull over path which is safe against static objects and get it's goal - const auto path_and_goal_opt = selectPullOverPath(pull_over_path_candidates, goal_candidates); + const auto path_and_goal_opt = selectPullOverPath( + pull_over_path_candidates, goal_candidates, + parameters_->object_recognition_collision_check_margin); // update thread_safe_data_ if (path_and_goal_opt) { @@ -1201,7 +1264,21 @@ bool GoalPlannerModule::isStuck() return false; } - return checkCollision(thread_safe_data_.get_pull_over_path()->getCurrentPath()); + if ( + parameters_->use_object_recognition && + checkObjectsCollision( + thread_safe_data_.get_pull_over_path()->getCurrentPath(), + parameters_->object_recognition_collision_check_margin)) { + return true; + } + + if ( + parameters_->use_occupancy_grid_for_path_collision_check && + checkOccupancyGridCollision(thread_safe_data_.get_pull_over_path()->getCurrentPath())) { + return true; + } + + return false; } bool GoalPlannerModule::hasFinishedCurrentPath() @@ -1293,18 +1370,19 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const return turn_signal; } -bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const +bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) const { - if (parameters_->use_occupancy_grid_for_path_collision_check && occupancy_grid_map_) { - const bool check_out_of_range = false; - if (occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range)) { - return true; - } - } - if (!parameters_->use_object_recognition) { + if (!occupancy_grid_map_) { return false; } + const bool check_out_of_range = false; + return occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range); +} +bool GoalPlannerModule::checkObjectsCollision( + const PathWithLaneId & path, const double collision_check_margin, + const bool update_debug_data) const +{ const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length); @@ -1319,7 +1397,30 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); } - std::vector ego_polygons_expanded; + /* Expand ego collision check polygon + * - `collision_check_margin` in all directions + * - `extra_stopping_margin` in the moving direction + * - `extra_lateral_margin` in external direction of path curve + * + * + * ^ moving direction + * x + * x + * x + * +----------------------+------x--+ + * | | x | + * | +---------------+ | xx | + * | | | | xx | + * | | ego footprint |xxxxx | + * | | | | | + * | +---------------+ | extra_stopping_margin + * | margin | | + * +----------------------+ | + * | extra_lateral_margin | + * +--------------------------------+ + * + */ + std::vector ego_polygons_expanded{}; const auto curvatures = motion_utils::calcCurvature(path.points); for (size_t i = 0; i < path.points.size(); ++i) { const auto p = path.points.at(i); @@ -1337,16 +1438,16 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); const auto ego_polygon = tier4_autoware_utils::toFootprint( lateral_offset_pose, - planner_data_->parameters.base_link2front + - parameters_->object_recognition_collision_check_margin + extra_stopping_margin, - planner_data_->parameters.base_link2rear + - parameters_->object_recognition_collision_check_margin, - planner_data_->parameters.vehicle_width + - parameters_->object_recognition_collision_check_margin * 2.0 + + planner_data_->parameters.base_link2front + collision_check_margin + extra_stopping_margin, + planner_data_->parameters.base_link2rear + collision_check_margin, + planner_data_->parameters.vehicle_width + collision_check_margin * 2.0 + std::abs(extra_lateral_margin)); ego_polygons_expanded.push_back(ego_polygon); } - debug_data_.ego_polygons_expanded = ego_polygons_expanded; + + if (update_debug_data) { + debug_data_.ego_polygons_expanded = ego_polygons_expanded; + } return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); }