​

接下来继续分析GetWaypointIndex函数

GetWaypointIndex函数

SearchForwardWaypointIndex函数

int LaneFollowMap::SearchForwardWaypointIndex(
    int start, const hdmap::LaneWaypoint& waypoint) const {
  int i = std::max(start, 0);
  while (i < static_cast<int>(route_indices_.size()) &&
         !hdmap::RouteSegments::WithinLaneSegment(route_indices_[i].segment,
                                                  waypoint)) {
    ++i;
  }
  return i;
}

route_indices_:的类型是std::vector<RouteIndex>

  struct RouteIndex {
    apollo::hdmap::LaneSegment segment;
    std::array<int, 3> index;
  };
e7d0d11e6b78416ab390a81dfc1c3435-URaR.png

​

上面图示的route_indices_

route_indices_[0].segment = lane0

route_indices_[0].index = {roadsegment0, passage0, lane0}

route_indices_[1].segment = lane1

route_indices_[1].index = {roadsegment1, passage0, lane1}

route_indices_[2].segment = lane2

route_indices_[2].index = {roadsegment1, passage1, lane2}

route_indices_[3].segment = lane3

route_indices_[3].index = {roadsegment1, passage1, lane3}

route_indices_[4].segment = lane4

route_indices_[4].index = {roadsegment1, passage2, lane4}

route_indices_[5].segment = lane5

route_indices_[5].index = {roadsegment2, passage0, lane5}

SearchForwardWaypointIndex函数的第一个参数表示的是,当前自车位置在全局路线上的匹配点在route_indices_中的索引,初始化时为-1.第二个参数是我们通过上一篇planning模块(1)-参考线的生成 中介绍的GetNearestPointFromRouting函数获取到的自车位置在全局路线上所对应的waypoint数据 ,SearchForwardWaypointIndex函数的作用就是从前往后搜索route_indices_看自车位置在全局路线上所对应的waypoint是否能和route_indices_中的哪个元素匹配上,然后返回其索引.

SearchBackwardWaypointIndex函数

是从后向前遍历route_indices_搜索,主要是为了容错

  if (forward_index == adc_route_index_ ||
      forward_index == adc_route_index_ + 1) {
    return forward_index;
  }

如果搜索到的索引在当前adc_route_index_索引附近,说明搜索到的forward_index是合理的,就直接返回forward_index

如果不是在当前adc_route_index_索引附近,并且如果向后搜索到的索引在当前adc_route_index_附近那就返回backward_index更合适,如果向后搜索不到,那就只能返回离当前自车位置比较远的索引forward_index.

UpdateNextRoutingWaypointIndex函数

std::vector<WaypointIndex> routing_waypoint_index_;
  struct WaypointIndex {
    apollo::hdmap::LaneWaypoint waypoint;
    int index;
    WaypointIndex(const apollo::hdmap::LaneWaypoint &waypoint, int index)
        : waypoint(waypoint), index(index) {}
  };

routing_waypoint_index_:表示的是算路请求指令中传入的waypoint在route_indices_中的索引等相关信息,routing_waypoint_index_是在UpdatePlanningCommand构建的

modules/planning/pnc_map/lane_follow_map/lane_follow_map.cc

  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() &&
           hdmap::RouteSegments::WithinLaneSegment(route_indices_[j].segment,
                                                   request_waypoints.Get(i))) {
      routing_waypoint_index_.emplace_back(
          hdmap::LaneWaypoint(route_indices_[j].segment.lane,
                              request_waypoints.Get(i).s()),
          j);
      ++i;
    }
  }

next_routing_waypoint_index_:表示下一个自车要开到的waypoint的索引

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_;
  }

如果当前next_routing_waypoint_index_所指向的waypoint在route_indices_索引大于当前自车在route_indices_匹配到的索引,说明在倒车所以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_;

发现当前自车waypoint匹配到的索引和当前next_routing_waypoint_index_的索引在一个车道上,并且自车的纵向距离小于当前next_routing_waypoint_index_所表示的waypoint的纵向距离说明也是在倒车next_routing_waypoint_index_要减小

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_;
  }

上面逻辑类似,是在前行所以next_routing_waypoint_index_要增加

UpdateRoutingRange函数

void LaneFollowMap::UpdateRoutingRange(int adc_index) {
  // Track routing range.
  range_lane_ids_.clear();
  range_start_ = std::max(0, adc_index - 1);
  range_end_ = 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_;
  }
}

传入的参数是当前自车waypoint的索引,根据当前自车waypoint的索引,把还没走到的lane_id存入range_lane_ids_,range_lane_ids_中, 走过的的车道就被删掉了.

  if (next_routing_waypoint_index_ == routing_waypoint_index_.size() - 1) {
    stop_for_destination_ = true;
  }

当next_routing_waypoint_index_ == routing_waypoint_index_.size() - 1
证明下一个要到达的waypoint是终点了.

到此UpdateVehicleState函数就分析完了.

GetNeighborPassages函数

std::vector<int> LaneFollowMap::GetNeighborPassages(
    const routing::RoadSegment& road, int start_passage)

road:当前自车位置匹配点所在的roadsegment索引

start_passage:当前自车位置匹配点所在的roadsegment中的所在passage索引

e7d0d11e6b78416ab390a81dfc1c3435-sRbA.png

​

18ab7f63b8bd4e6483bdfd5503d6ecdf-ZFjY.jpeg

​

需要注意一下roadsegment中passage的change_lane_type和ExtractBasicPassages函数中passage的change_lane_type不同

上面图示蓝色部分是passage的change_lane_type的构建方式,图标中是roadsegment中的change_lane_type的构建方式

roadsegment0 passage0 node0 FORWARD

roadsegment1 passage0 node1 LEFT

roadsegment1 passage1 node2 node3 RIGHT

roadsegment1 passage2 node4 FORWARD

roadsegment2 passage0 node5 FORWARD

GetNeighborPassages函数主要是针对换道的情况,如果当前自车位置在全局路线上所匹配到的位置是上图的node0,那么因为它在roadsegment中的passage的change_lane_type是FORWARD类型所以会直接返回node0所在passage的索引

如果当前自车位置在全局路线上所匹配到的位置是上图的node1,那么因为它在roadsegment中的passage的change_lane_type不是FORWARD就会继续执行下面的逻辑.

如果当前next_routing_waypoint_index_所表示的waypoint也与自车位置在全局路线上所匹配到的位置在同一个车道,同样直接返回当前自车所在passage的索引

如果change_lane_type是routing::LEFT或routing::RIGHT,那就会从map拓扑图中获取到所有当前车道的左右换道邻边的id存入neighbor_lanes中

然后在当前roadsegment中遍历所有passage中的车道,看是否有与neighbor_lanes中匹配的车道,如果存在就把匹配的车道所在的passage id存起来返回.

比如:如果当前自车位置在全局路线上所匹配到的位置是上图的node1那么它所在的passage的change_lane_type是routing::LEFT,那么就会在map的拓扑图中获取到所有它的左侧换道邻边存入neighbor_lanes中,然后在当前roadsegment1中的passage中去查找neighbor_lanes中是否有id相同的车道,比如node2,那么就会把node2所在的passage1的索引存入result中返回,这里返回的result是包含自车所在passage的,如果有符合条件的换道passage,result中也会包含.

这就是GetNeighborPassages函数所做的事情

auto drive_passages = GetNeighborPassages(road, passage_index);
  AINFO << "Neighbor passages: " << drive_passages.size();
  for (const int index : drive_passages) {
    const auto& passage = road.passage(index);
    hdmap::RouteSegments segments;
    if (!PassageToSegments(passage, &segments)) {
      ADEBUG << "Failed to convert passage to lane segments.";
      continue;
    }
e27602b604384f779ec6b083125c9ee8-koNy.png

​

假如车的当前位置如上图,在即将换道的位置,那么通过GetNeighborPassages获取到drive_passages就是roadsegment1的passage0和roadsegment1的passage1

然后遍历drive_passages分别执行下面逻辑

PassageToSegments(passage, &segments)
const PointENU nearest_point =
        index == passage_index
            ? adc_waypoint_.lane->GetSmoothPoint(adc_waypoint_.s)
            : PointFactory::ToPointENU(adc_state_);

GetSmoothPoint函数

还记得一条lane是由很多个点组成的吧,GetSmoothPoint的函数就是根据当前自车所对应的waypoint的纵向距离得到在车道上point的坐标

如果index是当前自车所在匹配的passage,那nearest_point就通过GetSmoothPoint去找平滑点,否则就使用当前来自location模块的自车位置坐标

segments.GetProjection(nearest_point, &sl, &segment_waypoint)

获取到最近点在当前passage的所有车道中横向距离最小的位置作为waypoint位置

只有当前遍历的index不是自车所在的passage时CanDriveFrom函数才会被调用,也就意味着只有存在换道的情况时CanDriveFrom函数才会被调用.

CanDriveFrom函数

    if (index != passage_index) {
      if (!segments.CanDriveFrom(adc_waypoint_)) {
        ADEBUG << "You cannot drive from current waypoint to passage: "
               << index;
        continue;
      }
    }

还记得吗adc_waypoint_是我们通过planning模块(1)-参考线的生成 GetNearestPointFromRouting函数获取到的.

  if (IsWaypointOnSegment(waypoint)) {
    return true;
  }

根据上面图示,当前自车的adc_waypoint_是在node1上,如果index是roadsegment1的passage1,IsWaypointOnSegment(waypoint)是false.

  auto point = waypoint.lane->GetSmoothPoint(waypoint.s);

  ...

  LaneWaypoint segment_waypoint;
  common::SLPoint route_sl;
  bool has_projection = GetProjection(point, &route_sl, &segment_waypoint);
  if (!has_projection) {
    AERROR << "No projection from waypoint: " << waypoint.DebugString();
    return false;
  }
  static constexpr double kMaxLaneWidth = 10.0;
  if (std::fabs(route_sl.l()) > 2 * kMaxLaneWidth) {
    return false;
  }

当index是roadsegment1的passage1时会继续向下执行

根据自车waypoint的纵向距离获取平滑点, 然后通过平滑点获取在roadsegment1的passage1上横向距离最小的投影点waypoint,如果横向距离大于20m认为无法变道

 double waypoint_heading = waypoint.lane->Heading(waypoint.s);
  double segment_heading = segment_waypoint.lane->Heading(segment_waypoint.s);
  double heading_diff =
      common::math::AngleDiff(waypoint_heading, segment_heading);
  if (std::fabs(heading_diff) > M_PI / 2) {
    ADEBUG << "Angle diff too large:" << heading_diff;
    return false;
  }

如果当前自车所在车道与目标车道(变道过去的那条车道)的车道方向角度差大于180,则认为无法变道.

double waypoint_left_width = 0.0;
  double waypoint_right_width = 0.0;
  waypoint.lane->GetWidth(waypoint.s, &waypoint_left_width,
                          &waypoint_right_width);
  double segment_left_width = 0.0;
  double segment_right_width = 0.0;
  segment_waypoint.lane->GetWidth(segment_waypoint.s, &segment_left_width,
                                  &segment_right_width);

获取自车所在车道的左右空间宽度,获取目标车道左右空间宽度 比如:1.75m

​

这个空间的宽度是从base_map.txt中的left_sample和right_sample获取到的

def1723249ef42a1b5d71b74c5d3266b-TWAs.png

​

  auto segment_projected_point =
      segment_waypoint.lane->GetSmoothPoint(segment_waypoint.s);
  double dist = common::util::DistanceXY(point, segment_projected_point);
  const double kLaneSeparationDistance = 0.3;
  if (route_sl.l() < 0) {  // waypoint at right side
    if (dist >
        waypoint_left_width + segment_right_width + kLaneSeparationDistance) {
      AERROR << "waypoint is too far to reach: " << dist;
      return false;
    }
  } else {  // waypoint at left side
    if (dist >
        waypoint_right_width + segment_left_width + kLaneSeparationDistance) {
      AERROR << "waypoint is too far to reach: " << dist;
      return false;
    }
  }

获取目标车道自车投影点的平滑点,然后计算自车所在车道上的平滑点,与目标车道投影点之间的距离,如下图红色线

3a3100db6a7447a2b7ca50a836c79a92-DzZw.png

​

route_sl.l() < 0表示自车在目标车道右侧,否则自车在目标车道左侧

如果自车在目标车道右侧,那么如果dist大于当前车道的左侧空间加上目标车道的右侧空间则无法变道

如果自车在目标车道左侧,那么如果dist大于当前车道的右侧空间加上目标车道的左侧空间则无法变道

这样CanDriveFrom函数就分析完了.

​