text: /******************************************************************************
   * Copyright 2017 The Apollo Authors. All Rights Reserved.
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   * http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   *****************************************************************************/
  
  #include "modules/map/pnc_map/pnc_map.h"
  
  #include <algorithm>
  #include <limits>
  
  #include "google/protobuf/text_format.h"
  
  #include "modules/map/proto/map_id.pb.h"
  
  #include "cyber/common/log.h"
  #include "modules/common/util/string_util.h"
  #include "modules/common/util/util.h"
  #include "modules/map/hdmap/hdmap_util.h"
  #include "modules/map/pnc_map/path.h"
  #include "modules/planning/common/planning_gflags.h"
  #include "modules/routing/common/routing_gflags.h"
  
  DEFINE_double(
      look_backward_distance, 50,
      "look backward this distance when creating reference line from routing");
  
  DEFINE_double(look_forward_short_distance, 180,
                "short look forward this distance when creating reference line "
                "from routing when ADC is slow");
  DEFINE_double(
      look_forward_long_distance, 250,
      "look forward this distance when creating reference line from routing");
  
  namespace apollo {
  namespace hdmap {
  
  using apollo::common::PointENU;
  using apollo::common::VehicleState;
  using apollo::routing::RoutingResponse;
  using common::util::MakePointENU;
  
  namespace {
  
  // Maximum lateral error used in trajectory approximation.
  const double kTrajectoryApproximationMaxError = 2.0;
  
  }  // namespace
  
  PncMap::PncMap(const HDMap *hdmap) : hdmap_(hdmap) {}
  
  const hdmap::HDMap *PncMap::hdmap() const { return hdmap_; }
  
  LaneWaypoint PncMap::ToLaneWaypoint(
      const routing::LaneWaypoint &waypoint) const {
    auto lane = hdmap_->GetLaneById(hdmap::MakeMapId(waypoint.id()));
    CHECK(lane) << "Invalid lane id: " << waypoint.id();
    return LaneWaypoint(lane, waypoint.s());
  }
  
  double PncMap::LookForwardDistance(double velocity) {
    auto forward_distance = velocity * FLAGS_look_forward_time_sec;
  
    return forward_distance > FLAGS_look_forward_short_distance
               ? FLAGS_look_forward_long_distance
               : FLAGS_look_forward_short_distance;
  }
  
  LaneSegment PncMap::ToLaneSegment(const routing::LaneSegment &segment) const {
    auto lane = hdmap_->GetLaneById(hdmap::MakeMapId(segment.id()));
    CHECK(lane) << "Invalid lane id: " << segment.id();
    return LaneSegment(lane, segment.start_s(), segment.end_s());
  }
  
  void PncMap::UpdateNextRoutingWaypointIndex(int cur_index) {
    if (cur_index < 0) {
      next_routing_waypoint_index_ = 0;
      return;
    }
    if (cur_index >= static_cast<int>(route_indices_.size())) {
      next_routing_waypoint_index_ = routing_waypoint_index_.size() - 1;
      return;
    }
    // Search backwards when the car is driven backward on the route.
    while (next_routing_waypoint_index_ != 0 &&
           next_routing_waypoint_index_ < routing_waypoint_index_.size() &&
           routing_waypoint_index_[next_routing_waypoint_index_].index >
               cur_index) {
      --next_routing_waypoint_index_;
    }
    while (next_routing_waypoint_index_ != 0 &&
           next_routing_waypoint_index_ < routing_waypoint_index_.size() &&
           routing_waypoint_index_[next_routing_waypoint_index_].index ==
               cur_index &&
           adc_waypoint_.s <
               routing_waypoint_index_[next_routing_waypoint_index_].waypoint.s) {
      --next_routing_waypoint_index_;
    }
    // Search forwards
    while (next_routing_waypoint_index_ < routing_waypoint_index_.size() &&
           routing_waypoint_index_[next_routing_waypoint_index_].index <
               cur_index) {
      ++next_routing_waypoint_index_;
    }
    while (next_routing_waypoint_index_ < routing_waypoint_index_.size() &&
           cur_index ==
               routing_waypoint_index_[next_routing_waypoint_index_].index &&
           adc_waypoint_.s >=
               routing_waypoint_index_[next_routing_waypoint_index_].waypoint.s) {
      ++next_routing_waypoint_index_;
    }
    if (next_routing_waypoint_index_ >= routing_waypoint_index_.size()) {
      next_routing_waypoint_index_ = routing_waypoint_index_.size() - 1;
    }
  }
  
  std::vector<routing::LaneWaypoint> PncMap::FutureRouteWaypoints() const {
    const auto &waypoints = routing_.routing_request().waypoint();
    return std::vector<routing::LaneWaypoint>(
        waypoints.begin() + next_routing_waypoint_index_, waypoints.end());
  }
  
  void PncMap::UpdateRoutingRange(int adc_index) {
    // Track routing range.
    if (range_start_ > adc_index || range_end_ < adc_index) {
      range_lane_ids_.clear();
      range_start_ = std::max(0, adc_index - 1);
      range_end_ = range_start_;
    }
    while (range_start_ + 1 < adc_index) {
      range_lane_ids_.erase(route_indices_[range_start_].segment.lane->id().id());
      ++range_start_;
    }
    while (range_end_ < static_cast<int>(route_indices_.size())) {
      const auto &lane_id = route_indices_[range_end_].segment.lane->id().id();
      if (range_lane_ids_.count(lane_id) != 0) {
        break;
      }
      range_lane_ids_.insert(lane_id);
      ++range_end_;
    }
  }
  
  bool PncMap::UpdateVehicleState(const VehicleState &vehicle_state) {
    if (!ValidateRouting(routing_)) {
      AERROR << "The routing is invalid when updatting vehicle state.";
      return false;
    }
    if (!adc_state_.has_x() ||
        (common::util::DistanceXY(adc_state_, vehicle_state) >
         FLAGS_replan_lateral_distance_threshold +
             FLAGS_replan_longitudinal_distance_threshold)) {
      // Position is reset, but not replan.
      next_routing_waypoint_index_ = 0;
      adc_route_index_ = -1;
      stop_for_destination_ = false;
    }
  
    adc_state_ = vehicle_state;
    if (!GetNearestPointFromRouting(vehicle_state, &adc_waypoint_)) {
      AERROR << "Failed to get waypoint from routing with point: "
             << "(" << vehicle_state.x() << ", " << vehicle_state.y() << ", "
             << vehicle_state.z() << ").";
      return false;
    }
    int route_index = GetWaypointIndex(adc_waypoint_);
    if (route_index < 0 ||
        route_index >= static_cast<int>(route_indices_.size())) {
      AERROR << "Cannot find waypoint: " << adc_waypoint_.DebugString();
      return false;
    }
  
    // Track how many routing request waypoints the adc have passed.
    UpdateNextRoutingWaypointIndex(route_index);
    adc_route_index_ = route_index;
    UpdateRoutingRange(adc_route_index_);
  
    if (routing_waypoint_index_.empty()) {
      AERROR << "No routing waypoint index.";
      return false;
    }
  
    int last_index = GetWaypointIndex(routing_waypoint_index_.back().waypoint);
    if (next_routing_waypoint_index_ == routing_waypoint_index_.size() - 1 ||
        (!stop_for_destination_ &&
         last_index == routing_waypoint_index_.back().index)) {
      stop_for_destination_ = true;
    }
    return true;
  }
  
  bool PncMap::IsNewRouting(const routing::RoutingResponse &routing) const {
    return IsNewRouting(routing_, routing);
  }
  
  bool PncMap::IsNewRouting(const routing::RoutingResponse &prev,
                            const routing::RoutingResponse &routing) {
    if (!ValidateRouting(routing)) {
      ADEBUG << "The provided routing is invalid.";
      return false;
    }
    return !common::util::IsProtoEqual(prev, routing);
  }
  
  bool PncMap::UpdateRoutingResponse(const routing::RoutingResponse &routing) {
    range_lane_ids_.clear();
    route_indices_.clear();
    all_lane_ids_.clear();
    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) {
          all_lane_ids_.insert(passage.segment(lane_index).id());
          route_indices_.emplace_back();
          route_indices_.back().segment =
              ToLaneSegment(passage.segment(lane_index));
          if (route_indices_.back().segment.lane == nullptr) {
            AERROR << "Failed to get lane segment from passage.";
            return false;
          }
          route_indices_.back().index = {road_index, passage_index, lane_index};
        }
      }
    }
  
    range_start_ = 0;
    range_end_ = 0;
    adc_route_index_ = -1;
    next_routing_waypoint_index_ = 0;
    UpdateRoutingRange(adc_route_index_);
  
    routing_waypoint_index_.clear();
    const auto &request_waypoints = routing.routing_request().waypoint();
    if (request_waypoints.empty()) {
      AERROR << "Invalid routing: no request waypoints.";
      return false;
    }
    int i = 0;
    for (size_t j = 0; j < route_indices_.size(); ++j) {
      while (i < request_waypoints.size() &&
             RouteSegments::WithinLaneSegment(route_indices_[j].segment,
                                              request_waypoints.Get(i))) {
        routing_waypoint_index_.emplace_back(
            LaneWaypoint(route_indices_[j].segment.lane,
                         request_waypoints.Get(i).s()),
            j);
        ++i;
      }
    }
    routing_ = routing;
    adc_waypoint_ = LaneWaypoint();
    stop_for_destination_ = false;
    return true;
  }
  
  const routing::RoutingResponse &PncMap::routing_response() const {
    return routing_;
  }
  
  bool PncMap::ValidateRouting(const RoutingResponse &routing) {
    const int num_road = routing.road_size();
    if (num_road == 0) {
      AERROR << "Route is empty.";
      return false;
    }
    if (!routing.has_routing_request() ||
        routing.routing_request().waypoint_size() < 2) {
      AERROR << "Routing does not have request.";
      return false;
    }
    for (const auto &waypoint : routing.routing_request().waypoint()) {
      if (!waypoint.has_id() || !waypoint.has_s()) {
        AERROR << "Routing waypoint has no lane_id or s.";
        return false;
      }
    }
    return true;
  }
  
  int PncMap::SearchForwardWaypointIndex(int start,
                                         const LaneWaypoint &waypoint) const {
    int i = std::max(start, 0);
    while (
        i < static_cast<int>(route_indices_.size()) &&
        !RouteSegments::WithinLaneSegment(route_indices_[i].segment, waypoint)) {
      ++i;
    }
    return i;
  }
  
  int PncMap::SearchBackwardWaypointIndex(int start,
                                          const LaneWaypoint &waypoint) const {
    int i = std::min(static_cast<int>(route_indices_.size() - 1), start);
    while (i >= 0 && !RouteSegments::WithinLaneSegment(route_indices_[i].segment,
                                                       waypoint)) {
      --i;
    }
    return i;
  }
  
  int PncMap::NextWaypointIndex(int index) const {
    if (index >= static_cast<int>(route_indices_.size() - 1)) {
      return static_cast<int>(route_indices_.size()) - 1;
    } else if (index < 0) {
      return 0;
    } else {
      return index + 1;
    }
  }
  
  int PncMap::GetWaypointIndex(const LaneWaypoint &waypoint) const {
    int forward_index = SearchForwardWaypointIndex(adc_route_index_, waypoint);
    if (forward_index >= static_cast<int>(route_indices_.size())) {
      return SearchBackwardWaypointIndex(adc_route_index_, waypoint);
    }
    if (forward_index == adc_route_index_ ||
        forward_index == adc_route_index_ + 1) {
      return forward_index;
    }
    auto backward_index = SearchBackwardWaypointIndex(adc_route_index_, waypoint);
    if (backward_index < 0) {
      return forward_index;
    }
  
    return (backward_index + 1 == adc_route_index_) ? backward_index
                                                    : forward_index;
  }
  
  bool PncMap::PassageToSegments(routing::Passage passage,
                                 RouteSegments *segments) const {
    CHECK_NOTNULL(segments);
    segments->clear();
    for (const auto &lane : passage.segment()) {
      auto lane_ptr = hdmap_->GetLaneById(hdmap::MakeMapId(lane.id()));
      if (!lane_ptr) {
        AERROR << "Failed to find lane: " << lane.id();
        return false;
      }
      segments->emplace_back(lane_ptr, std::max(0.0, lane.start_s()),
                             std::min(lane_ptr->total_length(), lane.end_s()));
    }
    return !segments->empty();
  }
  
  std::vector<int> PncMap::GetNeighborPassages(const routing::RoadSegment &road,
                                               int start_passage) const {
    CHECK_GE(start_passage, 0);
    CHECK_LE(start_passage, road.passage_size());
    std::vector<int> result;
    const auto &source_passage = road.passage(start_passage);
    result.emplace_back(start_passage);
    if (source_passage.change_lane_type() == routing::FORWARD) {
      return result;
    }
    if (source_passage.can_exit()) {  // No need to change lane
      return result;
    }
    RouteSegments source_segments;
    if (!PassageToSegments(source_passage, &source_segments)) {
      AERROR << "Failed to convert passage to segments";
      return result;
    }
    if (next_routing_waypoint_index_ < routing_waypoint_index_.size() &&
        source_segments.IsWaypointOnSegment(
            routing_waypoint_index_[next_routing_waypoint_index_].waypoint)) {
      ADEBUG << "Need to pass next waypoint[" << next_routing_waypoint_index_
             << "] before change lane";
      return result;
    }
    std::unordered_set<std::string> neighbor_lanes;
    if (source_passage.change_lane_type() == routing::LEFT) {
      for (const auto &segment : source_segments) {
        for (const auto &left_id :
             segment.lane->lane().left_neighbor_forward_lane_id()) {
          neighbor_lanes.insert(left_id.id());
        }
      }
    } else if (source_passage.change_lane_type() == routing::RIGHT) {
      for (const auto &segment : source_segments) {
        for (const auto &right_id :
             segment.lane->lane().right_neighbor_forward_lane_id()) {
          neighbor_lanes.insert(right_id.id());
        }
      }
    }
  
    for (int i = 0; i < road.passage_size(); ++i) {
      if (i == start_passage) {
        continue;
      }
      const auto &target_passage = road.passage(i);
      for (const auto &segment : target_passage.segment()) {
        if (neighbor_lanes.count(segment.id())) {
          result.emplace_back(i);
          break;
        }
      }
    }
    return result;
  }
  bool PncMap::GetRouteSegments(const VehicleState &vehicle_state,
                                std::list<RouteSegments> *const route_segments) {
    double look_forward_distance =
        LookForwardDistance(vehicle_state.linear_velocity());
    double look_backward_distance = FLAGS_look_backward_distance;
    return GetRouteSegments(vehicle_state, look_backward_distance,
                            look_forward_distance, route_segments);
  }
  
  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 || adc_route_index_ < 0 ||
        adc_route_index_ >= static_cast<int>(route_indices_.size())) {
      AERROR << "Invalid vehicle state in pnc_map, update vehicle state first.";
      return false;
    }
    const auto &route_index = route_indices_[adc_route_index_].index;
    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);
    for (const int index : drive_passages) {
      const auto &passage = road.passage(index);
      RouteSegments segments;
      if (!PassageToSegments(passage, &segments)) {
        ADEBUG << "Failed to convert passage to lane segments.";
        continue;
      }
      PointENU nearest_point =
          MakePointENU(adc_state_.x(), adc_state_.y(), adc_state_.z());
      if (index == passage_index) {
        nearest_point = adc_waypoint_.lane->GetSmoothPoint(adc_waypoint_.s);
      }
      common::SLPoint sl;
      LaneWaypoint segment_waypoint;
      if (!segments.GetProjection(nearest_point, &sl, &segment_waypoint)) {
        ADEBUG << "Failed to get projection from point: "
               << nearest_point.ShortDebugString();
        continue;
      }
      if (index != passage_index) {
        if (!segments.CanDriveFrom(adc_waypoint_)) {
          ADEBUG << "You cannot drive from current waypoint to passage: "
                 << index;
          continue;
        }
      }
      route_segments->emplace_back();
      const auto last_waypoint = segments.LastWaypoint();
      if (!ExtendSegments(segments, sl.s() - backward_length,
                          sl.s() + forward_length, &route_segments->back())) {
        AERROR << "Failed to extend segments with s=" << sl.s()
               << ", backward: " << backward_length
               << ", forward: " << forward_length;
        return false;
      }
      if (route_segments->back().IsWaypointOnSegment(last_waypoint)) {
        route_segments->back().SetRouteEndWaypoint(last_waypoint);
      }
      route_segments->back().SetCanExit(passage.can_exit());
      route_segments->back().SetNextAction(passage.change_lane_type());
      std::string route_segment_id =
          std::to_string(road_index) + "_" + std::to_string(index);
      route_segments->back().SetId(route_segment_id);
      route_segments->back().SetStopForDestination(stop_for_destination_);
      if (index == passage_index) {
        route_segments->back().SetIsOnSegment(true);
        route_segments->back().SetPreviousAction(routing::FORWARD);
      } else if (sl.l() > 0) {
        route_segments->back().SetPreviousAction(routing::RIGHT);
      } else {
        route_segments->back().SetPreviousAction(routing::LEFT);
      }
    }
    return !route_segments->empty();
  }
  
  bool PncMap::GetNearestPointFromRouting(const VehicleState &state,
                                          LaneWaypoint *waypoint) const {
    const double kMaxDistance = 10.0;  // meters.
    waypoint->lane = nullptr;
    std::vector<LaneInfoConstPtr> lanes;
    auto point = common::util::MakePointENU(state.x(), state.y(), state.z());
    const int status = hdmap_->GetLanesWithHeading(
        point, kMaxDistance, state.heading(), M_PI / 2.0, &lanes);
    if (status < 0) {
      AERROR << "Failed to get lane from point: " << point.ShortDebugString();
      return false;
    }
    if (lanes.empty()) {
      AERROR << "No valid lane found within " << kMaxDistance
             << " meters with heading " << state.heading();
      return false;
    }
    std::vector<LaneInfoConstPtr> valid_lanes;
    std::copy_if(lanes.begin(), lanes.end(), std::back_inserter(valid_lanes),
                 [&](LaneInfoConstPtr ptr) {
                   return range_lane_ids_.count(ptr->lane().id().id()) > 0;
                 });
    if (valid_lanes.empty()) {
      std::copy_if(lanes.begin(), lanes.end(), std::back_inserter(valid_lanes),
                   [&](LaneInfoConstPtr ptr) {
                     return all_lane_ids_.count(ptr->lane().id().id()) > 0;
                   });
    }
  
    // Get nearest_wayponints for current position
    double min_distance = std::numeric_limits<double>::infinity();
    for (const auto &lane : valid_lanes) {
      if (range_lane_ids_.count(lane->id().id()) == 0) {
        continue;
      }
      {
        double s = 0.0;
        double l = 0.0;
        if (!lane->GetProjection({point.x(), point.y()}, &s, &l)) {
          return false;
        }
        // Use large epsilon to allow projection diff
        constexpr double kEpsilon = 0.5;
        if (s > (lane->total_length() + kEpsilon) || (s + kEpsilon) < 0.0) {
          continue;
        }
      }
      double distance = 0.0;
      common::PointENU map_point =
          lane->GetNearestPoint({point.x(), point.y()}, &distance);
      if (distance < min_distance) {
        min_distance = distance;
        double s = 0.0;
        double l = 0.0;
        if (!lane->GetProjection({map_point.x(), map_point.y()}, &s, &l)) {
          AERROR << "Failed to get projection for map_point: "
                 << map_point.DebugString();
          return false;
        }
        waypoint->lane = lane;
        waypoint->s = s;
      }
    }
    if (waypoint->lane == nullptr) {
      AERROR << "Failed to find nearest point: " << point.ShortDebugString();
    }
    return waypoint->lane != nullptr;
  }
  
  LaneInfoConstPtr PncMap::GetRouteSuccessor(LaneInfoConstPtr lane) const {
    if (lane->lane().successor_id_size() == 0) {
      return nullptr;
    }
    hdmap::Id preferred_id = lane->lane().successor_id(0);
    for (const auto &lane_id : lane->lane().successor_id()) {
      if (range_lane_ids_.count(lane_id.id()) != 0) {
        preferred_id = lane_id;
        break;
      }
    }
    return hdmap_->GetLaneById(preferred_id);
  }
  
  LaneInfoConstPtr PncMap::GetRoutePredecessor(LaneInfoConstPtr lane) const {
    if (lane->lane().predecessor_id_size() == 0) {
      return nullptr;
    }
    hdmap::Id preferred_id = lane->lane().predecessor_id(0);
    for (const auto &lane_id : lane->lane().predecessor_id()) {
      if (range_lane_ids_.count(lane_id.id()) != 0) {
        preferred_id = lane_id;
        break;
      }
    }
    return hdmap_->GetLaneById(preferred_id);
  }
  
  bool PncMap::ExtendSegments(const RouteSegments &segments,
                              const common::PointENU &point, double look_backward,
                              double look_forward,
                              RouteSegments *extended_segments) {
    common::SLPoint sl;
    LaneWaypoint waypoint;
    if (!segments.GetProjection(point, &sl, &waypoint)) {
      AERROR << "point: " << point.ShortDebugString() << " is not on segment";
      return false;
    }
    return ExtendSegments(segments, sl.s() - look_backward, sl.s() + look_forward,
                          extended_segments);
  }
  
  bool PncMap::ExtendSegments(const RouteSegments &segments, double start_s,
                              double end_s,
                              RouteSegments *const truncated_segments) const {
    if (segments.empty()) {
      AERROR << "The input segments is empty";
      return false;
    }
    CHECK_NOTNULL(truncated_segments);
    truncated_segments->SetProperties(segments);
  
    if (start_s >= end_s) {
      AERROR << "start_s(" << start_s << " >= end_s(" << end_s << ")";
      return false;
    }
    std::unordered_set<std::string> unique_lanes;
    constexpr double kRouteEpsilon = 1e-3;
    // Extend the trajectory towards the start of the trajectory.
    if (start_s < 0) {
      const auto &first_segment = *segments.begin();
      auto lane = first_segment.lane;
      double s = first_segment.start_s;
      double extend_s = -start_s;
      std::vector<LaneSegment> extended_lane_segments;
      while (extend_s > kRouteEpsilon) {
        if (s <= kRouteEpsilon) {
          lane = GetRoutePredecessor(lane);
          if (lane == nullptr ||
              unique_lanes.find(lane->id().id()) != unique_lanes.end()) {
            break;
          }
          s = lane->total_length();
        } else {
          const double length = std::min(s, extend_s);
          extended_lane_segments.emplace_back(lane, s - length, s);
          extend_s -= length;
          s -= length;
          unique_lanes.insert(lane->id().id());
        }
      }
      truncated_segments->insert(truncated_segments->begin(),
                                 extended_lane_segments.rbegin(),
                                 extended_lane_segments.rend());
    }
    bool found_loop = false;
    double router_s = 0;
    for (const auto &lane_segment : segments) {
      const double adjusted_start_s = std::max(
          start_s - router_s + lane_segment.start_s, lane_segment.start_s);
      const double adjusted_end_s =
          std::min(end_s - router_s + lane_segment.start_s, lane_segment.end_s);
      if (adjusted_start_s < adjusted_end_s) {
        if (!truncated_segments->empty() &&
            truncated_segments->back().lane->id().id() ==
                lane_segment.lane->id().id()) {
          truncated_segments->back().end_s = adjusted_end_s;
        } else if (unique_lanes.find(lane_segment.lane->id().id()) ==
                   unique_lanes.end()) {
          truncated_segments->emplace_back(lane_segment.lane, adjusted_start_s,
                                           adjusted_end_s);
          unique_lanes.insert(lane_segment.lane->id().id());
        } else {
          found_loop = true;
          break;
        }
      }
      router_s += (lane_segment.end_s - lane_segment.start_s);
      if (router_s > end_s) {
        break;
      }
    }
    if (found_loop) {
      return true;
    }
    // Extend the trajectory towards the end of the trajectory.
    if (router_s < end_s && !truncated_segments->empty()) {
      auto &back = truncated_segments->back();
      if (back.lane->total_length() > back.end_s) {
        double origin_end_s = back.end_s;
        back.end_s =
            std::min(back.end_s + end_s - router_s, back.lane->total_length());
        router_s += back.end_s - origin_end_s;
      }
    }
    auto last_lane = segments.back().lane;
    while (router_s < end_s - kRouteEpsilon) {
      last_lane = GetRouteSuccessor(last_lane);
      if (last_lane == nullptr ||
          unique_lanes.find(last_lane->id().id()) != unique_lanes.end()) {
        break;
      }
      const double length = std::min(end_s - router_s, last_lane->total_length());
      truncated_segments->emplace_back(last_lane, 0, length);
      unique_lanes.insert(last_lane->id().id());
      router_s += length;
    }
    return true;
  }
  
  void PncMap::AppendLaneToPoints(LaneInfoConstPtr lane, const double start_s,
                                  const double end_s,
                                  std::vector<MapPathPoint> *const points) {
    if (points == nullptr || start_s >= end_s) {
      return;
    }
    double accumulate_s = 0.0;
    for (size_t i = 0; i < lane->points().size(); ++i) {
      if (accumulate_s >= start_s && accumulate_s <= end_s) {
        points->emplace_back(lane->points()[i], lane->headings()[i],
                             LaneWaypoint(lane, accumulate_s));
      }
      if (i < lane->segments().size()) {
        const auto &segment = lane->segments()[i];
        const double next_accumulate_s = accumulate_s + segment.length();
        if (start_s > accumulate_s && start_s < next_accumulate_s) {
          points->emplace_back(segment.start() + segment.unit_direction() *
                                                     (start_s - accumulate_s),
                               lane->headings()[i], LaneWaypoint(lane, start_s));
        }
        if (end_s > accumulate_s && end_s < next_accumulate_s) {
          points->emplace_back(
              segment.start() + segment.unit_direction() * (end_s - accumulate_s),
              lane->headings()[i], LaneWaypoint(lane, end_s));
        }
        accumulate_s = next_accumulate_s;
      }
      if (accumulate_s > end_s) {
        break;
      }
    }
  }
  
  }  // namespace hdmap
  }  // namespace apollo
  