planning: fix waypoint tracking bug in pnc_map

* change to use synchronize update vehicle state in pnc_map

modules/map/pnc_map/pnc_map.cc

@@ -99,11 +99,43 @@ LaneWaypoint PncMap::ToLaneWaypoint(
 }
 
 void PncMap::UpdateNextRoutingWaypointIndex() {
+  if (route_index_.empty()) {
+    next_routing_waypoint_index_ = 0;
+    return;
+  }
   while (next_routing_waypoint_index_ + 1 < routing_waypoints_.size()) {
     const auto &next_waypoint =
         routing_waypoints_[next_routing_waypoint_index_];
-    auto waypoint_index = GetWaypointIndex(next_waypoint);
-    if (waypoint_index < route_index_) {
+
+    std::vector<int> waypoint_index;
+    // if not found following history route_index_, search forward
+    for (int road_index = 0; road_index < routing_.road_size(); ++road_index) {
+      const auto &road_segment = routing_.road(road_index);
+      for (int passage_index = 0; passage_index < road_segment.passage_size();
+           ++passage_index) {
+        const auto &passage = road_segment.passage(passage_index);
+        for (int lane_index = 0; lane_index < passage.segment_size();
+             ++lane_index) {
+          if (RouteSegments::WithinLaneSegment(passage.segment(lane_index),
+                                               next_waypoint)) {
+            std::vector<int> index{road_index, passage_index, lane_index};
+            if (index < route_index_) {
+              continue;
+            } else {
+              waypoint_index = index;
+              break;
+            }
+          }
+        }
+        if (!waypoint_index.empty()) {
+          break;
+        }
+      }
+      if (!waypoint_index.empty()) {
+        break;
+      }
+    }
+    if (waypoint_index.empty()) {
       ++next_routing_waypoint_index_;
     } else if (waypoint_index == route_index_ &&
                adc_waypoint_.s >= next_waypoint.s) {
@@ -335,9 +367,14 @@ std::vector<int> PncMap::GetNeighborPassages(const routing::RoadSegment &road,
   return result;
 }
 
-bool PncMap::GetRouteSegments(
-    const double backward_length, const double forward_length,
-    std::list<RouteSegments> *const route_segments) const {
+bool PncMap::GetRouteSegments(const VehicleState &vehicle_state,
+                              const double backward_length,
+                              const double forward_length,
+                              std::list<RouteSegments> *const route_segments) {
+  if (!UpdateVehicleState(vehicle_state)) {
+    AERROR << "Failed to update vehicle state in pnc_map";
+    return false;
+  }
   // vehicle has to be this close to lane center before considering change
   // lane
   if (!adc_waypoint_.lane || route_index_.size() != 3) {

modules/map/pnc_map/pnc_map.h

@@ -50,16 +50,15 @@ class PncMap {
 
   bool UpdateRoutingResponse(const routing::RoutingResponse &routing_response);
 
-  bool UpdateVehicleState(const common::VehicleState &vehicle_state);
-
   const routing::RoutingResponse &routing_response() const;
 
   static bool CreatePathFromLaneSegments(const RouteSegments &segments,
                                          Path *const path);
 
-  bool GetRouteSegments(const double backward_length,
+  bool GetRouteSegments(const common::VehicleState &vehicle_state,
+                        const double backward_length,
                         const double forward_length,
-                        std::list<RouteSegments> *const route_segments) const;
+                        std::list<RouteSegments> *const route_segments);
 
   /**
    * Check if the routing is the same as existing one in PncMap
@@ -77,6 +76,7 @@ class PncMap {
   std::vector<routing::LaneWaypoint> FutureRouteWaypoints() const;
 
  private:
+  bool UpdateVehicleState(const common::VehicleState &vehicle_state);
   /**
    * @brief Find the waypoint index of a routing waypoint. It updates
    * route_index_ with a vector with three values: Road index in

modules/map/pnc_map/pnc_map_test.cc

@@ -116,8 +116,7 @@ TEST_F(PncMapTest, GetRouteSegments_NoChangeLane) {
   state.set_z(point.y());
   state.set_heading(M_PI);
   std::list<RouteSegments> segments;
-  ASSERT_TRUE(pnc_map_->UpdateVehicleState(state));
-  ASSERT_TRUE(pnc_map_->GetRouteSegments(10, 30, &segments));
+  ASSERT_TRUE(pnc_map_->GetRouteSegments(state, 10, 30, &segments));
   // first time on this passage, should not immediately change lane
   ASSERT_EQ(2, segments.size());
   EXPECT_NEAR(40, RouteLength(segments.front()), 1e-4);
@@ -138,8 +137,7 @@ TEST_F(PncMapTest, GetRouteSegments_ChangeLane) {
   state.set_z(point.y());
   state.set_heading(M_PI);
   std::list<RouteSegments> segments;
-  ASSERT_TRUE(pnc_map_->UpdateVehicleState(state));
-  bool result = pnc_map_->GetRouteSegments(10, 30, &segments);
+  bool result = pnc_map_->GetRouteSegments(state, 10, 30, &segments);
   ASSERT_TRUE(result);
   ASSERT_EQ(2, segments.size());
   const auto& first = segments.front();

modules/planning/reference_line/reference_line_provider.cc

@@ -285,7 +285,7 @@ bool ReferenceLineProvider::CreateRouteSegments(
     std::list<hdmap::RouteSegments> *segments) {
   {
     std::lock_guard<std::mutex> lock(pnc_map_mutex_);
-    if (!pnc_map_->GetRouteSegments(look_backward_distance,
+    if (!pnc_map_->GetRouteSegments(vehicle_state, look_backward_distance,
                                     look_forward_distance, segments)) {
       AERROR << "Failed to extract segments from routing";
       return false;
@@ -337,12 +337,6 @@ bool ReferenceLineProvider::CreateReferenceLine(
       segment_history_.clear();
       segment_history_id_.clear();
     }
-
-    // Update vehicle state in pnc_map
-    std::lock_guard<std::mutex> lock(pnc_map_mutex_);
-    if (!pnc_map_->UpdateVehicleState(vehicle_state)) {
-      AERROR << "Failed to update vehicle state in pnc map";
-    }
   }
 
   double look_forward_distance = LookForwardDistance(vehicle_state);