Merge pull request #1419 from tier4/fix/start_goal_v0.26.1

fix(start/goal_planner): improve start/goal planner

Author: shmpwk

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -994,33 +994,51 @@ calcCurvature<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>(
  * curvature calculation
  */
 template <class T>
-std::vector<std::pair<double, double>> calcCurvatureAndArcLength(const T & points)
+std::vector<std::pair<double, std::pair<double, double>>> calcCurvatureAndSegmentLength(
+  const T & points)
 {
-  // Note that arclength is for the segment, not the sum.
-  std::vector<std::pair<double, double>> curvature_arc_length_vec;
-  curvature_arc_length_vec.emplace_back(0.0, 0.0);
+  // segment length is pair of segment length between {p1, p2} and {p2, p3}
+  std::vector<std::pair<double, std::pair<double, double>>> curvature_and_segment_length_vec;
+  curvature_and_segment_length_vec.reserve(points.size());
+  curvature_and_segment_length_vec.emplace_back(0.0, std::make_pair(0.0, 0.0));
   for (size_t i = 1; i < points.size() - 1; ++i) {
     const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1));
     const auto p2 = tier4_autoware_utils::getPoint(points.at(i));
     const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1));
     const double curvature = tier4_autoware_utils::calcCurvature(p1, p2, p3);
-    const double arc_length = tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)) +
-                              tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1));
-    curvature_arc_length_vec.emplace_back(curvature, arc_length);
+
+    // The first point has only the next point, so put the distance to that point.
+    // In other words, assign the first segment length at the second point to the
+    // second_segment_length at the first point.
+    if (i == 1) {
+      curvature_and_segment_length_vec.at(0).second.second =
+        tier4_autoware_utils::calcDistance2d(p1, p2);
+    }
+
+    // The second_segment_length of the previous point and the first segment length of the current
+    // point are equal.
+    const std::pair<double, double> arc_length{
+      curvature_and_segment_length_vec.back().second.second,
+      tier4_autoware_utils::calcDistance2d(p2, p3)};
+
+    curvature_and_segment_length_vec.emplace_back(curvature, arc_length);
   }
-  curvature_arc_length_vec.emplace_back(0.0, 0.0);
 
-  return curvature_arc_length_vec;
+  // set to the last point
+  curvature_and_segment_length_vec.emplace_back(
+    0.0, std::make_pair(curvature_and_segment_length_vec.back().second.second, 0.0));
+
+  return curvature_and_segment_length_vec;
 }
 
-extern template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_auto_planning_msgs::msg::PathPoint>>(
+extern template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_auto_planning_msgs::msg::PathPoint>>(
   const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & points);
-extern template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId>>(
+extern template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId>>(
   const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points);
-extern template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>(
+extern template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>(
   const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points);
 
 /**

common/motion_utils/src/trajectory/trajectory.cpp

@@ -250,14 +250,14 @@ calcCurvature<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>(
   const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points);
 
 //
-template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_auto_planning_msgs::msg::PathPoint>>(
+template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_auto_planning_msgs::msg::PathPoint>>(
   const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & points);
-template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId>>(
+template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId>>(
   const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points);
-template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>(
+template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>(
   const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points);
 
 //

planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml

@@ -22,6 +22,7 @@
         lateral_offset_interval: 0.25
         ignore_distance_from_lane_start: 0.0
         margin_from_boundary: 0.5
+        high_curvature_threshold: 0.1
 
       # occupancy grid map
       occupancy_grid:

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp

@@ -60,6 +60,7 @@ struct GoalPlannerParameters
   double lateral_offset_interval{0.0};
   double ignore_distance_from_lane_start{0.0};
   double margin_from_boundary{0.0};
+  double high_curvature_threshold{0.0};
 
   // occupancy grid map
   bool use_occupancy_grid_for_goal_search{false};

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp

@@ -52,6 +52,7 @@ struct PullOverPath
   size_t goal_id{};
   size_t id{};
   bool decided_velocity{false};
+  double max_curvature{0.0};  // max curvature of the parking path
 
   PathWithLaneId getFullPath() const
   {
@@ -72,6 +73,7 @@ struct PullOverPath
     return path;
   }
 
+  // path from the pull over start pose to the end pose
   PathWithLaneId getParkingPath() const
   {
     const PathWithLaneId full_path = getFullPath();

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -269,9 +269,18 @@ void GoalPlannerModule::onTimer()
     planner->setPlannerData(local_planner_data);
     planner->setPreviousModuleOutput(previous_module_output);
     auto pull_over_path = planner->plan(goal_candidate.goal_pose);
-    if (pull_over_path) {
+    if (pull_over_path && pull_over_path->getParkingPath().points.size() >= 3) {
       pull_over_path->goal_id = goal_candidate.id;
       pull_over_path->id = path_candidates.size();
+
+      // calculate absolute maximum curvature of parking path(start pose to end pose) for path
+      // priority
+      const std::vector<double> curvatures =
+        autoware::motion_utils::calcCurvature(pull_over_path->getParkingPath().points);
+      pull_over_path->max_curvature = std::abs(*std::max_element(
+        curvatures.begin(), curvatures.end(),
+        [](const double & a, const double & b) { return std::abs(a) < std::abs(b); }));
+
       path_candidates.push_back(*pull_over_path);
       // calculate closest pull over start pose for stop path
       const double start_arc_length =
@@ -795,23 +804,37 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
   const auto debugPrintPathPriority =
     [this](
       const std::vector<PullOverPath> & sorted_pull_over_path_candidates,
-      const std::map<size_t, size_t> & goal_id_to_index,
-      const std::optional<std::map<size_t, double>> & path_id_to_margin_map_opt = std::nullopt,
-      const std::optional<std::function<bool(const PullOverPath &)>> & isSoftMarginOpt =
-        std::nullopt) {
+      const std::map<size_t, size_t> & goal_id_to_index, const GoalCandidates & goal_candidates,
+      const std::map<size_t, double> & path_id_to_margin_map,
+      const std::function<bool(const PullOverPath &)> & isSoftMargin,
+      const std::function<bool(const PullOverPath &)> & isHighCurvature) {
       std::stringstream ss;
-      ss << "\n---------------------- path priority ----------------------\n";
-      for (const auto & path : sorted_pull_over_path_candidates) {
-        // clang-format off
-        ss << "path_type: " << magic_enum::enum_name(path.type)
-           << ", path_id: " << path.id
-           << ", goal_id: " << path.goal_id
-           << ", goal_priority:" << goal_id_to_index.at(path.goal_id);
-        // clang-format on
-        if (path_id_to_margin_map_opt && isSoftMarginOpt) {
-          ss << ", margin: " << path_id_to_margin_map_opt->at(path.id)
-             << ((*isSoftMarginOpt)(path) ? " (soft)" : " (hard)");
+
+      // unsafe goal and it's priority are not visible as debug marker in rviz,
+      // so exclude unsafe goal from goal_priority
+      std::map<size_t, int> goal_id_and_priority;
+      {
+        int priority = 0;
+        for (const auto & goal_candidate : goal_candidates) {
+          goal_id_and_priority[goal_candidate.id] = goal_candidate.is_safe ? priority++ : -1;
         }
+      }
+
+      ss << "\n---------------------- path priority ----------------------\n";
+      for (size_t i = 0; i < sorted_pull_over_path_candidates.size(); ++i) {
+        const auto & path = sorted_pull_over_path_candidates[i];
+
+        // goal_index is same to goal priority including unsafe goal
+        const int goal_index = static_cast<int>(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_margin_map.at(path.id)
+           << (isSoftMargin(path) ? " (soft)" : " (hard)") << ", curvature: " << path.max_curvature
+           << (isHighCurvature(path) ? " (high)" : " (low)");
         ss << "\n";
       }
       ss << "-----------------------------------------------------------\n";
@@ -822,6 +845,7 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
   const auto & soft_margins = parameters_->object_recognition_collision_check_soft_margins;
   const auto & hard_margins = parameters_->object_recognition_collision_check_hard_margins;
 
+  // (1) Sort pull_over_path_candidates based on the order in goal_candidates
   // Create a map of goal_id to its index in goal_candidates
   std::map<size_t, size_t> goal_id_to_index;
   for (size_t i = 0; i < goal_candidates.size(); ++i) {
@@ -846,6 +870,7 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
 
   // if object recognition is enabled, sort by collision check margin
   if (parameters_->use_object_recognition) {
+    // (2) Sort by collision check margins
     const std::vector<double> margins = std::invoke([&]() {
       std::vector<double> combined_margins = soft_margins;
       combined_margins.insert(combined_margins.end(), hard_margins.begin(), hard_margins.end());
@@ -892,42 +917,70 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
         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") {
-      const auto isSoftMargin = [&](const PullOverPath & path) -> bool {
-        const double margin = path_id_to_margin_map[path.id];
-        return std::any_of(
-          soft_margins.begin(), soft_margins.end(),
-          [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; });
-      };
-      const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool {
-        return !isSoftMargin(a) && !isSoftMargin(b) &&
-               std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01;
-      };
+    // (3) Sort by curvature
+    // If the curvature is less than the threshold, prioritize the path.
+    const auto isHighCurvature = [&](const PullOverPath & path) -> bool {
+      return path.max_curvature >= parameters_->high_curvature_threshold;
+    };
 
+    const auto isSoftMargin = [&](const PullOverPath & path) -> bool {
+      const double margin = path_id_to_margin_map[path.id];
+      return std::any_of(
+        soft_margins.begin(), soft_margins.end(),
+        [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; });
+    };
+    const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool {
+      return !isSoftMargin(a) && !isSoftMargin(b) &&
+             std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01;
+    };
+
+    // NOTE: this is just partition sort based on curvature threshold within each sub partitions
+    std::stable_sort(
+      sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(),
+      [&](const PullOverPath & a, const PullOverPath & b) {
+        // if both are soft margin or both are same hard margin, prioritize the path with lower
+        // curvature.
+        if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) {
+          return !isHighCurvature(a) && isHighCurvature(b);
+        }
+        // otherwise, keep the order based on the margin.
+        return false;
+      });
+
+    // (4) Sort pull_over_path_candidates based on the order in efficient_path_order keeping the
+    // collision check margin and curvature priority.
+    if (parameters_->path_priority == "efficient_path") {
       std::stable_sort(
         sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(),
         [&](const auto & a, const auto & b) {
-          // if both are soft margin or both are same hard margin, sort by planner priority
+          // if any of following conditions are met, sort by path type priority
+          // - both are soft margin
+          // - both are same hard margin
           if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) {
             return comparePathTypePriority(a, b);
           }
           // otherwise, keep the order.
           return false;
         });
 
-      // debug print path priority: sorted by efficient_path_order and collision check margin
+      // debug print path priority sorted by
+      // - efficient_path_order
+      // - collision check margin
+      // - curvature
       debugPrintPathPriority(
-        sorted_pull_over_path_candidates, goal_id_to_index, path_id_to_margin_map, isSoftMargin);
+        sorted_pull_over_path_candidates, goal_id_to_index, goal_candidates, path_id_to_margin_map,
+        isSoftMargin, isHighCurvature);
     }
   } else {
-    // Sort pull_over_path_candidates based on the order in efficient_path_order
+    /**
+     *  NOTE: use_object_recognition=false is not recommended. This option will be deprecated in the
+     * future. sort by curvature is not implemented yet.
+     * Sort pull_over_path_candidates based on the order in efficient_path_order
+     */
     if (parameters_->path_priority == "efficient_path") {
       std::stable_sort(
         sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(),
         [&](const auto & a, const auto & b) { return comparePathTypePriority(a, b); });
-      // debug print path priority: sorted by efficient_path_order and collision check margin
-      debugPrintPathPriority(sorted_pull_over_path_candidates, goal_id_to_index);
     }
   }
 

planning/behavior_path_goal_planner_module/src/manager.cpp

@@ -61,6 +61,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node)
     p.ignore_distance_from_lane_start =
       node->declare_parameter<double>(ns + "ignore_distance_from_lane_start");
     p.margin_from_boundary = node->declare_parameter<double>(ns + "margin_from_boundary");
+    p.high_curvature_threshold = node->declare_parameter<double>(ns + "high_curvature_threshold");
 
     const std::string parking_policy_name =
       node->declare_parameter<std::string>(ns + "parking_policy");

planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp

@@ -313,8 +313,19 @@ double ShiftPullOver::calcBeforeShiftedArcLength(
 
   double before_arc_length{0.0};
   double after_arc_length{0.0};
-  for (const auto & [k, segment_length] :
-       motion_utils::calcCurvatureAndArcLength(reversed_path.points)) {
+
+  const auto curvature_and_segment_length =
+    motion_utils::calcCurvatureAndSegmentLength(reversed_path.points);
+
+  for (size_t i = 0; i < curvature_and_segment_length.size(); ++i) {
+    const auto & [k, segment_length_pair] = curvature_and_segment_length[i];
+
+    // If it is the last point, add the lengths of the previous and next segments.
+    // For other points, only add the length of the previous segment.
+    const double segment_length = i == curvature_and_segment_length.size() - 1
+                                    ? segment_length_pair.first
+                                    : segment_length_pair.first + segment_length_pair.second;
+
     // after shifted segment length
     const double after_segment_length =
       k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr);

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp

@@ -52,6 +52,9 @@ struct StartPlannerDebugData
   std::vector<Pose> start_pose_candidates;
   size_t selected_start_pose_candidate_index;
   double margin_for_start_pose_candidate;
+
+  // for isPreventingRearVehicleFromPassingThrough
+  std::optional<Pose> estimated_stop_pose;
 };
 
 struct StartPlannerParameters

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -229,8 +229,31 @@ class StartPlannerModule : public SceneModuleInterface
 
   bool isModuleRunning() const;
   bool isCurrentPoseOnMiddleOfTheRoad() const;
+
+  /**
+   * @brief Check if the ego vehicle is preventing the rear vehicle from passing through.
+   *
+   * This function just call isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) with
+   * two poses. If rear vehicle is obstructed by ego vehicle at either of the two poses, it returns
+   * true.
+   *
+   * @return true if the ego vehicle is preventing the rear vehicle from passing through with the
+   * current pose or the pose if it stops.
+   */
   bool isPreventingRearVehicleFromPassingThrough() const;
 
+  /**
+    * @brief Check if the ego vehicle is preventing the rear vehicle from passing through.
+    *
+    * This function measures the distance to the lane boundary from the current pose and the pose if
+it stops, and determines whether there is enough space for the rear vehicle to pass through. If
+    * it is obstructing at either of the two poses, it returns true.
+    *
+    * @return true if the ego vehicle is preventing the rear vehicle from passing through with given
+ego pose.
+    */
+  bool isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) const;
+
   bool isCloseToOriginalStartPose() const;
   bool hasArrivedAtGoal() const;
   bool isMoving() const;

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

@@ -407,7 +407,17 @@ double ShiftPullOut::calcBeforeShiftedArcLength(
   double before_arc_length{0.0};
   double after_arc_length{0.0};
 
-  for (const auto & [k, segment_length] : motion_utils::calcCurvatureAndArcLength(path.points)) {
+  const auto curvatures_and_segment_lengths =
+    motion_utils::calcCurvatureAndSegmentLength(path.points);
+  for (size_t i = 0; i < curvatures_and_segment_lengths.size(); ++i) {
+    const auto & [k, segment_length_pair] = curvatures_and_segment_lengths.at(i);
+
+    // If it is the last point, add the lengths of the previous and next segments.
+    // For other points, only add the length of the previous segment.
+    const double segment_length = i == curvatures_and_segment_lengths.size() - 1
+                                    ? segment_length_pair.first + segment_length_pair.second
+                                    : segment_length_pair.first;
+
     // after shifted segment length
     const double after_segment_length =
       k < 0 ? segment_length * (1 - k * dr) : segment_length / (1 + k * dr);