接着上一篇planning模块(2)-参考线的生成我们继续解释GetRouteSegments函数,上一篇已经解释了CanDriveFrom函数,它是在判断从GetNeighborPassages函数获取到的drive_passages,是否可以从当前自车位置开到drive_passages中所要切换的车道上.

!segments.CanDriveFrom(adc_waypoint_)

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

GetRouteSegments函数

ExtendSegments函数

我们接下来介绍ExtendSegments函数,首先通过GetNeighborPassages获取到的drive_passages里面应该有两个,如果当前自车位置如下:

drive_passages包含了roadsegment1中的passage0和passage1

当index是passage0时

const auto last_waypoint = segments.LastWaypoint();

获取到的last_waypoint就是node1的相关车道信息

ExtendSegments(segments, sl.s() - backward_length,
                        sl.s() + forward_length, &route_segments->back())

假如从配置中获取到的back ward_length:50和forward_length:120

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

对于上面的代码的逻辑,用图示的例子来说明,红色位置是当前自车所匹配到的在参考路线上的位置

由于node1节点从自车位置向后的距离只有10m不足50m,所以需要通过GetRoutePredecessor函数从map拓扑图中获取到node1的驶入邻边,比如获取到的lane是node0(需要注意GetRoutePredecessor获取到的驶入邻边是非换道的驶入邻边,不包含换道驶入的边)

比如lane_7可以从lane_41和lane_35两条非换道车道驶入

那么此时s = first_segment.start_s=0,因为是node1的start_s

extend_s是40

现在进入while循环,由于node1从自车位置10m处向后的距离不足50m,所以需要通过GetRoutePredecessor函数获取node1的驶入邻边node0,它的长度是50,所以此时s为50

继续while循环,由于此时的s为50,所以会走进else的逻辑中

length = std::min(50, 40) = 40

extended_lane_segments.emplace_back(node0, 50 - 40 = 10, 50)

extend_s = 40 - 40 = 0

s = 50 - 40 = 10

因为此时extend_s为0所以while循环不再继续执行,此时truncated_segments使用extended_lane_segments赋值

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

接下来继续分析上面的代码逻辑,因为当前遍历的index是自车所在passage,所以对应图示是roadsegment1 passage0,所以segments包含node1车道

router_s = 0

start_s = -40

lane_segment.start_s=0 (此时的lane_segment是node1)

lane_segment.end_s=110

adjusted_start_s = std::max(-40 - 0 + 0, 0) = 0

adjusted_end_s = std::min(130 - 0 + 0, 110) = 110

因为此时truncated_segments->back()存的车道是node0和当前车道node1不是一条车道,并且unique_lanes只有node0,所以将node1加入truncated_segments.

truncated_segments目前包含

(node0, 10, 50)

(node1, 0, 110)

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

继续看上面的逻辑

目前router_s = 110, end_s = 130, router_s < end_s并且truncated_segments不为空

auto& back = truncated_segments->back()获取到的是(node1, 0, 110)

back.lane->total_length()的长度是120, back.end_s是110

origin_end_s = 110

back.end_s = std::min(110 + 130 - 110, 120) = 120

在这里把node1可行驶区间的end_s修改为了120

router_s = 110 + 120 - 110 = 120

auto last_lane = segments.back().lane;获取到是(node1, 0, 120)

此时router_s为120, end_s为130 router_s < end_s符合while循环条件,说明此时自车前方的参考线长度还没有达到我们要求的130m,但是此时node1这条车道的长度已经不够了,所以需要通过GetRouteSuccessor函数获取到node1所有驶出的非换道邻边,因为是非换道邻边所以通过GetRouteSuccessor函数获取的车道为空,而不是node2,node2是换道边,所以while循环会终止

此时truncated_segments中包含:

(node0, 10, 50)

(node1, 0, 120)

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());
    const std::string route_segment_id = absl::StrCat(road_index, "_", 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);
    }

上面代码逻辑是设置一些属性

上面是index为当前自车所在的passage,按照下图就是passage0,当index为passage1时的处理逻辑

与index为passage0时的处理逻辑类似,只不过参考线是以换道边上的自车投影点扩展的,并且扩展的是换道边的驶入驶出邻边.

如果按照上图,最后route_segments的size为2,一个是当前车道前后的扩展参考线,一个是换道边投影点的前后扩展参考线

void LaneFollowMap::UpdateRouteSegmentsLaneIds(
    const std::list<hdmap::RouteSegments>* route_segments) {
  route_segments_lane_ids_.clear();
  for (auto& route_seg : *route_segments) {
    for (auto& lane_seg : route_seg) {
      if (nullptr == lane_seg.lane) {
        continue;
      }
      route_segments_lane_ids_.insert(lane_seg.lane->id().id());
    }
  }
}

将有效的lane id存入route_segments_lane_ids_中

到此GetRouteSegments函数就介绍完了

PrioritizeChangeLane函数

void ReferenceLineProvider::PrioritizeChangeLane(
    std::list<hdmap::RouteSegments>* route_segments) {
  CHECK_NOTNULL(route_segments);
  auto iter = route_segments->begin();
  while (iter != route_segments->end()) {
    if (!iter->IsOnSegment()) {
      route_segments->splice(route_segments->begin(), *route_segments, iter);
      break;
    }
    ++iter;
  }
}

iter->IsOnSegment()这个函数获取到的值是在上面设置一些属性那里设置的,只有route_segment是自车所在车道的扩展参考线时,才会被设置为true.

也就是说PrioritizeChangeLane函数的逻辑是针对换道边的扩展参考线的,它的作用是把换道边的扩展参考线的route_segment移动到route_segments链表的最前面,也就是说这里的策略是换道的扩展参考线优先级更高,这个后面我们在决策的部分会看到.

至此,CreateRouteSegments函数就介绍完了.