planning: extend reference line to dotted white lane boundary

modules/map/hdmap/hdmap_common.h

@@ -26,6 +26,7 @@ limitations under the License.
 #include "modules/common/math/math_utils.h"
 #include "modules/common/math/polygon2d.h"
 #include "modules/common/math/vec2d.h"
+#include "modules/map/proto/map_clear_area.pb.h"
 #include "modules/map/proto/map_crosswalk.pb.h"
 #include "modules/map/proto/map_id.pb.h"
 #include "modules/map/proto/map_junction.pb.h"
@@ -33,10 +34,9 @@ limitations under the License.
 #include "modules/map/proto/map_overlap.pb.h"
 #include "modules/map/proto/map_road.pb.h"
 #include "modules/map/proto/map_signal.pb.h"
+#include "modules/map/proto/map_speed_bump.pb.h"
 #include "modules/map/proto/map_stop_sign.pb.h"
 #include "modules/map/proto/map_yield_sign.pb.h"
-#include "modules/map/proto/map_clear_area.pb.h"
-#include "modules/map/proto/map_speed_bump.pb.h"
 
 /**
  * @namespace apollo::hdmap
@@ -162,8 +162,8 @@ class LaneInfo {
   void Init();
   void PostProcess(const HDMapImpl &map_instance);
   void UpdateOverlaps(const HDMapImpl &map_instance);
-  double GetWidthFromSample(
-      const std::vector<LaneInfo::SampledWidth> &samples, const double s) const;
+  double GetWidthFromSample(const std::vector<LaneInfo::SampledWidth> &samples,
+                            const double s) const;
   void CreateKDTree();
   void set_road_id(const Id &road_id) { road_id_ = road_id; }
   void set_section_id(const Id &section_id) { section_id_ = section_id; }
@@ -332,6 +332,7 @@ class SpeedBumpInfo {
   const std::vector<apollo::common::math::LineSegment2d> &segments() const {
     return segments_;
   }
+
  private:
   void Init();
 

modules/map/pnc_map/BUILD

@@ -13,6 +13,7 @@ cc_library(
     deps = [
         "//modules/common/math",
         "//modules/map/hdmap",
+        "//modules/map/hdmap:hdmap_util",
         "//modules/map/proto:map_proto",
         "//modules/routing/proto:routing_proto",
     ],

modules/map/pnc_map/path.cc

@@ -64,6 +64,95 @@ std::string LaneWaypoint::DebugString() const {
   return common::util::StrCat("id = ", lane->id().id(), "  s = ", s);
 }
 
+LaneBoundaryType::Type LeftBoundaryType(const LaneWaypoint& waypoint) {
+  if (!waypoint.lane) {
+    return LaneBoundaryType::UNKNOWN;
+  }
+  for (const auto& type :
+       waypoint.lane->lane().left_boundary().boundary_type()) {
+    if (type.s() <= waypoint.s) {
+      if (type.types_size() > 0) {
+        return type.types(0);
+      } else {
+        return LaneBoundaryType::UNKNOWN;
+      }
+    }
+  }
+  return LaneBoundaryType::UNKNOWN;
+}
+
+LaneBoundaryType::Type RightBoundaryType(const LaneWaypoint& waypoint) {
+  if (!waypoint.lane) {
+    return LaneBoundaryType::UNKNOWN;
+  }
+  for (const auto& type :
+       waypoint.lane->lane().right_boundary().boundary_type()) {
+    if (type.s() <= waypoint.s) {
+      if (type.types_size() > 0) {
+        return type.types(0);
+      } else {
+        return LaneBoundaryType::UNKNOWN;
+      }
+    }
+  }
+  return LaneBoundaryType::UNKNOWN;
+}
+
+LaneWaypoint LeftNeighborWaypoint(const LaneWaypoint& waypoint) {
+  LaneWaypoint neighbor;
+  if (!waypoint.lane) {
+    return neighbor;
+  }
+  auto point = waypoint.lane->GetSmoothPoint(waypoint.s);
+  auto map_ptr = HDMapUtil::BaseMapPtr();
+  for (const auto& lane_id :
+       waypoint.lane->lane().left_neighbor_forward_lane_id()) {
+    auto lane = map_ptr->GetLaneById(lane_id);
+    if (!lane) {
+      return neighbor;
+    }
+    double s = 0.0;
+    double l = 0.0;
+    if (!lane->GetProjection({point.x(), point.y()}, &s, &l)) {
+      continue;
+    }
+
+    if (s < -kSampleDistance || s > lane->total_length() + kSampleDistance) {
+      continue;
+    } else {
+      return LaneWaypoint(lane, s);
+    }
+  }
+  return neighbor;
+}
+
+LaneWaypoint RightNeighborWaypoint(const LaneWaypoint& waypoint) {
+  LaneWaypoint neighbor;
+  if (!waypoint.lane) {
+    return neighbor;
+  }
+  auto point = waypoint.lane->GetSmoothPoint(waypoint.s);
+  auto map_ptr = HDMapUtil::BaseMapPtr();
+  for (const auto& lane_id :
+       waypoint.lane->lane().right_neighbor_forward_lane_id()) {
+    auto lane = map_ptr->GetLaneById(lane_id);
+    if (!lane) {
+      return neighbor;
+    }
+    double s = 0.0;
+    double l = 0.0;
+    if (!lane->GetProjection({point.x(), point.y()}, &s, &l)) {
+      continue;
+    }
+    if (s < -kSampleDistance || s > lane->total_length() + kSampleDistance) {
+      continue;
+    } else {
+      return LaneWaypoint(lane, s);
+    }
+  }
+  return neighbor;
+}
+
 std::string LaneSegment::DebugString() const {
   if (lane == nullptr) {
     return "(lane is null)";

modules/map/pnc_map/path.h

@@ -33,6 +33,7 @@
 #include "modules/common/math/vec2d.h"
 #include "modules/map/hdmap/hdmap.h"
 #include "modules/map/hdmap/hdmap_common.h"
+#include "modules/map/hdmap/hdmap_util.h"
 
 namespace apollo {
 namespace hdmap {
@@ -53,6 +54,28 @@ struct LaneWaypoint {
   std::string DebugString() const;
 };
 
+/**
+ * @brief get left boundary type at a waypoint.
+ */
+LaneBoundaryType::Type LeftBoundaryType(const LaneWaypoint& waypoint);
+
+/**
+ * @brief get left boundary type at a waypoint.
+ */
+LaneBoundaryType::Type RightBoundaryType(const LaneWaypoint& waypoint);
+
+/**
+ * @brief get left neighbor lane waypoint. If not exist, the Waypoint.lane will
+ * be null.
+ */
+LaneWaypoint LeftNeighborWaypoint(const LaneWaypoint& waypoint);
+
+/**
+ * @brief get left neighbor lane waypoint. If not exist, the Waypoint.lane will
+ * be null.
+ */
+LaneWaypoint RightNeighborWaypoint(const LaneWaypoint& waypoint);
+
 struct LaneSegment {
   LaneSegment() = default;
   LaneSegment(LaneInfoConstPtr lane, const double start_s, const double end_s)

modules/planning/common/planning_gflags.cc

@@ -61,6 +61,9 @@ DEFINE_double(reference_line_stitch_overlap_distance, 20,
 DEFINE_double(reference_line_lateral_buffer, 0.5,
               "When creating reference line, the minimum distance with road "
               "curb for a vehicle driving on this line.");
+DEFINE_double(reference_line_lateral_extension, 0.5,
+              "When creating reference line, the minimum distance with road "
+              "curb for a vehicle driving on this line.");
 DEFINE_double(prepare_rerouting_time, 2.0,
               "If there are this amount of seconds left to finish driving on "
               "current route, and there is no routing, do rerouting");

modules/planning/common/planning_gflags.h

@@ -35,6 +35,7 @@ DECLARE_bool(enable_reference_line_stitching);
 DECLARE_double(look_forward_extend_distance);
 DECLARE_double(reference_line_stitch_overlap_distance);
 DECLARE_double(reference_line_lateral_buffer);
+DECLARE_double(reference_line_lateral_extension);
 DECLARE_double(prepare_rerouting_time);
 DECLARE_double(rerouting_cooldown_time);
 

modules/planning/reference_line/reference_line_provider.cc

@@ -499,12 +499,71 @@ bool ReferenceLineProvider::IsReferenceLineSmoothValid(
   return true;
 }
 
+AnchorPoint ReferenceLineProvider::GetAnchorPoint(
+    const ReferenceLine &reference_line, double s) const {
+  AnchorPoint anchor;
+  anchor.longitudinal_bound = smoother_config_.longitudinal_boundary_bound();
+  const auto half_width =
+      VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;
+  auto ref_point = reference_line.GetReferencePoint(s);
+  if (FLAGS_reference_line_lateral_extension > 1e-6 &&
+      ref_point.lane_waypoints().empty()) {
+    double left_width = 0.0;
+    double right_width = 0.0;
+    reference_line.GetLaneWidth(s, &left_width, &right_width);
+    auto shift = (left_width - right_width) / 2.0 *
+                 Vec2d::CreateUnitVec2d(ref_point.heading() + M_PI / 2.0);
+    ref_point += shift;
+    anchor.path_point = ref_point.ToPathPoint(s);
+    double effective_width = (left_width + right_width) / 2.0;
+    anchor.lateral_bound = std::max(
+        smoother_config_.lateral_boundary_bound(),
+        effective_width - half_width - FLAGS_reference_line_lateral_buffer);
+  } else {
+    const auto &waypoint = ref_point.lane_waypoints().front();
+    auto left_boundary_type = LeftBoundaryType(waypoint);
+    double left_extend = 0.0;
+    if (left_boundary_type == hdmap::LaneBoundaryType::DOTTED_WHITE) {
+      auto neighbor = LeftNeighborWaypoint(waypoint);
+      if (neighbor.lane &&
+          (neighbor.lane->lane().type() == hdmap::Lane::BIKING ||
+           neighbor.lane->lane().type() == hdmap::Lane::CITY_DRIVING ||
+           neighbor.lane->lane().type() == hdmap::Lane::PARKING)) {
+        left_extend = FLAGS_reference_line_lateral_extension;
+      }
+    }
+    auto right_boundary_type = RightBoundaryType(waypoint);
+    double right_extend = 0.0;
+    if (right_boundary_type == hdmap::LaneBoundaryType::DOTTED_WHITE) {
+      auto neighbor = RightNeighborWaypoint(waypoint);
+      if (neighbor.lane &&
+          (neighbor.lane->lane().type() == hdmap::Lane::BIKING ||
+           neighbor.lane->lane().type() == hdmap::Lane::CITY_DRIVING ||
+           neighbor.lane->lane().type() == hdmap::Lane::PARKING)) {
+        right_extend = FLAGS_reference_line_lateral_extension;
+      }
+    }
+    double left_width = 0.0;
+    double right_width = 0.0;
+    reference_line.GetLaneWidth(s, &left_width, &right_width);
+    left_width += left_extend;
+    right_width += right_extend;
+    auto shift = (left_width - right_width) / 2.0 *
+                 Vec2d::CreateUnitVec2d(ref_point.heading() + M_PI / 2.0);
+    ref_point += shift;
+    anchor.path_point = ref_point.ToPathPoint(s);
+    double effective_width = (left_width + right_width) / 2.0;
+    anchor.lateral_bound = std::max(
+        smoother_config_.lateral_boundary_bound(),
+        effective_width - half_width - FLAGS_reference_line_lateral_buffer);
+  }
+  return anchor;
+}
+
 void ReferenceLineProvider::GetAnchorPoints(
     const ReferenceLine &reference_line,
     std::vector<AnchorPoint> *anchor_points) const {
   CHECK_NOTNULL(anchor_points);
-  const auto half_width =
-      VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;
   const double interval = smoother_config_.max_constraint_interval();
   int num_of_anchors =
       std::max(2, static_cast<int>(reference_line.Length() / interval + 0.5));
@@ -512,19 +571,7 @@ void ReferenceLineProvider::GetAnchorPoints(
   common::util::uniform_slice(0.0, reference_line.Length(), num_of_anchors - 1,
                               &anchor_s);
   for (const double s : anchor_s) {
-    anchor_points->emplace_back();
-    auto &last_anchor = anchor_points->back();
-    auto ref_point = reference_line.GetReferencePoint(s);
-    last_anchor.path_point = ref_point.ToPathPoint(s);
-    last_anchor.longitudinal_bound =
-        smoother_config_.longitudinal_boundary_bound();
-    double left_width = 0.0;
-    double right_width = 0.0;
-    reference_line.GetLaneWidth(s, &left_width, &right_width);
-    last_anchor.lateral_bound =
-        std::max(smoother_config_.lateral_boundary_bound(),
-                 std::min(left_width, right_width) - half_width -
-                     FLAGS_reference_line_lateral_buffer);
+    anchor_points->emplace_back(GetAnchorPoint(reference_line, s));
   }
   anchor_points->front().longitudinal_bound = 1e-6;
   anchor_points->front().lateral_bound = 1e-6;

modules/planning/reference_line/reference_line_provider.h

@@ -133,6 +133,9 @@ class ReferenceLineProvider {
                            hdmap::RouteSegments* segments,
                            ReferenceLine* reference_line);
 
+  AnchorPoint GetAnchorPoint(const ReferenceLine& reference_line,
+                             double s) const;
+
  private:
   DECLARE_SINGLETON(ReferenceLineProvider);