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 ba3f79c09a889..3e4e08007f515 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 @@ -109,7 +109,13 @@ 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 70127a390e743..cfe5c797d7130 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 @@ -1456,8 +1456,6 @@ 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_, @@ -1504,7 +1502,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() // return to lane parking if it is possible if ( - is_freespace && thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE && + thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE && canReturnToLaneParking()) { thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); }