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 <kosuke.tnp@gmail.com>

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

@@ -161,6 +161,22 @@ std::string makePathPriorityDebugMessage(
  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

@@ -119,11 +119,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);
@@ -194,7 +191,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

@@ -687,4 +687,59 @@ std::string makePathPriorityDebugMessage(
  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

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp

@@ -82,3 +82,86 @@ TEST_F(TestUtilWithMap, isWithinAreas)
  baselink_footprint, bus_stop_area_polygons),
  true);
 }
+
+TEST_F(TestUtilWithMap, combineLanePoints)
+{
+ // 1) combine points with no duplicate IDs
+ {
+ lanelet::Points3d points{
+ {lanelet::Point3d(1, 0, 0), lanelet::Point3d(2, 0, 0), lanelet::Point3d(3, 0, 0)}};
+ lanelet::Points3d points_next{
+ {lanelet::Point3d(4, 0, 0), lanelet::Point3d(5, 0, 0), lanelet::Point3d(6, 0, 0)}};
+
+ const auto combined_points =
+ autoware::behavior_path_planner::goal_planner_utils::combineLanePoints(points, points_next);
+ EXPECT_EQ(combined_points.size(), 6);
+ }
+
+ // 2) combine points with duplicate IDs
+ {
+ lanelet::Points3d points{
+ {lanelet::Point3d(1, 0, 0), lanelet::Point3d(2, 0, 0), lanelet::Point3d(3, 0, 0)}};
+ lanelet::Points3d points_next{
+ {lanelet::Point3d(3, 0, 0), lanelet::Point3d(4, 0, 0), lanelet::Point3d(5, 0, 0)}};
+
+ const auto combined_points =
+ autoware::behavior_path_planner::goal_planner_utils::combineLanePoints(points, points_next);
+ EXPECT_EQ(combined_points.size(), 5);
+ }
+}
+
+TEST_F(TestUtilWithMap, createDepartureCheckLanelet)
+{
+ const auto lanelet_map_ptr = route_handler->getLaneletMapPtr();
+
+ const geometry_msgs::msg::Pose goal_pose =
+ geometry_msgs::build<geometry_msgs::msg::Pose>()
+ .position(geometry_msgs::build<geometry_msgs::msg::Point>()
+ .x(433.42254638671875)
+ .y(465.3381652832031)
+ .z(0.0))
+ .orientation(geometry_msgs::build<geometry_msgs::msg::Quaternion>()
+ .x(0.0)
+ .y(0.0)
+ .z(0.306785474523741)
+ .w(0.9517786888879384));
+
+ // 1) get target shoulder lane and check it's lane id
+ const auto target_shoulder_lane = route_handler->getPullOverTarget(goal_pose);
+ EXPECT_EQ(target_shoulder_lane.has_value(), true);
+ EXPECT_EQ(target_shoulder_lane.value().id(), 18391);
+
+ // 2) get shoulder lane sequence
+ const auto target_shoulder_lanes =
+ route_handler->getShoulderLaneletSequence(target_shoulder_lane.value(), goal_pose);
+ EXPECT_EQ(target_shoulder_lanes.size(), 3);
+
+ // 3) check if the right bound of the departure check lane extended to the right end matches the
+ // right bound of the rightmost lanelet
+ const auto to_points3d = [](const lanelet::ConstLineString3d & bound) {
+ lanelet::Points3d points;
+ for (const auto & pt : bound) {
+ points.push_back(lanelet::Point3d(pt));
+ }
+ return points;
+ };
+
+ const auto departure_check_lane =
+ autoware::behavior_path_planner::goal_planner_utils::createDepartureCheckLanelet(
+ target_shoulder_lanes, *route_handler, true);
+ const auto departure_check_lane_right_bound_points =
+ to_points3d(departure_check_lane.rightBound());
+
+ const std::vector<lanelet::Id> most_right_lanelet_ids = {18381, 18383, 18388};
+ lanelet::Points3d right_bound_points;
+ for (const auto & id : most_right_lanelet_ids) {
+ const auto lanelet = lanelet_map_ptr->laneletLayer.get(id);
+ right_bound_points = autoware::behavior_path_planner::goal_planner_utils::combineLanePoints(
+ right_bound_points, to_points3d(lanelet.rightBound()));
+ }
+
+ EXPECT_EQ(departure_check_lane_right_bound_points.size(), right_bound_points.size());
+ for (size_t i = 0; i < departure_check_lane_right_bound_points.size(); ++i) {
+ EXPECT_EQ(departure_check_lane_right_bound_points.at(i).id(), right_bound_points.at(i).id());
+ }
+}