pnc_map: add position history in pnc map

which can be used to define change lane behavior

modules/map/pnc_map/pnc_map.cc

@@ -248,6 +248,27 @@ PncMap::PncMap(const HDMap *hdmap) : hdmap_(hdmap) {}
 
 const hdmap::HDMap *PncMap::hdmap() const { return hdmap_; }
 
+bool PncMap::UpdatePosition(const common::PointENU &point) {
+  if (!GetNearestPointFromRouting(point, &current_waypoint_)) {
+    AERROR << "Failed to get waypoint from routing";
+    return false;
+  }
+  auto current_route_index = GetWaypointIndex(current_waypoint_);
+  if (current_route_index.size() != 3 || current_route_index[0] < 0) {
+    AERROR << "Failed to get routing index from waypoint";
+    return false;
+  }
+
+  // on the same passage as last time, don't update
+  if (route_index_.size() != 3 || route_index_[0] != current_route_index[0] ||
+      route_index_[1] != current_route_index[1]) {
+    passage_start_point_ = point;
+  }
+  current_point_ = point;
+  route_index_ = current_route_index;
+  return true;
+}
+
 bool PncMap::UpdateRoutingResponse(const routing::RoutingResponse &routing) {
   if (routing_.has_header() && routing.has_header() &&
       routing_.header().sequence_num() == routing.header().sequence_num() &&
@@ -268,7 +289,10 @@ bool PncMap::UpdateRoutingResponse(const routing::RoutingResponse &routing) {
       }
     }
   }
-  last_waypoint_.reset(nullptr);
+  current_waypoint_.lane = nullptr;
+  route_index_.clear();
+  current_point_.Clear();
+  passage_start_point_.Clear();
   routing_ = routing;
   return true;
 }
@@ -376,21 +400,15 @@ std::vector<int> PncMap::GetNeighborPassages(const routing::RoadSegment &road,
 }
 
 bool PncMap::GetRouteSegments(
-    const common::PointENU &point, const double backward_length,
-    const double forward_length,
+    const double backward_length, const double forward_length,
     std::vector<RouteSegments> *const route_segments) const {
-  LaneWaypoint waypoint;
-  if (!GetNearestPointFromRouting(point, &waypoint)) {
-    AERROR << "Failed to get waypoint from routing";
-    return false;
-  }
-  auto index = GetWaypointIndex(waypoint);
-  if (index.size() != 3 || index[0] < 0) {
-    AERROR << "Failed to get routing index from waypoint";
+  if (!current_waypoint_.lane || route_index_.size() != 3 ||
+      route_index_[0] < 0) {
+    AERROR << "Invalid position, use UpdatePosition() function first";
     return false;
   }
-  int road_index = index[0];
-  int passage_index = index[1];
+  const int road_index = route_index_[0];
+  const int passage_index = route_index_[1];
   const auto &road = routing_.road(road_index);
   // raw filter to find all neighboring passages
   auto drive_passages = GetNeighborPassages(road, passage_index);
@@ -401,9 +419,10 @@ bool PncMap::GetRouteSegments(
       ADEBUG << "Failed to convert passage to lane segments.";
       continue;
     }
-    auto nearest_point = point;
+    auto nearest_point = current_point_;
     if (index == passage_index) {
-      nearest_point = waypoint.lane->GetSmoothPoint(waypoint.s);
+      nearest_point =
+          current_waypoint_.lane->GetSmoothPoint(current_waypoint_.s);
     }
     double s = 0.0;
     double l = 0.0;
@@ -414,7 +433,7 @@ bool PncMap::GetRouteSegments(
       continue;
     }
     // check if possible to drive from current waypoint to the passage.
-    if (index != passage_index && !segments.CanDriveFrom(waypoint)) {
+    if (index != passage_index && !segments.CanDriveFrom(current_waypoint_)) {
       ADEBUG << "You cannot drive from current waypoint to passage: " << index;
       continue;
     }

modules/map/pnc_map/pnc_map.h

@@ -139,14 +139,14 @@ class PncMap {
   const hdmap::HDMap *hdmap() const;
 
   bool UpdateRoutingResponse(const routing::RoutingResponse &routing_response);
+  bool UpdatePosition(const common::PointENU &point);
 
   const routing::RoutingResponse &routing_response() const;
 
   static bool CreatePathFromLaneSegments(const RouteSegments &segments,
                                          Path *const path);
 
-  bool GetRouteSegments(const common::PointENU &point,
-                        const double backward_length,
+  bool GetRouteSegments(const double backward_length,
                         const double forward_length,
                         std::vector<RouteSegments> *const route_segments) const;
 
@@ -195,7 +195,10 @@ class PncMap {
  private:
   routing::RoutingResponse routing_;
   std::unordered_set<std::string> routing_lane_ids_;
-  std::unique_ptr<LaneWaypoint> last_waypoint_;
+  LaneWaypoint current_waypoint_;
+  common::PointENU current_point_;
+  std::vector<int> route_index_;
+  common::PointENU passage_start_point_;
   const hdmap::HDMap *hdmap_ = nullptr;
 };
 

modules/map/pnc_map/pnc_map_test.cc

@@ -136,7 +136,10 @@ TEST_F(PncMapTest, GetRouteSegments) {
   ASSERT_TRUE(lane);
   auto point = lane->GetSmoothPoint(0);
   std::vector<RouteSegments> segments;
-  bool result = pnc_map_->GetRouteSegments(point, 10, 30, &segments);
+  pnc_map_->route_index_.clear();
+  EXPECT_FALSE(pnc_map_->GetRouteSegments(10, 30, &segments));
+  EXPECT_TRUE(pnc_map_->UpdatePosition(point));
+  bool result = pnc_map_->GetRouteSegments(10, 30, &segments);
   ASSERT_TRUE(result);
   ASSERT_EQ(2, segments.size());
   EXPECT_NEAR(40, RouteLength(segments[0]), 1e-4);
@@ -147,6 +150,44 @@ TEST_F(PncMapTest, GetRouteSegments) {
   EXPECT_FALSE(segments[1].IsOnSegment());
 }
 
+TEST_F(PncMapTest, UpdatePosition) {
+  auto lane1 = hdmap_.GetLaneById(hdmap::MakeMapId("9_1_-1"));
+  auto first_point = lane1->GetSmoothPoint(5);
+  pnc_map_->route_index_.clear();
+  pnc_map_->UpdatePosition(first_point);
+  EXPECT_EQ(3, pnc_map_->route_index_.size());
+  EXPECT_EQ(0, pnc_map_->route_index_[0]);
+  EXPECT_EQ(2, pnc_map_->route_index_[1]);
+  EXPECT_EQ(0, pnc_map_->route_index_[2]);
+
+  EXPECT_EQ(lane1, pnc_map_->current_waypoint_.lane);
+  EXPECT_NEAR(5, pnc_map_->current_waypoint_.s, 1e-4);
+  EXPECT_EQ(first_point.x(), pnc_map_->passage_start_point_.x());
+  EXPECT_EQ(first_point.y(), pnc_map_->passage_start_point_.y());
+  EXPECT_EQ(first_point.z(), pnc_map_->passage_start_point_.z());
+  EXPECT_EQ(first_point.x(), pnc_map_->current_point_.x());
+  EXPECT_EQ(first_point.y(), pnc_map_->current_point_.y());
+  EXPECT_EQ(first_point.z(), pnc_map_->current_point_.z());
+
+  auto second_point = lane1->GetSmoothPoint(6);
+  pnc_map_->UpdatePosition(second_point);
+  EXPECT_EQ(3, pnc_map_->route_index_.size());
+  EXPECT_EQ(0, pnc_map_->route_index_[0]);
+  EXPECT_EQ(2, pnc_map_->route_index_[1]);
+  EXPECT_EQ(0, pnc_map_->route_index_[2]);
+
+  EXPECT_EQ(lane1, pnc_map_->current_waypoint_.lane);
+  EXPECT_NEAR(6, pnc_map_->current_waypoint_.s, 1e-4);
+  EXPECT_EQ(first_point.x(), pnc_map_->passage_start_point_.x());
+  EXPECT_EQ(first_point.y(), pnc_map_->passage_start_point_.y());
+  EXPECT_EQ(first_point.z(), pnc_map_->passage_start_point_.z());
+  EXPECT_EQ(second_point.x(), pnc_map_->current_point_.x());
+  EXPECT_EQ(second_point.y(), pnc_map_->current_point_.y());
+  EXPECT_EQ(second_point.z(), pnc_map_->current_point_.z());
+
+  pnc_map_->route_index_.clear();
+}
+
 TEST_F(PncMapTest, GetNeighborPassages) {
   const auto& road0 = routing_.road(0);
   {

modules/planning/common/frame.cc

@@ -284,7 +284,12 @@ bool Frame::CreateReferenceLineFromRouting(
                                      ? FLAGS_look_forward_distance
                                      : FLAGS_look_forward_min_distance;
 
-  if (!pnc_map_->GetRouteSegments(position, FLAGS_look_backward_distance,
+  if (!pnc_map_->UpdatePosition(position)) {
+    AERROR << "Failed to update position: " << position.ShortDebugString()
+           << " in pnc map.";
+    return false;
+  }
+  if (!pnc_map_->GetRouteSegments(FLAGS_look_backward_distance,
                                   look_forward_distance, &route_segments)) {
     AERROR << "Failed to extract segments from routing";
     return false;

modules/planning/reference_line/reference_line_provider.cc

@@ -138,7 +138,12 @@ bool ReferenceLineProvider::CreateReferenceLineFromRouting(
                                      : FLAGS_look_forward_min_distance;
   {
     std::lock_guard<std::mutex> lock(pnc_map_mutex_);
-    if (!pnc_map_->GetRouteSegments(position, FLAGS_look_backward_distance,
+    if (!pnc_map_->UpdatePosition(position)) {
+      AERROR << "Failed to update pnc_map position: "
+             << position.ShortDebugString();
+      return false;
+    }
+    if (!pnc_map_->GetRouteSegments(FLAGS_look_backward_distance,
                                     look_forward_distance, &route_segments)) {
       AERROR << "Failed to extract segments from routing";
       return false;
@@ -161,8 +166,7 @@ bool ReferenceLineProvider::CreateReferenceLineFromRouting(
       ReferenceLine raw_reference_line(hdmap_path);
       ReferenceLine reference_line;
       if (FLAGS_enable_spiral_reference_line) {
-        if (!spiral_smoother.Smooth(raw_reference_line,
-          &reference_line)) {
+        if (!spiral_smoother.Smooth(raw_reference_line, &reference_line)) {
           AERROR << "Failed to smooth reference_line with spiral smoother";
         }
       } else {