@@ -293,9 +293,18 @@ void GoalPlannerModule::onTimer()
293293 planner->setPlannerData (local_planner_data);
294294 planner->setPreviousModuleOutput (previous_module_output);
295295 auto pull_over_path = planner->plan (goal_candidate.goal_pose );
296- if (pull_over_path) {
296+ if (pull_over_path && pull_over_path-> getParkingPath (). points . size () >= 3 ) {
297297 pull_over_path->goal_id = goal_candidate.id ;
298298 pull_over_path->id = path_candidates.size ();
299+
300+ // calculate absolute maximum curvature of parking path(start pose to end pose) for path
301+ // priority
302+ const std::vector<double > curvatures =
303+ autoware::motion_utils::calcCurvature (pull_over_path->getParkingPath ().points );
304+ pull_over_path->max_curvature = std::abs (*std::max_element (
305+ curvatures.begin (), curvatures.end (),
306+ [](const double & a, const double & b) { return std::abs (a) < std::abs (b); }));
307+
299308 path_candidates.push_back (*pull_over_path);
300309 // calculate closest pull over start pose for stop path
301310 const double start_arc_length =
@@ -817,23 +826,37 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
817826 const auto debugPrintPathPriority =
818827 [this ](
819828 const std::vector<PullOverPath> & sorted_pull_over_path_candidates,
820- const std::map<size_t , size_t > & goal_id_to_index,
821- const std::optional<std:: map<size_t , double >> & path_id_to_margin_map_opt = std::nullopt ,
822- const std::optional<std:: function<bool (const PullOverPath &)>> & isSoftMarginOpt =
823- std::nullopt ) {
829+ const std::map<size_t , size_t > & goal_id_to_index, const GoalCandidates & goal_candidates,
830+ const std::map<size_t , double > & path_id_to_margin_map ,
831+ const std::function<bool (const PullOverPath &)> & isSoftMargin,
832+ const std::function< bool ( const PullOverPath &)> & isHighCurvature ) {
824833 std::stringstream ss;
825- ss << " \n ---------------------- path priority ----------------------\n "
826- for (const auto & path : sorted_pull_over_path_candidates) {
827- // clang-format off
828- ss << " path_type: " magic_enum::enum_name (path.type )
829- << " , path_id: " id
830- << " , goal_id: " goal_id
831- << " , goal_priority:" at (path.goal_id );
832- // clang-format on
833- if (path_id_to_margin_map_opt && isSoftMarginOpt) {
834- ss << " , margin: " at (path.id )
835- << ((*isSoftMarginOpt)(path) ? " (soft)" " (hard)"
834+
835+ // unsafe goal and it's priority are not visible as debug marker in rviz,
836+ // so exclude unsafe goal from goal_priority
837+ std::map<size_t , int > goal_id_and_priority;
838+ {
839+ int priority = 0 ;
840+ for (const auto & goal_candidate : goal_candidates) {
841+ goal_id_and_priority[goal_candidate.id ] = goal_candidate.is_safe ? priority++ : -1 ;
836842 }
843+ }
844+
845+ ss << " \n ---------------------- path priority ----------------------\n "
846+ for (size_t i = 0 ; i < sorted_pull_over_path_candidates.size (); ++i) {
847+ const auto & path = sorted_pull_over_path_candidates[i];
848+
849+ // goal_index is same to goal priority including unsafe goal
850+ const int goal_index = static_cast <int >(goal_id_to_index.at (path.goal_id ));
851+ const bool is_safe_goal = goal_candidates[goal_index].is_safe ;
852+ const int goal_priority = goal_id_and_priority[path.goal_id ];
853+
854+ ss << " path_priority: " " , path_type: " magic_enum::enum_name (path.type )
855+ << " , path_id: " id << " , goal_id: " goal_id
856+ << " , goal_priority: " std::to_string (goal_priority) : " unsafe"
857+ << " , margin: " at (path.id )
858+ << (isSoftMargin (path) ? " (soft)" " (hard)" " , curvature: " max_curvature
859+ << (isHighCurvature (path) ? " (high)" " (low)"
837860 ss << " \n "
838861 }
839862 ss << " -----------------------------------------------------------\n "
@@ -844,6 +867,7 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
844867 const auto & soft_margins = parameters_->object_recognition_collision_check_soft_margins ;
845868 const auto & hard_margins = parameters_->object_recognition_collision_check_hard_margins ;
846869
870+ // (1) Sort pull_over_path_candidates based on the order in goal_candidates
847871 // Create a map of goal_id to its index in goal_candidates
848872 std::map<size_t , size_t > goal_id_to_index;
849873 for (size_t i = 0 ; i < goal_candidates.size (); ++i) {
@@ -868,6 +892,7 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
868892
869893 // if object recognition is enabled, sort by collision check margin
870894 if (parameters_->use_object_recognition ) {
895+ // (2) Sort by collision check margins
871896 const std::vector<double > margins = std::invoke ([&]() {
872897 std::vector<double > combined_margins = soft_margins;
873898 combined_margins.insert (combined_margins.end (), hard_margins.begin (), hard_margins.end ());
@@ -914,42 +939,70 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
914939 return path_id_to_margin_map[a.id ] > path_id_to_margin_map[b.id ];
915940 });
916941
917- // Sort pull_over_path_candidates based on the order in efficient_path_order
918- if (parameters_->path_priority == " efficient_path"
919- const auto isSoftMargin = [&](const PullOverPath & path) -> bool {
920- const double margin = path_id_to_margin_map[path.id ];
921- return std::any_of (
922- soft_margins.begin (), soft_margins.end (),
923- [margin](const double soft_margin) { return std::abs (margin - soft_margin) < 0.01 ; });
924- };
925- const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool {
926- return !isSoftMargin (a) && !isSoftMargin (b) &&
927- std::abs (path_id_to_margin_map[a.id ] - path_id_to_margin_map[b.id ]) < 0.01 ;
928- };
942+ // (3) Sort by curvature
943+ // If the curvature is less than the threshold, prioritize the path.
944+ const auto isHighCurvature = [&](const PullOverPath & path) -> bool {
945+ return path.max_curvature >= parameters_->high_curvature_threshold ;
946+ };
929947
948+ const auto isSoftMargin = [&](const PullOverPath & path) -> bool {
949+ const double margin = path_id_to_margin_map[path.id ];
950+ return std::any_of (
951+ soft_margins.begin (), soft_margins.end (),
952+ [margin](const double soft_margin) { return std::abs (margin - soft_margin) < 0.01 ; });
953+ };
954+ const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool {
955+ return !isSoftMargin (a) && !isSoftMargin (b) &&
956+ std::abs (path_id_to_margin_map[a.id ] - path_id_to_margin_map[b.id ]) < 0.01 ;
957+ };
958+
959+ // NOTE: this is just partition sort based on curvature threshold within each sub partitions
960+ std::stable_sort (
961+ sorted_pull_over_path_candidates.begin (), sorted_pull_over_path_candidates.end (),
962+ [&](const PullOverPath & a, const PullOverPath & b) {
963+ // if both are soft margin or both are same hard margin, prioritize the path with lower
964+ // curvature.
965+ if ((isSoftMargin (a) && isSoftMargin (b)) || isSameHardMargin (a, b)) {
966+ return !isHighCurvature (a) && isHighCurvature (b);
967+ }
968+ // otherwise, keep the order based on the margin.
969+ return false ;
970+ });
971+
972+ // (4) Sort pull_over_path_candidates based on the order in efficient_path_order keeping the
973+ // collision check margin and curvature priority.
974+ if (parameters_->path_priority == " efficient_path"
930975 std::stable_sort (
931976 sorted_pull_over_path_candidates.begin (), sorted_pull_over_path_candidates.end (),
932977 [&](const auto & a, const auto & b) {
933- // if both are soft margin or both are same hard margin, sort by planner priority
978+ // if any of following conditions are met, sort by path type priority
979+ // - both are soft margin
980+ // - both are same hard margin
934981 if ((isSoftMargin (a) && isSoftMargin (b)) || isSameHardMargin (a, b)) {
935982 return comparePathTypePriority (a, b);
936983 }
937984 // otherwise, keep the order.
938985 return false ;
939986 });
940987
941- // debug print path priority: sorted by efficient_path_order and collision check margin
988+ // debug print path priority sorted by
989+ // - efficient_path_order
990+ // - collision check margin
991+ // - curvature
942992 debugPrintPathPriority (
943- sorted_pull_over_path_candidates, goal_id_to_index, path_id_to_margin_map, isSoftMargin);
993+ sorted_pull_over_path_candidates, goal_id_to_index, goal_candidates, path_id_to_margin_map,
994+ isSoftMargin, isHighCurvature);
944995 }
945996 } else {
946- // Sort pull_over_path_candidates based on the order in efficient_path_order
997+ /* *
998+ * NOTE: use_object_recognition=false is not recommended. This option will be deprecated in the
999+ * future. sort by curvature is not implemented yet.
1000+ * Sort pull_over_path_candidates based on the order in efficient_path_order
1001+ */
9471002 if (parameters_->path_priority == " efficient_path"
9481003 std::stable_sort (
9491004 sorted_pull_over_path_candidates.begin (), sorted_pull_over_path_candidates.end (),
9501005 [&](const auto & a, const auto & b) { return comparePathTypePriority (a, b); });
951- // debug print path priority: sorted by efficient_path_order and collision check margin
952- debugPrintPathPriority (sorted_pull_over_path_candidates, goal_id_to_index);
9531006 }
9541007 }
9551008
0 commit comments