Apollo中的PnC功能详解

1　UpdateRoutingResponse函数

　　PncMap::UpdateRoutingResponse函数只被reference_line_provider中的CreateReferenceLine函数调用。它的作用是：当有新的Routing结果到来时，用最新的Routing结果初始化PncMap中的一些变量。具体来说，首先遍历Routing结果中我们最关心的数据，即将要行驶的路线。把路线上所有的lane（的id）存放到all_lane_ids_中（注意它被定义成防止出现重复元素的std::unordered_set类型）。变量all_lane_ids_很简单，就是待行驶的路线上所有lane（的id）。

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));
        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();
  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;
}

　　接下来初始化route_indices_变量。这个route_indices_就没那么简单了，它在后面还经常用到。route_indices_元素的index字段存放了lane、passage、road三部分的索引。lane、passage、road三个东西就像套娃，从小到大。

  struct RouteIndex {
    LaneSegment segment;
    std::array index;
  };
  std::vector route_indices_;

2　UpdateRoutingRange函数

　　PncMap::UpdateRoutingRange函数的作用是得到剩余Routing路线上包含的lane。剩余Routing路线指的是从自车当前时刻实际位置所在lane的编号（或索引）开始，一直到终点。当然如果自车还未开始运动，那得到的就是整条Routing路线包含的lane。随着自车运动，剩余的routing路线会不断缩短，所以包含的lane也会不断减少。找到的结果存储在range_lane_ids_中。
　　其中，在实现上值得注意的是range_lane_ids_这个变量，它不是普通的std::vector，而是std::unordered_set。使用unordered_set而不用vector肯定有原因。unordered_set的特点是其中的元素不会有重复的。所以，range_lane_ids_中能够保证不会有重复的lane。采用unordered_set的另一个目的是防止出现环路（loop），在遍历lane时如果发现它已经在range_lane_ids_中了就立即break退出，不会添加两次。

3　PassageToSegments函数

　　顾名思义，PncMap::PassageToSegments函数的作用是根据Routing路由结果中的一段routing::Passage得到一个RouteSegments。Passage中有若干LaneSegment（proto中的类型），而得到的RouteSegments中也有若干LaneSegment（path.h中定义的结构体），没有错误的情况下它们的数量也应该是相等的。根据Passage中每个LaneSegment的id，借助高精地图的GetLaneById接口函数拿到了对应的同id的lane的指针。因此，RouteSegments相比routing::Passage不只是单薄的id了，而是包含lane在高精地图中的指针，通过指针可以拿到丰富的lane上的信息，比如左右边界、中心线等等。

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();
}

　　Passage是路由结果中最重要的一个数据结构了，它的定义在routing.proto中，如下。可见，Passage就是若干（收尾相接）的LaneSegment。不用猜，一个LaneSegment就对应一个高精地图中的lane。

message LaneSegment {
  optional string id = 1;
  optional double start_s = 2;
  optional double end_s = 3;
}
message Passage {
  repeated LaneSegment segment = 1;
  optional bool can_exit = 2;
  optional ChangeLaneType change_lane_type = 3 [default = FORWARD];
}

5　GetNearestPointFromRouting函数

　　PncMap::GetNearestPointFromRouting函数的作用是找到自车当前在哪条lane上，以及在这条lane上的具体位置（也就是$s$）。首先，根据自车的当前位姿$(x、y、θ)$调用高精地图的GetLanesWithHeading函数寻找一定范围（10米）内的所有lane。这些lane不一定在Routing的路线上，所以要挑选出同时出现在range_lane_ids_中的那些lane，存放到valid_lanes变量中。然后再根据距离找到最近的一条lane，这就是我们想要的结果。

　　在实现上使用了一个高级的库函数std::copy_if，但是这行代码的功能很简单，就是在lanes中找出在range_lane_ids_中的那些lane，将其都存到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;
               });