@@ -642,7 +642,7 @@ bool GoalPlannerModule::canReturnToLaneParking()
642642 if (
643643 parameters_->use_object_recognition &&
644644 checkObjectsCollision (
645- path, parameters_->object_recognition_collision_check_margin ,
645+ path, parameters_->object_recognition_collision_check_hard_margins . back () ,
646646 /* extract_static_objects=*/ false )) {
647647 return false ;
648648 }
@@ -697,7 +697,37 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
697697 const std::vector<PullOverPath> & pull_over_path_candidates,
698698 const GoalCandidates & goal_candidates) const
699699{
700- auto sorted_pull_over_path_candidates = pull_over_path_candidates;
700+ // ==========================================================================================
701+ // print path priority for debug
702+ const auto debugPrintPathPriority =
703+ [this ](
704+ const std::vector<PullOverPath> & sorted_pull_over_path_candidates,
705+ const std::map<size_t , size_t > & goal_id_to_index,
706+ const std::optional<std::map<size_t , double >> & path_id_to_margin_map_opt = std::nullopt,
707+ const std::optional<std::function<bool (const PullOverPath &)>> & isSoftMarginOpt =
708+ std::nullopt) {
709+ std::stringstream ss;
710+ ss << " \n ---------------------- path priority ----------------------\n "
711+ for (const auto & path : sorted_pull_over_path_candidates) {
712+ // clang-format off
713+ ss << " path_type: " magic_enum::enum_name (path.type )
714+ << " , path_id: " id
715+ << " , goal_id: " goal_id
716+ << " , goal_priority:" at (path.goal_id );
717+ // clang-format on
718+ if (path_id_to_margin_map_opt && isSoftMarginOpt) {
719+ ss << " , margin: " at (path.id )
720+ << ((*isSoftMarginOpt)(path) ? " (soft)" " (hard)"
721+ }
722+ ss << " \n "
723+ }
724+ ss << " -----------------------------------------------------------\n "
725+ RCLCPP_DEBUG_STREAM (getLogger (), ss.str ());
726+ };
727+ // ==========================================================================================
728+
729+ const auto & soft_margins = parameters_->object_recognition_collision_check_soft_margins ;
730+ const auto & hard_margins = parameters_->object_recognition_collision_check_hard_margins ;
701731
702732 // Create a map of goal_id to its index in goal_candidates
703733 std::map<size_t , size_t > goal_id_to_index;
@@ -706,13 +736,29 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
706736 }
707737
708738 // Sort pull_over_path_candidates based on the order in goal_candidates
739+ auto sorted_pull_over_path_candidates = pull_over_path_candidates;
709740 std::stable_sort (
710741 sorted_pull_over_path_candidates.begin (), sorted_pull_over_path_candidates.end (),
711742 [&goal_id_to_index](const auto & a, const auto & b) {
712743 return goal_id_to_index[a.goal_id ] < goal_id_to_index[b.goal_id ];
713744 });
714745
746+ // compare to sort pull_over_path_candidates based on the order in efficient_path_order
747+ const auto comparePathTypePriority = [&](const PullOverPath & a, const PullOverPath & b) -> bool {
748+ const auto & order = parameters_->efficient_path_order ;
749+ const auto a_pos = std::find (order.begin (), order.end (), magic_enum::enum_name (a.type ));
750+ const auto b_pos = std::find (order.begin (), order.end (), magic_enum::enum_name (b.type ));
751+ return a_pos < b_pos;
752+ };
753+
754+ // if object recognition is enabled, sort by collision check margin
715755 if (parameters_->use_object_recognition ) {
756+ const std::vector<double > margins = std::invoke ([&]() {
757+ std::vector<double > combined_margins = soft_margins;
758+ combined_margins.insert (combined_margins.end (), hard_margins.begin (), hard_margins.end ());
759+ return combined_margins;
760+ });
761+
716762 // Create a map of PullOverPath pointer to largest collision check margin
717763 auto calcLargestMargin = [&](const PullOverPath & pull_over_path) {
718764 // check has safe goal
@@ -724,16 +770,14 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
724770 if (goal_candidate_it != goal_candidates.end () && !goal_candidate_it->is_safe ) {
725771 return 0.0 ;
726772 }
727- // calc largest margin
728- std::vector<double > scale_factors{3.0 , 2.0 , 1.0 };
729- const double margin = parameters_->object_recognition_collision_check_margin ;
773+ // check path collision margin
730774 const auto resampled_path =
731775 utils::resamplePathWithSpline (pull_over_path.getParkingPath (), 0.5 );
732- for (const auto & scale_factor : scale_factors ) {
776+ for (const auto & margin : margins ) {
733777 if (!checkObjectsCollision (
734- resampled_path, margin * scale_factor ,
778+ resampled_path, margin,
735779 /* extract_static_objects=*/ true )) {
736- return margin * scale_factor ;
780+ return margin;
737781 }
738782 }
739783 return 0.0 ;
@@ -754,18 +798,44 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
754798 }
755799 return path_id_to_margin_map[a.id ] > path_id_to_margin_map[b.id ];
756800 });
757- }
758801
759- // Sort pull_over_path_candidates based on the order in efficient_path_order
760- if (parameters_->path_priority == " efficient_path"
761- std::stable_sort (
762- sorted_pull_over_path_candidates.begin (), sorted_pull_over_path_candidates.end (),
763- [this ](const auto & a, const auto & b) {
764- const auto & order = parameters_->efficient_path_order ;
765- const auto a_pos = std::find (order.begin (), order.end (), magic_enum::enum_name (a.type ));
766- const auto b_pos = std::find (order.begin (), order.end (), magic_enum::enum_name (b.type ));
767- return a_pos < b_pos;
768- });
802+ // Sort pull_over_path_candidates based on the order in efficient_path_order
803+ if (parameters_->path_priority == " efficient_path"
804+ const auto isSoftMargin = [&](const PullOverPath & path) -> bool {
805+ const double margin = path_id_to_margin_map[path.id ];
806+ return std::any_of (
807+ soft_margins.begin (), soft_margins.end (),
808+ [margin](const double soft_margin) { return std::abs (margin - soft_margin) < 0.01 ; });
809+ };
810+ const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool {
811+ return !isSoftMargin (a) && !isSoftMargin (b) &&
812+ std::abs (path_id_to_margin_map[a.id ] - path_id_to_margin_map[b.id ]) < 0.01 ;
813+ };
814+
815+ std::stable_sort (
816+ sorted_pull_over_path_candidates.begin (), sorted_pull_over_path_candidates.end (),
817+ [&](const auto & a, const auto & b) {
818+ // if both are soft margin or both are same hard margin, sort by planner priority
819+ if ((isSoftMargin (a) && isSoftMargin (b)) || isSameHardMargin (a, b)) {
820+ return comparePathTypePriority (a, b);
821+ }
822+ // otherwise, keep the order.
823+ return false ;
824+ });
825+
826+ // debug print path priority: sorted by efficient_path_order and collision check margin
827+ debugPrintPathPriority (
828+ sorted_pull_over_path_candidates, goal_id_to_index, path_id_to_margin_map, isSoftMargin);
829+ }
830+ } else {
831+ // Sort pull_over_path_candidates based on the order in efficient_path_order
832+ if (parameters_->path_priority == " efficient_path"
833+ std::stable_sort (
834+ sorted_pull_over_path_candidates.begin (), sorted_pull_over_path_candidates.end (),
835+ [&](const auto & a, const auto & b) { return comparePathTypePriority (a, b); });
836+ // debug print path priority: sorted by efficient_path_order and collision check margin
837+ debugPrintPathPriority (sorted_pull_over_path_candidates, goal_id_to_index);
838+ }
769839 }
770840
771841 return sorted_pull_over_path_candidates;
@@ -979,7 +1049,7 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const
9791049 // check current parking path collision
9801050 const auto parking_path = utils::resamplePathWithSpline (pull_over_path->getParkingPath (), 0.5 );
9811051 const double margin =
982- parameters_->object_recognition_collision_check_margin * hysteresis_factor;
1052+ parameters_->object_recognition_collision_check_hard_margins . back () * hysteresis_factor;
9831053 if (checkObjectsCollision (parking_path, margin, /* extract_static_objects=*/ false )) {
9841054 RCLCPP_DEBUG (
9851055 getLogger (),
@@ -1115,14 +1185,17 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
11151185 // select pull over path which is safe against static objects and get it's goal
11161186 const auto path_and_goal_opt = selectPullOverPath (
11171187 pull_over_path_candidates, goal_candidates,
1118- parameters_->object_recognition_collision_check_margin );
1188+ parameters_->object_recognition_collision_check_hard_margins . back () );
11191189
11201190 // update thread_safe_data_
11211191 if (path_and_goal_opt) {
11221192 auto [pull_over_path, modified_goal] = *path_and_goal_opt;
11231193 deceleratePath (pull_over_path);
11241194 thread_safe_data_.set (
11251195 goal_candidates, pull_over_path_candidates, pull_over_path, modified_goal);
1196+ RCLCPP_DEBUG (
1197+ getLogger (), " selected pull over path: path_id: %ld, goal_id: %ld" id ,
1198+ modified_goal.id );
11261199 } else {
11271200 thread_safe_data_.set (goal_candidates, pull_over_path_candidates);
11281201 }
@@ -1436,7 +1509,8 @@ bool GoalPlannerModule::isStuck()
14361509 parameters_->use_object_recognition &&
14371510 checkObjectsCollision (
14381511 thread_safe_data_.get_pull_over_path ()->getCurrentPath (),
1439- /* extract_static_objects=*/ false , parameters_->object_recognition_collision_check_margin )) {
1512+ /* extract_static_objects=*/ false ,
1513+ parameters_->object_recognition_collision_check_hard_margins .back ())) {
14401514 return true ;
14411515 }
14421516
0 commit comments