feat(goal_planner): use neighboring lane of pull over lane to check g…

…oal footprint (autowarefoundation#8716)

move to utils and add tests

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp

@@ -141,6 +141,29 @@ MarkerArray createLaneletPolygonMarkerArray(
 MarkerArray createNumObjectsToAvoidTextsMarkerArray(
   const GoalCandidates & goal_candidates, std::string && ns,
   const std_msgs::msg::ColorRGBA & color);
+std::string makePathPriorityDebugMessage(
+  const std::vector<size_t> & sorted_path_indices,
+  const std::vector<PullOverPath> & pull_over_path_candidates,
+  const std::map<size_t, size_t> & goal_id_to_index, const GoalCandidates & goal_candidates,
+  const std::map<size_t, double> & path_id_to_rough_margin_map,
+  const std::function<bool(const PullOverPath &)> & isSoftMargin,
+  const std::function<bool(const PullOverPath &)> & isHighCurvature);
+/**
+ * @brief combine two points
+ * @param points lane points
+ * @param points_next next lane points
+ * @return combined points
+ */
+lanelet::Points3d combineLanePoints(
+  const lanelet::Points3d & points, const lanelet::Points3d & points_next);
+/** @brief Create a lanelet that represents the departure check area.
+ * @param [in] pull_over_lanes Lanelets that the vehicle will pull over to.
+ * @param [in] route_handler RouteHandler object.
+ * @return Lanelet that goal footprints should be inside.
+ */
+lanelet::Lanelet createDepartureCheckLanelet(
+  const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler,
+  const bool left_side_parking);
 }  // namespace autoware::behavior_path_planner::goal_planner_utils
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

@@ -115,11 +115,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & p
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *route_handler, left_side_parking_, parameters_.backward_goal_search_length,
     parameters_.forward_goal_search_length);
-  auto lanes = utils::getExtendedCurrentLanes(
-    planner_data, backward_length, forward_length,
-    /*forward_only_in_route*/ false);
-  lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end());
-
+  const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet(
+    pull_over_lanes, *route_handler, left_side_parking_);
   const auto goal_arc_coords =
     lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_);
   const double s_start = std::max(0.0, goal_arc_coords.length - backward_length);
@@ -177,7 +174,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & p
         break;
       }
 
-      if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) {
+      if (!boost::geometry::within(
+            transformed_vehicle_footprint, departure_check_lane.polygon2d().basicPolygon())) {
         continue;
       }
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp

@@ -576,4 +576,96 @@ std::vector<Polygon2d> createPathFootPrints(
   return footprints;
 }
 
+std::string makePathPriorityDebugMessage(
+  const std::vector<size_t> & sorted_path_indices,
+  const std::vector<PullOverPath> & pull_over_path_candidates,
+  const std::map<size_t, size_t> & goal_id_to_index, const GoalCandidates & goal_candidates,
+  const std::map<size_t, double> & path_id_to_rough_margin_map,
+  const std::function<bool(const PullOverPath &)> & isSoftMargin,
+  const std::function<bool(const PullOverPath &)> & 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<size_t, int> 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<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_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<lanelet::Id> 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)
+{
+  const auto getBoundPoints =
+    [&](const lanelet::ConstLanelet & lane, const bool is_outer) -> lanelet::Points3d {
+    lanelet::Points3d points;
+    const auto & bound = left_side_parking ? (is_outer ? lane.leftBound() : lane.rightBound())
+                                           : (is_outer ? lane.rightBound() : lane.leftBound());
+    for (const auto & pt : bound) {
+      points.push_back(lanelet::Point3d(pt));
+    }
+    return points;
+  };
+
+  const auto getMostInnerLane = [&](const lanelet::ConstLanelet & lane) -> lanelet::ConstLanelet {
+    return left_side_parking ? route_handler.getMostRightLanelet(lane, false, true)
+                             : route_handler.getMostLeftLanelet(lane, false, true);
+  };
+
+  lanelet::Points3d outer_bound_points{};
+  lanelet::Points3d inner_bound_points{};
+  for (const auto & lane : pull_over_lanes) {
+    const auto current_outer_bound_points = getBoundPoints(lane, true);
+    const auto most_inner_lane = getMostInnerLane(lane);
+    const auto current_inner_bound_points = getBoundPoints(most_inner_lane, false);
+    outer_bound_points = combineLanePoints(outer_bound_points, current_outer_bound_points);
+    inner_bound_points = combineLanePoints(inner_bound_points, current_inner_bound_points);
+  }
+
+  const auto outer_linestring = lanelet::LineString3d(lanelet::InvalId, outer_bound_points);
+  const auto inner_linestring = lanelet::LineString3d(lanelet::InvalId, inner_bound_points);
+  return lanelet::Lanelet(
+    lanelet::InvalId, left_side_parking ? outer_linestring : inner_linestring,
+    left_side_parking ? inner_linestring : outer_linestring);
+}
+
 }  // namespace autoware::behavior_path_planner::goal_planner_utils