From ad36fc13b89455904a997ef0fe93767b2169d9aa Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 30 Sep 2024 10:01:48 +0900 Subject: [PATCH] fix(goal_planner): fix freespace planning chattering (#8981) Signed-off-by: kosuke55 --- .../util.hpp | 8 +-- .../src/goal_planner_module.cpp | 3 + .../src/util.cpp | 55 ------------------- 3 files changed, 4 insertions(+), 62 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 0f9eda8efe25c..c4491753cb0a3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -141,13 +141,7 @@ MarkerArray createLaneletPolygonMarkerArray( MarkerArray createNumObjectsToAvoidTextsMarkerArray( const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color); -std::string makePathPriorityDebugMessage( - const std::vector & sorted_path_indices, - const std::vector & pull_over_path_candidates, - const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, - const std::map & path_id_to_rough_margin_map, - const std::function & isSoftMargin, - const std::function & isHighCurvature); + /** * @brief combine two points * @param points lane points 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 b321abe1490a1..d8ff9261392b1 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 @@ -1492,6 +1492,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() return getPreviousModuleOutput(); } + const bool is_freespace = + thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE; if ( hasNotDecidedPath( planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, @@ -1540,6 +1542,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() // return to lane parking if it is possible if ( + is_freespace && thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE && canReturnToLaneParking()) { thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 96cb7d9cdf436..6fc66d961fb59 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -576,61 +576,6 @@ std::vector createPathFootPrints( return footprints; } -std::string makePathPriorityDebugMessage( - const std::vector & sorted_path_indices, - const std::vector & pull_over_path_candidates, - const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, - const std::map & path_id_to_rough_margin_map, - const std::function & isSoftMargin, - const std::function & isHighCurvature) -{ - std::stringstream ss; - - // Unsafe goal and its priority are not visible as debug marker in rviz, - // so exclude unsafe goal from goal_priority - std::map goal_id_and_priority; - for (size_t i = 0; i < goal_candidates.size(); ++i) { - goal_id_and_priority[goal_candidates[i].id] = goal_candidates[i].is_safe ? i : -1; - } - - ss << "\n---------------------- path priority ----------------------\n"; - for (size_t i = 0; i < sorted_path_indices.size(); ++i) { - const auto & path = pull_over_path_candidates[sorted_path_indices[i]]; - // goal_index is same to goal priority including unsafe goal - const int goal_index = static_cast(goal_id_to_index.at(path.goal_id)); - const bool is_safe_goal = goal_candidates[goal_index].is_safe; - const int goal_priority = goal_id_and_priority[path.goal_id]; - - ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type) - << ", path_id: " << path.id << ", goal_id: " << path.goal_id - << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") - << ", margin: " << path_id_to_rough_margin_map.at(path.id) - << (isSoftMargin(path) ? " (soft)" : " (hard)") - << ", curvature: " << path.getParkingPathMaxCurvature() - << (isHighCurvature(path) ? " (high)" : " (low)") << "\n"; - } - ss << "-----------------------------------------------------------\n"; - return ss.str(); -} - -lanelet::Points3d combineLanePoints( - const lanelet::Points3d & points, const lanelet::Points3d & points_next) -{ - lanelet::Points3d combined_points; - std::unordered_set point_ids; - for (const auto & point : points) { - if (point_ids.insert(point.id()).second) { - combined_points.push_back(point); - } - } - for (const auto & point : points_next) { - if (point_ids.insert(point.id()).second) { - combined_points.push_back(point); - } - } - return combined_points; -} - lanelet::Lanelet createDepartureCheckLanelet( const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler, const bool left_side_parking)