​

在routing模块(1)-全局路线的生成上篇文章中已经介绍了起点,途径点,终点构造成LaneWaypoint数据格式的过程,并且它们是存储在apollo::routing::RoutingRequest数据类型中的waypoint中

message RoutingRequest {
  optional apollo.common.Header header = 1;
  // at least two points. The first is start point, the end is final point.
  // The routing must go through each point in waypoint.
  repeated apollo.routing.LaneWaypoint waypoint = 2;
  repeated apollo.routing.LaneSegment blacklisted_lane = 3;
  repeated string blacklisted_road = 4;
  optional bool broadcast = 5 [default = true];
  optional apollo.routing.ParkingInfo parking_info = 6 [deprecated = true];
  // If the start pose is set as the first point of "way_point".
  optional bool is_start_pose_set = 7 [default = false];
}

第一篇也是Convert函数的处理过程

bool convert_result = Convert(command, routing_request);

接下来我们继续讲Convert函数之后的部分

routing_->Process(routing_request, routing_response.get())

在Process函数中FillLaneInfoIfMissing函数

std::vector<routing::RoutingRequest> Routing::FillLaneInfoIfMissing(
    const routing::RoutingRequest& routing_request)

这是一个特殊场景下的处理逻辑,只有当routing_request中的waypoint没有包含id数据的情况下才会走这个处理逻辑,可以先忽略掉这些细节,认为此函数返回的请求就只有一个,就是我们传入的请求

fixed_requests.push_back(fixed_request);

这个函数就直接返回fixed_requests,只包含起点和终点信息的一个请求.

通过dreamview plus下发算路请求时,起点和终点的waypoint数据中都是有id信息的,所以暂时不必在这个函数本身纠结,并且它里面其实也没什么复杂算法.

继续

1.SearchRoute函数

navigator_ptr_->SearchRoute(fixed_request, &routing_response_temp)

在SearchRoute函数中,需要介绍一下Init函数

Init(request, graph_.get(), &way_nodes, &way_s)

request:入参算路请求

graph_.get():入参路网拓扑图

way_nodes:出参算路请求中waypoint在拓扑图中对应的节点node

way_s:waypoint在离它最近的segment线段上投影点的纵向距离

我们可以把routing_map.txt文件中的edge和地图上的车道进行比对

可以看到lane_0和lane_1是相邻车道,lane_0和lane_35,lane_0和lane_46是衔接车道.这样在routing_map.txt中lane_0和lane_1构成了一条边,lane_0和lane_35构成了一条边,lane_0和lane_46构成了一条边.并且这里每条lane都会构造成node结构,每个相邻的,衔接的node之间建立edge的概念,就这样所有的车道,也就是所有的node结构和edge关系,构建了一个有向的拓扑图,也就是graph_.get()所表示的含义

f5e778e7689e4a559230b953ff832976-zltF.png

​

因为我们的request算路指令中只有起点和终点的waypoint信息,所以Init函数的作用就是在拓扑图中查找起点和终点对应的节点其实也是waypoint所在车道,在拓扑图中所在的节点然后存到way_nodes中,起点和终点在离它最近的segment线段上投影点的纵向距离存入way_s.

因为我们算路请求指令中没有设置道路或车道黑名单,所以先不考虑GenerateBlackMapFromRequest

bool Navigator::Init(const routing::RoutingRequest& request,
                     const TopoGraph* graph,
                     std::vector<const TopoNode*>* const way_nodes,
                     std::vector<double>* const way_s) {
  Clear();
  if (!GetWayNodes(request, graph_.get(), way_nodes, way_s)) {
    AERROR << "Failed to find search terminal point in graph!";
    return false;
  }
  black_list_generator_->GenerateBlackMapFromRequest(request, graph_.get(),
                                                     &topo_range_manager_);
  return true;
}

2.SearchRouteByStrategy

way_nodes是我们在上面,获取到的起点和终点在拓扑图中对应的节点数据,这里我们只考虑起点和终点的case,那么way_nodes.size()就是2

way_start:起点节点

way_end:终点节点

way_start_s:起点在离它最近的segment线段上投影点的纵向距离

way_end_s:终点在离它最近的segment线段上投影点的纵向距离

for (size_t i = 1; i < way_nodes.size(); ++i) {
    const auto* way_start = way_nodes[i - 1];
    const auto* way_end = way_nodes[i];
    double way_start_s = way_s[i - 1];
    double way_end_s = way_s[i];

    TopoRangeManager full_range_manager = topo_range_manager_;
    black_list_generator_->AddBlackMapFromTerminal(
        way_start, way_end, way_start_s, way_end_s, &full_range_manager);

1)AddBlackMapFromTerminal函数

void BlackListRangeGenerator::AddBlackMapFromTerminal(
    const TopoNode* src_node, const TopoNode* dest_node, double start_s,
    double end_s, TopoRangeManager* const range_manager) const {
  double start_length = src_node->Length();
  double end_length = dest_node->Length();

  static constexpr double kEpsilon = 1e-2;
  AINFO << "start_s: " << start_s << ", start_length: " << start_length
        << "end_s: " << end_s << ", end_length: " << end_length;
  const double start_s_adjusted =
      (start_s > start_length && start_s - start_length <= kEpsilon)
          ? start_length
          : start_s;
  const double end_s_adjusted =
      (end_s > end_length && end_s - end_length <= kEpsilon) ? end_length
                                                             : end_s;

start_length:是起点所在车道长度

end_length:是终点所在车道长度

start_s_adjusted和end_s_adjusted是在考虑精度问题,因为start_length和end_length是routing_map.txt文件中定长的车道长度,而start_s和end_s是我们之前在Convert函数中计算出来的起点和终点在离它最近的segment线段上投影点的纵向距离,当设置的起点或终点正好设置到了微微超过所在车道长度的位置时,这个逻辑会起作用,现在我们就认为

start_s_adjusted = start_s
end_s_adjusted = end_s

MoveSBackward和MoveSForward主要作用是调整起点和终点的位置,以消除浮点精度误差、避免参考线分段边界、防止逻辑判断歧义

double start_cut_s = MoveSBackward(start_s_adjusted, 0.0);
  range_manager->Add(src_node, start_cut_s, start_cut_s);
  AddBlackMapFromOutParallel(src_node, start_cut_s / start_length,
                             range_manager);

  double end_cut_s = MoveSForward(end_s_adjusted, end_length);
  range_manager->Add(dest_node, end_cut_s, end_cut_s);
  AddBlackMapFromInParallel(dest_node, end_cut_s / end_length, range_manager);
void TopoRangeManager::Add(const TopoNode* node, double start_s, double end_s) {
  NodeSRange range(start_s, end_s);
  range_map_[node].push_back(range);
}
std::unordered_map<const TopoNode*, std::vector<NodeSRange>> range_map_;

 然后,将起点的节点/终点的节点以及他们对应的在离他们最近的车道上的可通行纵向范围[start_cut_s, start_cut_s]和[end_cut_s, end_cut_s],存入range_map_中,这里你应该会感到疑惑为什么可通行范围是一个点而不是一个线段,这个后面会介绍到.

AddBlackMapFromOutParallel/AddBlackMapFromInParallel

看图,对于lane_29来说通过AddBlackMapFromOutParallel函数中的GetOutParallelLane获取到的是lane_31所属的节点

对于lane_29来说通过AddBlackMapFromInParallel函数中的GetOutParallelLane获取到的也是lane_31所属节点

edge {
  from_lane_id: "lane_29"
  to_lane_id: "lane_31"
  cost: 676.48454366313979
  direction_type: LEFT
}
edge {
  from_lane_id: "lane_31"
  to_lane_id: "lane_29"
  cost: 686.48083592279852
  direction_type: RIGHT
}
98fdd0b4c85c4391acb3c9aaf651c785-pdlv.png

​

3f4ee83c8f6244c99d176563b62cbad4-CXSx.png

​

AddBlackMapFromOutParallel是在递归获取起点节点,可以变道的平行车道节点

AddBlackMapFromInParallel是在递归获取终点节点,可以变道的平行车道节点

并按照起点/终点节点可通行范围的起点在车道上所占的比例,start_cut_s / start_length去设置平行车道可通行范围的起点/终点纵向距离所在位置

同样存入range_map_哈希表中

SortAndMerge函数

void TopoRangeManager::SortAndMerge() {
  for (auto& iter : range_map_) {
    std::vector<NodeSRange> merged_range_vec;
    merge_block_range(iter.first, iter.second, &merged_range_vec);
    iter.second.assign(merged_range_vec.begin(), merged_range_vec.end());
  }
}
void merge_block_range(const TopoNode* topo_node,
                       const std::vector<NodeSRange>& origin_range,
                       std::vector<NodeSRange>* block_range) {
  std::vector<NodeSRange> sorted_origin_range(origin_range);
  std::sort(sorted_origin_range.begin(), sorted_origin_range.end());
  size_t cur_index = 0;
  auto total_size = sorted_origin_range.size();
  while (cur_index < total_size) {
    NodeSRange range(sorted_origin_range[cur_index]);
    ++cur_index;
    while (cur_index < total_size &&
           range.MergeRangeOverlap(sorted_origin_range[cur_index])) {
      ++cur_index;
    }
    if (range.EndS() < topo_node->StartS() ||
        range.StartS() > topo_node->EndS()) {
      continue;
    }
    range.SetStartS(std::max(topo_node->StartS(), range.StartS()));
    range.SetEndS(std::min(topo_node->EndS(), range.EndS()));
    block_range->push_back(std::move(range));
  }
}

说明一下,因为我们这个专栏是假设下发请求路线的指令中是没有设置黑名单车道/黑名单道路的,所以这里的每个节点的range就只有一个,比如起点就是起点的可通行range,终点就是终点的可通行range,中间没有不可通行的节点,所以sort排序也就相当于什么也没有做

MergeRangeOverlap函数

然后是MergeRangeOverlap函数,它的作用就是当节点中存在重叠的部分就进行合并

比如我们在一个节点,上设置了不可通行的范围1[start0_s, end0_s]和范围2[start1_s, end1_s].

首先,[start0_s, end0_s]和[start1_s, end1_s]已经按照start_s进行从小到到大排序了,然后因为start1_s小于end0_s,所以当前节点的不可通行范围会被重新设置为[start0_s, end1_s]也就是合并了两个不可通行范围的重叠部分这就是MergeRangeOverlap函数所做的事情.

a43957d8e9f64cd89ff6bca0b9b1f363-hwWU.png

​

bool NodeSRange::MergeRangeOverlap(const NodeSRange& other) {
  if (!IsValid() || !other.IsValid()) {
    return false;
  }
  if (other.StartS() > EndS() || other.EndS() < StartS()) {
    return false;
  }
  SetEndS(std::max(EndS(), other.EndS()));
  SetStartS(std::min(StartS(), other.StartS()));
  return true;
}

然后重新赋值给range_map_对应节点的NodeSRange值,因为本专栏在算路请求指令中并没有设置黑名单车道/道路,所以range_map_只有两个节点,start/end point所代表的节点.

所以full_range_manager的range_map_也只有两个节点

class TopoRangeManager {
 public:
  TopoRangeManager() = default;
  virtual ~TopoRangeManager() = default;

  const std::unordered_map<const TopoNode*, std::vector<NodeSRange>>& RangeMap()
      const;
  const std::vector<NodeSRange>* Find(const TopoNode* node) const;
  void PrintDebugInfo() const;

  void Clear();
  void Add(const TopoNode* node, double start_s, double end_s);
  void SortAndMerge();

 private:
  std::unordered_map<const TopoNode*, std::vector<NodeSRange>> range_map_;
};

2)SubTopoGraph类构建子拓扑图

接下来介绍SubTopoGraph类的初始化

SubTopoGraph sub_graph(full_range_manager.RangeMap());
SubTopoGraph::SubTopoGraph(
    const std::unordered_map<const TopoNode*, std::vector<NodeSRange>>&
        black_map) {
  std::vector<NodeSRange> valid_range;
  for (const auto& map_iter : black_map) {
    valid_range.clear();
    GetSortedValidRange(map_iter.first, map_iter.second, &valid_range);
    InitSubNodeByValidRange(map_iter.first, valid_range);
  }

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

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

GetSortedValidRange函数

void GetSortedValidRange(const TopoNode* topo_node,
                         const std::vector<NodeSRange>& origin_range,
                         std::vector<NodeSRange>* valid_range) {
  std::vector<NodeSRange> block_range;
  MergeBlockRange(topo_node, origin_range, &block_range);
  double start_s = topo_node->StartS();
  double end_s = topo_node->EndS();
  std::vector<double> all_value;
  all_value.push_back(start_s);
  for (const auto& range : block_range) {
    all_value.push_back(range.StartS());
    all_value.push_back(range.EndS());
  }
  all_value.push_back(end_s);
  for (size_t i = 0; i < all_value.size(); i += 2) {
    NodeSRange new_range(all_value[i], all_value[i + 1]);
    valid_range->push_back(std::move(new_range));
  }
}
b45eab1793134d0faf6ef9f5cb8fbee1-AojV.png

​

假如这条路上有三根lane,也就是三个节点,仔细看上图lane与lane之间用红点进行了分割,加入我们在算路指令中设置了黑名单lane不可通行范围,经过上面的MergeRangeOverlap函数调整后不可通行区域是node1 [start0, end1_s]范围,则GetSortedValidRange加上for循环整体的作用是

  for (const auto& map_iter : black_map) {
    valid_range.clear();
    GetSortedValidRange(map_iter.first, map_iter.second, &valid_range);
    InitSubNodeByValidRange(map_iter.first, valid_range);
  }

start_s:当前车道的起点固定为0

end_s:当前车道的终点节点长度(从routing_map.txt获取到的)

上图,一共三根车道分别为node0, node1, node2使用红点分割

首先将node0的start_s, start point的可通行范围起点start_s, start point的可通行范围终点start_s, node0的end_s, node1的start_s, node1的不可通行范围起点start0_s, node1的不可通行范围终点end1_s,node1的end_s, node2的start_s, end point可通行范围起点start_s, end point可通行范围终点start_s, node2的end_s存入all_value.

然后开始构建valid_range

 for (size_t i = 0; i < all_value.size(); i += 2) {
    NodeSRange new_range(all_value[i], all_value[i + 1]);
    valid_range->push_back(std::move(new_range));
  }

上面循环的作用就是将[node0的start_s,start point的可通行范围起点start_s ], [start point的可通行范围终点start_s, node0的end_s], [node1的start_s, node1的不可通行范围起点start0_s], [node1的不可通行范围终点end1_s, node1的end_s], [node2的start_s,  end point可通行范围起点start_s], [end point可通行范围终点start_s, node2的end_s]构建为valid_range

其实,就是将下面绿色的可通行区域存入了valid_range中.

7daaa5ea22964b9dad4989df98f86cfe-aylZ.png

​

如果请求算路指令中没有传入黑名单车道,那就是将下面绿色的可通行区域存入了valid_range中,因为只有起点/终点节点,没有黑名单车道节点.

18091d4ea96744e785aee9d40cf65e50-AzhL.png

​

InitSubNodeByValidRange函数

如果如上面图所示,只考虑起点/终点节点,不考虑黑名单车道的情况,那么下面通过GetSortedValidRange获取到的valid_range的size是2

  for (const auto& map_iter : black_map) {
    valid_range.clear();
    GetSortedValidRange(map_iter.first, map_iter.second, &valid_range);
    InitSubNodeByValidRange(map_iter.first, valid_range);
  }

InitSubNodeByValidRange主要是建立一些数据关系,供后面使用

InitSubEdge函数

  for (const auto& map_iter : black_map) {
    InitSubEdge(map_iter.first);
  }
GetSubNodes函数
bool SubTopoGraph::GetSubNodes(
    const TopoNode* node,
    std::unordered_set<TopoNode*>* const sub_nodes) const {
  const auto& iter = sub_node_map_.find(node);
  if (iter == sub_node_map_.end()) {
    return false;
  }
  sub_nodes->clear();
  sub_nodes->insert(iter->second.begin(), iter->second.end());
  return true;
}

sub_node_map_是在InitSubNodeByValidRange函数中构建的

std::unordered_map<const TopoNode*, std::unordered_set<TopoNode*>> sub_node_map_;
auto& sub_node_set = sub_node_map_[topo_node];

for (const auto& range : valid_range) {
    std::shared_ptr<TopoNode> sub_topo_node_ptr;
    sub_topo_node_ptr.reset(new TopoNode(topo_node, range));
    sub_node_set.insert(sub_topo_node_ptr.get());
  }

起点节点调用InitSubEdge通过GetSubNodes函数获取到的就是下面范围0和1所代表的新的子节点,节点还是所在车道所表示的节点,只是范围不同,终点节点也是一样.

18091d4ea96744e785aee9d40cf65e50-tsVP.png

​

​