consider all overlap lanelets as a goal lanelet

Signed-off-by: beyza <bnk@leodrive.ai>

Author: beyza

planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

@@ -314,7 +314,7 @@ bool DefaultPlanner::is_goal_valid(
   const auto logger = node_->get_logger();
 
   const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position);
-
+  bool is_goal_valid = false;
   // check if goal is in shoulder lanelet
   lanelet::Lanelet closest_shoulder_lanelet;
   if (lanelet::utils::query::getClosestLanelet(
@@ -331,9 +331,14 @@ bool DefaultPlanner::is_goal_valid(
     }
   }
 
-  lanelet::Lanelet closest_lanelet;
-  if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) {
-    return false;
+  lanelet::ConstLanelet goal_lanelet;
+  lanelet::ConstLanelets goal_lanelets;
+  if (!lanelet::utils::query::getCurrentLanelets(
+        road_lanelets_, goal, &goal_lanelets)) {
+    if (!lanelet::utils::query::getClosestLanelet(
+          road_lanelets_, goal, &goal_lanelet)) {
+    }
+    goal_lanelets = {goal_lanelet};
   }
 
   const auto local_vehicle_footprint = vehicle_info_.createFootprint();
@@ -348,25 +353,24 @@ bool DefaultPlanner::is_goal_valid(
     combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_);
 
   // check if goal footprint exceeds lane when the goal isn't in parking_lot
-  if (
-    param_.check_footprint_inside_lanes &&
-    !check_goal_footprint_inside_lanes(
-      closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
-    !is_in_parking_lot(
-      lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_),
-      lanelet::utils::conversion::toLaneletPoint(goal.position))) {
-    RCLCPP_WARN(logger, "Goal's footprint exceeds lane!");
-    return false;
-  }
-
-  if (is_in_lane(closest_lanelet, goal_lanelet_pt)) {
-    const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal.position);
-    const auto goal_yaw = tf2::getYaw(goal.orientation);
-    const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw);
+  for (const auto & gl_llt : goal_lanelets) {
+    if (
+      param_.check_footprint_inside_lanes &&
+      !check_goal_footprint_inside_lanes(gl_llt, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
+      !is_in_parking_lot(
+        lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_),
+        lanelet::utils::conversion::toLaneletPoint(goal.position))) {
+      RCLCPP_WARN(logger, "Goal's footprint exceeds lane!");
+      is_goal_valid = false;
+    } else if (is_in_lane(gl_llt, goal_lanelet_pt)) {
+      const auto lane_yaw = lanelet::utils::getLaneletAngle(gl_llt, goal.position);
+      const auto goal_yaw = tf2::getYaw(goal.orientation);
+      const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw);
 
-    const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg);
-    if (std::abs(angle_diff) < th_angle) {
-      return true;
+      const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg);
+      if (std::abs(angle_diff) < th_angle) {
+        return true;
+      }
     }
   }
 
@@ -382,7 +386,7 @@ bool DefaultPlanner::is_goal_valid(
     return true;
   }
 
-  return false;
+  return is_goal_valid;
 }
 
 PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)

planning/route_handler/src/route_handler.cpp

@@ -2141,48 +2141,65 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
 
   // Find lanelets for goal point.
   lanelet::ConstLanelet goal_lanelet;
-  if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal_checkpoint, &goal_lanelet)) {
-    RCLCPP_WARN_STREAM(
-      logger_, "Failed to find closest lanelet."
-                 << std::endl
-                 << " - start checkpoint: " << toString(start_checkpoint) << std::endl
-                 << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl);
-    return false;
+  lanelet::ConstLanelets goal_lanelets;
+  if (!lanelet::utils::query::getCurrentLanelets(
+        road_lanelets_, goal_checkpoint, &goal_lanelets)) {
+    if (!lanelet::utils::query::getClosestLanelet(
+          road_lanelets_, goal_checkpoint, &goal_lanelet)) {
+      RCLCPP_WARN_STREAM(
+        logger_, "Failed to find current lanelet."
+                   << std::endl
+                   << " - start checkpoint: " << toString(start_checkpoint) << std::endl
+                   << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl);
+      return false;
+    }
+    goal_lanelets = {goal_lanelet};
   }
 
   lanelet::Optional<lanelet::routing::Route> optional_route;
   std::vector<lanelet::ConstLanelets> candidate_paths;
   lanelet::routing::LaneletPath shortest_path;
   bool is_route_found = false;
 
+  lanelet::routing::LaneletPath drivable_lane_path;
+  double shortest_path_length2d = std::numeric_limits<double>::max();
   double smallest_angle_diff = std::numeric_limits<double>::max();
   constexpr double yaw_threshold = M_PI / 2.0;
 
   for (const auto & st_llt : start_lanelets) {
-    // check if the angle difference between start_checkpoint and start lanelet center line
-    // orientation is in yaw_threshold range
-    double lanelet_angle = lanelet::utils::getLaneletAngle(st_llt, start_checkpoint.position);
-    double pose_yaw = tf2::getYaw(start_checkpoint.orientation);
-    double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw));
-
-    bool is_proper_angle = angle_diff <= std::abs(yaw_threshold);
-
-    optional_route = routing_graph_ptr_->getRoute(st_llt, goal_lanelet, 0);
-    if (!optional_route || !is_proper_angle) {
-      RCLCPP_ERROR_STREAM(
-        logger_, "Failed to find a proper route!"
-                   << std::endl
-                   << " - start checkpoint: " << toString(start_checkpoint) << std::endl
-                   << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl
-                   << " - start lane id: " << st_llt.id() << std::endl
-                   << " - goal lane id: " << goal_lanelet.id() << std::endl);
-      continue;
-    }
-    is_route_found = true;
-    if (angle_diff < smallest_angle_diff) {
-      smallest_angle_diff = angle_diff;
-      shortest_path = optional_route->shortestPath();
-      start_lanelet = st_llt;
+    for (const auto & gl_llt : goal_lanelets) {
+      // check if the angle difference between start_checkpoint and start lanelet center line
+      // orientation is in yaw_threshold range
+
+      double lanelet_angle = lanelet::utils::getLaneletAngle(st_llt, start_checkpoint.position);
+      double pose_yaw = tf2::getYaw(start_checkpoint.orientation);
+      double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw));
+
+      bool is_proper_angle = angle_diff <= std::abs(yaw_threshold);
+
+      if (angle_diff < smallest_angle_diff) {
+        smallest_angle_diff = angle_diff;
+        is_proper_angle = true;
+      }
+
+      optional_route = routing_graph_ptr_->getRoute(st_llt, gl_llt, 0);
+      if (!optional_route || !is_proper_angle) {
+        RCLCPP_ERROR_STREAM(
+          logger_, "Failed to find a proper route!"
+                     << std::endl
+                     << " - start checkpoint: " << toString(start_checkpoint) << std::endl
+                     << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl
+                     << " - start lane id: " << st_llt.id() << std::endl
+                     << " - goal lane id: " << gl_llt.id() << std::endl);
+      } else {
+        is_route_found = true;
+        if (optional_route->length2d() < shortest_path_length2d) {
+          shortest_path_length2d = optional_route->length2d();
+          shortest_path = optional_route->shortestPath();
+          start_lanelet = st_llt;
+          goal_lanelet = gl_llt;
+        }
+      }
     }
   }