​

InitInSubNodeSubEdge函数

现在接着routing模块(2)-全局路线的生成上一篇继续分析

  for (auto* sub_node : sub_nodes) {
    InitInSubNodeSubEdge(sub_node, topo_node->InFromAllEdge());
    InitOutSubNodeSubEdge(sub_node, topo_node->OutToAllEdge());
  }

InitInSubNodeSubEdge函数里包含了其它一些函数,我们一个一个的讲

在InitInSubNodeSubEdge中调用了IsOverlapEnough

bool TopoNode::IsOverlapEnough(const TopoNode* sub_node,
                               const TopoEdge* edge_for_type) const {
  if (edge_for_type->Type() == TET_LEFT) {
    return (is_left_range_enough_ &&
            IsOutRangeEnough(left_out_sorted_range_, sub_node->StartS(),
                             sub_node->EndS()));
  }
  if (edge_for_type->Type() == TET_RIGHT) {
    return (is_right_range_enough_ &&
            IsOutRangeEnough(right_out_sorted_range_, sub_node->StartS(),
                             sub_node->EndS()));
  }
  if (edge_for_type->Type() == TET_FORWARD) {
    return IsOutToSucEdgeValid() && sub_node->IsInFromPreEdgeValid();
  }
  return true;
}

在IsOverlapEnough函数中,edge_for_type->Type():edge的direction_type表示拓扑图中,表示节点间edge的左/右/直关系,你可以在routing_map.txt中看到,比如下面就表示从lane_17到lane_16存在一条edge

edge {
  from_lane_id: "lane_17"
  to_lane_id: "lane_16"
  cost: 1415.6025095862331
  direction_type: LEFT
}

而传入IsOutRangeEnough函数的参数是在ConvertOutRange函数中获取到的

ConvertOutRange函数

  ConvertOutRange(pb_node_.left_out(), start_s_, end_s_,
                  &left_out_sorted_range_, &left_prefer_range_index_);
void ConvertOutRange(const RepeatedPtrField<CurveRange>& range_vec,
                     double start_s, double end_s,
                     std::vector<NodeSRange>* out_range, int* prefer_index) {
  out_range->clear();
  for (const auto& c_range : range_vec) {
    double s_s = c_range.start().s();
    double e_s = c_range.end().s();
    if (e_s < start_s || s_s > end_s || e_s < s_s) {
      continue;
    }
    s_s = std::max(start_s, s_s);
    e_s = std::min(end_s, e_s);
    NodeSRange s_range(s_s, e_s);
    out_range->push_back(std::move(s_range));
  }
  sort(out_range->begin(), out_range->end());
  int max_index = -1;
  double max_diff = 0.0;
  for (size_t i = 0; i < out_range->size(); ++i) {
    if (out_range->at(i).Length() > max_diff) {
      max_index = static_cast<int>(i);
      max_diff = out_range->at(i).Length();
    }
  }
  *prefer_index = max_index;
}

所以需要先看一下ConvertOutRange函数,它是在一个TopoNode节点初始化的时候被调用的

pb_node_.left_out()

ConvertOutRange函数所需的第一个参数

pb_node_.left_out():是通过AddOutBoundary函数获取到的,表示的是可换道的纵向范围

void AddOutBoundary(const LaneBoundary& bound, double lane_length,
                    RepeatedPtrField<CurveRange>* const out_range) {
  for (int i = 0; i < bound.boundary_type_size(); ++i) {
    if (!IsAllowedOut(bound.boundary_type(i))) {
      continue;
    }
    CurveRange* range = out_range->Add();
    range->mutable_start()->set_s(GetLengthbyRate(bound.boundary_type(i).s(),
                                                  bound.length(), lane_length));
    if (i != bound.boundary_type_size() - 1) {
      range->mutable_end()->set_s(GetLengthbyRate(
          bound.boundary_type(i + 1).s(), bound.length(), lane_length));
    } else {
      range->mutable_end()->set_s(lane_length);
    }
  }
}

比如下面是同向双车道,蓝色虚线是车道中心线,然后两条车道中心线间的boundary其实就是bound.boundary_type,它是从base_map.txt中获取到的,每一条lane结构下面都会包含这条lane的left_boundary和right_boundary,比如下面lane center line 1这条车道它的left_boundary的boundary_type有三个分别是DOTTED_WHITE, SOLID_WHITE, DOTTED_WHITE也就是白色虚线,白色实线,白色虚线并且虚线表示可以换道.

43d56e9aec834ca1a3aca3f795269023-JYKG.png

​

你能在base_map.txt文件中找到lane的结构,并能看到它的内部包含left_boundary和right_boundary

0feead187cae4e2c8bdf09d5fe36bad5-JvuX.png

​

AddOutBoundary函数

AddOutBoundary的作用就是获取out_range,out_range是RepeatedPtrField<CurveRange>* 类型

out_range它表示的是车在当前车道上行驶时可换道的区间范围

message CurvePoint {
  optional double s = 1;
}

message CurveRange {
  optional CurvePoint start = 1;
  optional CurvePoint end = 2;
}
void AddOutBoundary(const LaneBoundary& bound, double lane_length,
                    RepeatedPtrField<CurveRange>* const out_range) {
  for (int i = 0; i < bound.boundary_type_size(); ++i) {
    if (!IsAllowedOut(bound.boundary_type(i))) {
      continue;
    }
    AINFO << "boundary_type_s:" << bound.boundary_type(i).s();
    CurveRange* range = out_range->Add();
    range->mutable_start()->set_s(GetLengthbyRate(bound.boundary_type(i).s(),
                                                  bound.length(), lane_length));
    if (i != bound.boundary_type_size() - 1) {
      range->mutable_end()->set_s(GetLengthbyRate(
          bound.boundary_type(i + 1).s(), bound.length(), lane_length));
    } else {
      range->mutable_end()->set_s(lane_length);
    }
  }
}

举例说明:下面图示中间的boundary已经标明每一个boundary_type的纵向距离,比如第一段虚线是从boundary纵向距离0-40m是虚线, 40-70m是实线, 70-90是虚线.也就意味着这个boundary总长90m,现在假设lane center line1总长120m(因为boundary和lane center line1弧度不同所以长度也会不同的)

double new_length = cur_s / cur_total_length * target_length;

70 / 90 * 120 = 90.33m,这也意味着当车在 lane center line1车道上行驶时,行驶到90.33是可以切换到lane center line 2这根车道上的

7ae22e93f6d144efa9433011dbdeee91-SIXp.png

​

所以AddOutBoundary函数的作用就是在boundary存在换道boundary_type时获取当前车辆正在行驶的车道的可换道区间也就是pb_node_.left_out(),也是ConvertOutRange函数第一个参数的含义

ConvertOutRange函数第二个参数start_s:当前车道所表示节点的开始位置一般是0

ConvertOutRange函数第三个参数end_s:当前车道所表示节点的结束位置

ConvertOutRange函数第四个参数left_out_sorted_range_:保证当前车辆正在行驶的车道的可换道区间,在节点范围内

这里解释一下:虽然pb_node_.left_out()已经是按照boundary的区间比例转换为lane的可换道区间,但是要注意转换的时候是按照lane的长度,而不是按照node的长度,而node的长度有可能不是整条lane,比如你设置的终点,设置在了lane的中间某一点,那node的长度就不是lane的长度,所以这里要保证可换道区间在节点范围内

第五个参数left_prefer_range_index_:表示可换道区间范围最大的区间索引

回到Init函数中

void TopoNode::Init() {
  if (!FindAnchorPoint()) {
    AWARN << "Be attention!!! Find anchor point failed for lane: " << LaneId();
  }
  ConvertOutRange(pb_node_.left_out(), start_s_, end_s_,
                  &left_out_sorted_range_, &left_prefer_range_index_);

  is_left_range_enough_ =
      (left_prefer_range_index_ >= 0) &&
      left_out_sorted_range_[left_prefer_range_index_].IsEnoughForChangeLane();

  ConvertOutRange(pb_node_.right_out(), start_s_, end_s_,
                  &right_out_sorted_range_, &right_prefer_range_index_);
  is_right_range_enough_ = (right_prefer_range_index_ >= 0) &&
                           right_out_sorted_range_[right_prefer_range_index_]
                               .IsEnoughForChangeLane();
}

is_left_range_enough_:表示当向左可换道最大区间>5m时,认为可以换道为true

is_right_range_enough_:表示当向右可换道最大区间>5m时,认为可以换道为true

IsOverlapEnough函数

现在我们再回到IsOverlapEnough函数

bool TopoNode::IsOverlapEnough(const TopoNode* sub_node,
                               const TopoEdge* edge_for_type) const {
  if (edge_for_type->Type() == TET_LEFT) {
    return (is_left_range_enough_ &&
            IsOutRangeEnough(left_out_sorted_range_, sub_node->StartS(),
                             sub_node->EndS()));
  }
  if (edge_for_type->Type() == TET_RIGHT) {
    return (is_right_range_enough_ &&
            IsOutRangeEnough(right_out_sorted_range_, sub_node->StartS(),
                             sub_node->EndS()));
  }
  if (edge_for_type->Type() == TET_FORWARD) {
    return IsOutToSucEdgeValid() && sub_node->IsInFromPreEdgeValid();
  }
  return true;
}

IsOutRangeEnough函数

主要是判断当前子节点可行驶区间是否与向左/向右换道区间范围重叠并且符合最低换道纵向距离

bool TopoNode::IsOutRangeEnough(const std::vector<NodeSRange>& range_vec,
                                double start_s, double end_s) {
  if (!NodeSRange::IsEnoughForChangeLane(start_s, end_s)) {
    return false;
  }
  int start_index = BinarySearchForSLarger(range_vec, start_s);
  int end_index = BinarySearchForSSmaller(range_vec, end_s);

  int index_diff = end_index - start_index;
  if (start_index < 0 || end_index < 0) {
    return false;
  }
  if (index_diff > 1) {
    return true;
  }

  double pre_s_s = std::max(start_s, range_vec[start_index].StartS());
  double suc_e_s = std::min(end_s, range_vec[end_index].EndS());

  if (index_diff == 1) {
    double dlt = range_vec[start_index].EndS() - pre_s_s;
    dlt += suc_e_s - range_vec[end_index].StartS();
    return NodeSRange::IsEnoughForChangeLane(dlt);
  }
  if (index_diff == 0) {
    return NodeSRange::IsEnoughForChangeLane(pre_s_s, suc_e_s);
  }
  return false;
}

直行时判断当前子节点是否与前一个节点或子节点可以建立连接

if (edge_for_type->Type() == TET_FORWARD) {
    return IsOutToSucEdgeValid() && sub_node->IsInFromPreEdgeValid();
  }

InitOutSubNodeSubEdge函数

同理

现在按照下方图示总结一下InitInSubNodeSubEdge/InitOutSubNodeSubEdge函数

dd6571b0010144d8822e176beebcc0c0-LItf.png

​

可以看中间实际的车道示意图,粉色lane是最开始的node划分,最开始根据粉色节点构建的拓扑图是最上面的图.在上一篇routing模块(2)-全局路线的生成 2)SubTopoGraph类构建子拓扑图介绍了在构建子图的过程中会创造子节点,因为我们的例子中,只有两个waypoint,一个是起点,一个是终点,所以就将起点分成了两个子节点lane0_0和lane0_1,将终点分成了lane7_0和lane7_1两个子节点.

  for (const auto& map_iter : black_map) {
    InitSubEdge(map_iter.first);
  }

  for (const auto& map_iter : black_map) {
    AddPotentialEdge(map_iter.first);
  }

上面的过程就是将子图连接到最开始的拓扑图中的过程如上面最后的拓扑图,InitSubEdge是在找每个子节点前后的连接关系,AddPotentialEdge是在找子节点左右的连接关系

整个子图以及合并子图到原始拓扑图就介绍完了,这部分没有复杂的逻辑,所以想要熟悉只有反复的详细从头到尾多看几遍.

​