From fd1c92bab60fadc9f16fe4b77dbf9f7f2713d82c Mon Sep 17 00:00:00 2001
From: Dong Li <lidong.main@gmail.com>
Date: Wed, 31 Jan 2018 16:13:48 -0800
Subject: [PATCH] path: added next lane overlap function

---
 modules/map/pnc_map/path.cc                       | 11 +++++++++++
 modules/map/pnc_map/path.h                        |  2 ++
 modules/planning/reference_line/reference_line.cc |  4 +---
 3 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/modules/map/pnc_map/path.cc b/modules/map/pnc_map/path.cc
index 34b85ee276..77d9f81bad 100644
--- a/modules/map/pnc_map/path.cc
+++ b/modules/map/pnc_map/path.cc
@@ -408,6 +408,17 @@ void Path::GetAllOverlaps(GetOverlapFromLaneFunc GetOverlaps_from_lane,
             });
 }
 
+const PathOverlap* Path::NextLaneOverlap(double s) const {
+  auto next = std::upper_bound(
+      lane_overlaps_.begin(), lane_overlaps_.end(), s,
+      [](double s, const PathOverlap& o) { return s < o.start_s; });
+  if (next == lane_overlaps_.end()) {
+    return nullptr;
+  } else {
+    return &(*next);
+  }
+}
+
 void Path::InitOverlaps() {
   GetAllOverlaps(std::bind(&LaneInfo::cross_lanes, _1), &lane_overlaps_);
   GetAllOverlaps(std::bind(&LaneInfo::signals, _1), &signal_overlaps_);
diff --git a/modules/map/pnc_map/path.h b/modules/map/pnc_map/path.h
index dc610ec0e1..786b0a6d11 100644
--- a/modules/map/pnc_map/path.h
+++ b/modules/map/pnc_map/path.h
@@ -264,6 +264,8 @@ class Path {
   const PathApproximation* approximation() const { return &approximation_; }
   double length() const { return length_; }
 
+  const PathOverlap* NextLaneOverlap(double s) const;
+
   const std::vector<PathOverlap>& lane_overlaps() const {
     return lane_overlaps_;
   }
diff --git a/modules/planning/reference_line/reference_line.cc b/modules/planning/reference_line/reference_line.cc
index 2a7f03d953..9530074726 100644
--- a/modules/planning/reference_line/reference_line.cc
+++ b/modules/planning/reference_line/reference_line.cc
@@ -369,9 +369,7 @@ const std::vector<ReferencePoint>& ReferenceLine::reference_points() const {
   return reference_points_;
 }
 
-const MapPath& ReferenceLine::map_path() const {
-  return map_path_;
-}
+const MapPath& ReferenceLine::map_path() const { return map_path_; }
 
 bool ReferenceLine::GetLaneWidth(const double s, double* const left_width,
                                  double* const right_width) const {
-- 
GitLab