Apollo Routing拓扑地图生成源码学习

参考：

apollo介绍之Routing模块(六)

1.概念学习

  Apollo routing模块需要的是一个拓扑结构的图，要想做routing，第一步就是把原始的高精度地图转换成包含拓扑结构的导航地图，apollo中把base_map转换为routing_map。

  routing模块的建图在“apollo/modules/routing/topo_creator”中实现。

  routing模块的整体工作流程如下：

  1.TopoCreate实现读取routing_config配置文件（转弯，变道惩罚系数，设定速度等）以及高精度地图base_map.xml或者是apollo内部的protobuf格式map;

  2.TopoCreate读取相关文件后生成routing_map.*拓扑地图用于routing 模块进行路径搜索。

  3. routing 模块接受client的routing request，发送routing response给client.

 

 

1.1 topo_creator文件目录结构

 

  核心的实现程序是graph_creator.cc

1.2 拓扑地图的生成指令

  根据Routing模块的说明文档 

  Apollo中的routing_map.*路由地图可以通过命令来生成

  bash scripts/generate_routing_topo_graph.sh

  //源码generate_routing_topo_graph.sh

  DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"

  source "${DIR}/apollo_base.sh"

  # generate routing_map.bin in map directory.

  ${APOLLO_BIN_PREFIX}/modules/routing/topo_creator/topo_creator \

    --flagfile=modules/routing/conf/routing.conf \

    $@

  可以看出这段代码的主要实现根据routing/conf/routing.conf里的参数routing配置文件生成拓扑地图routing_map.bin

1.3 拓扑结构

  Apollo中的导航地图路径是车道线级别的，Apollo中节点Node就是一小段车道，edge边就是车道与车道的连接。

  一条道路分成了2段，每一段包含3条车道(lane)，下图中车道信息主要标识了车道唯一id，左边界，右边界，参考线，长度，前车道，后车道，左边相邻的车道，右边相邻的车道等这些结构化的信息，就知道车道之间的相互关系，从而规划出一条到达目的地的车道线级别的路线。

1.3.1 Node节点

  节点和边的结构在protobuf中定义，在文件"modules/routing/proto/topo_graph.proto"中

message Node {                          //节点的消息格式

  optional string lane_id = 1;        //车道段id

  optional double length = 2;        //车道长度

  repeated CurveRange left_out = 3;      //左出口？？

  repeated CurveRange right_out = 4;    //右出口？？

  optional double cost = 5;                      //节点的代价

  optional apollo.hdmap.Curve central_curve = 6;      //中心线

  optional bool is_virtual = 7 [default = true];  //是否可见

  optional string road_id = 8;     //所属道路ID

}

1.3.2 edge边

message Edge {        //边的消息格式

  enum DirectionType {      //枚举可选的边的方向类型向前，左，右

    FORWARD = 0;

    LEFT = 1;

    RIGHT = 2;

  }

  optional string from_lane_id = 1;        //edge的起始节点

  optional string to_lane_id = 2;             //edge边指向的节点

  optional double cost = 3;                    //边的代价

  optional DirectionType direction_type = 4;      //边的方向类型

}

1. 拓扑地图生成核心源码解析

2.1 main函数 topo_creator.cc

  主要实现调用graph_creator.cc里的Create()建图函数

2.2 建图graph_creator.cc

详细解析该程序。

//定义函数：判断是否允许向指定车道边界变道

bool IsAllowedToCross(const LaneBoundary& boundary) {

  for (const auto& boundary_type : boundary.boundary_type()) {

    if (boundary_type.types(0) != LaneBoundaryType::DOTTED_YELLOW &&

        boundary_type.types(0) != LaneBoundaryType::DOTTED_WHITE) {

      return false;

    }

  }

  return true;

}

//对于定义的GraphCreator类对象的构造函数初始化，定义输入、输出地图路径及

//routing_conf配置

GraphCreator::GraphCreator(const std::string& base_map_file_path,

                           const std::string& dump_topo_file_path,

                           const RoutingConfig& routing_conf)

    : base_map_file_path_(base_map_file_path),

      dump_topo_file_path_(dump_topo_file_path),

      routing_conf_(routing_conf) {}

//定义函数：获取边的ID

std::string GraphCreator::GetEdgeID(const std::string& from_id,

                                    const std::string& to_id) {

  return from_id + "->" + to_id;

}

//定义函数：增加边。其引用的GetPbEdge函数下一节详细说明

void GraphCreator::AddEdge(const Node& from_node,

                           const RepeatedPtrField<Id>& to_node_vec,

                           const Edge::DirectionType& type) {

  for (const auto& to_id : to_node_vec) {

    if (forbidden_lane_id_set_.find(to_id.id()) !=

        forbidden_lane_id_set_.end()) {

      ADEBUG << "Ignored lane [id = " << to_id.id();

      continue;

    }

    const std::string edge_id = GetEdgeID(from_node.lane_id(), to_id.id());

    if (showed_edge_id_set_.count(edge_id) != 0) {

      continue;

    }

    showed_edge_id_set_.insert(edge_id);

    const auto& iter = node_index_map_.find(to_id.id());

    if (iter == node_index_map_.end()) {

      continue;

    }

    const auto& to_node = graph_.node(iter->second);

    edge_creator::GetPbEdge(from_node, to_node, type, routing_conf_,

                            graph_.add_edge());

  }

}

//定义函数：判断是否是有效的掉头，根据最小转弯半径而定

bool GraphCreator::IsValidUTurn(const hdmap::Lane& lane, const double radius) {

  if (lane.turn() != hdmap::Lane::U_TURN) {  // not a u-turn

    return false;

  }

  // approximate the radius from start point, middle point and end point.

  if (lane.central_curve().segment().empty() ||

      !lane.central_curve().segment(0).has_line_segment()) {

    return false;

  }

  std::vector<PointENU> points;

  for (const auto& segment : lane.central_curve().segment()) {

    points.insert(points.end(), segment.line_segment().point().begin(),

                  segment.line_segment().point().end());

  }

  if (points.empty()) {

    return false;

  }

  Vec2d p1{points.front().x(), points.front().y()};

  const auto& mid = points[points.size() / 2];

  Vec2d p2{mid.x(), mid.y()};

  Vec2d p3{points.back().x(), points.back().y()};

  Vec2d q1 = ((p1 + p2) / 2);                  // middle of p1---p2

  Vec2d q2 = (p2 - p1).rotate(M_PI / 2) + q1;  // perpendicular to p1-p2

  Vec2d q3 = ((p2 + p3) / 2);                  // middle of p2 -- p3

  Vec2d q4 = (p3 - p2).rotate(M_PI / 2) + q3;  // perpendicular to p2-p3

  const double s1 = CrossProd(q3, q1, q2);

  if (std::fabs(s1) < kMathEpsilon) {  // q3 is the circle center

    return q3.DistanceTo(p1) >= radius;

  }

  const double s2 = CrossProd(q4, q1, q2);

  if (std::fabs(s2) < kMathEpsilon) {  // q4 is the circle center

    return q4.DistanceTo(p1) >= radius;

  }

  if (std::fabs(s1 - s2) < kMathEpsilon) {  // parallel case, a wide u-turn

    return true;

  }

  Vec2d center = q3 + (q4 - q3) * s1 / (s1 - s2);

  return p1.DistanceTo(center) >= radius;

}

//定义函数：初始化被禁止的车道(类型不是city_driving的)

void GraphCreator::InitForbiddenLanes() {

  for (const auto& lane : pbmap_.lane()) {

    if (lane.type() != hdmap::Lane::CITY_DRIVING) {

      forbidden_lane_id_set_.insert(lane.id().id());

    }

  }

}

//定义核心函数：建立拓扑地图函数Create()

bool GraphCreator::Create() {

  /*判断输入路径下的文件是以"xml"后缀结尾？是则加载到Map类型对象pb_map，否就显示

  加载失败信息*/

  if (absl::EndsWith(base_map_file_path_, ".xml")) {

    if (!hdmap::adapter::OpendriveAdapter::LoadData(base_map_file_path_,

                                                    &pbmap_)) {

      AERROR << "Failed to load base map file from " << base_map_file_path_;

      return false;

    }

  } else {

  //从输入路径加载protobuf格式地图到pbmap_，否就显示加载失败信息

    if (!cyber::common::GetProtoFromFile(base_map_file_path_, &pbmap_)) {

      AERROR << "Failed to load base map file from " << base_map_file_path_;

      return false;

    }

  }

  AINFO << "Number of lanes: " << pbmap_.lane_size();

//graph_为最后保存的拓扑地图，其格式在topo_graph.proto里说明

//设定地图版本，区域？

  graph_.set_hdmap_version(pbmap_.header().version());

  graph_.set_hdmap_district(pbmap_.header().district());

 //清空两个二维数组用来储存道路，车道，节点之间的对应关系

 node_index_map_.clear();

  road_id_map_.clear();

  showed_edge_id_set_.clear();

//遍历base_map中的lane,将lane与road id对应起来

  for (const auto& road : pbmap_.road()) {

    for (const auto& section : road.section()) {

      for (const auto& lane_id : section.lane_id()) {

        road_id_map_[lane_id.id()] = road.id().id();

      }

    }

  }

 //初始化禁止车道，将类型不为city_driving的车道设置放入set容器里

  InitForbiddenLanes();

  //加载最小转弯半径\apollo\modules\common\configs\proto\vehicle_config.proto

const double min_turn_radius =

      VehicleConfigHelper::GetConfig().vehicle_param().min_turn_radius();

 //遍历pbmap_中的车道

  for (const auto& lane : pbmap_.lane()) {

    const auto& lane_id = lane.id().id();

   //如果是禁止车道，属于非city_driving类型车道，则跳过

    if (forbidden_lane_id_set_.find(lane_id) != forbidden_lane_id_set_.end()) {

      ADEBUG << "Ignored lane id: " << lane_id

             << " because its type is NOT CITY_DRIVING.";

      continue;

    }

    //如果掉头是无效的，转弯半径过小则跳过

    if (lane.turn() == hdmap::Lane::U_TURN &&

        !IsValidUTurn(lane, min_turn_radius)) {

      ADEBUG << "The u-turn lane radius is too small for the vehicle to turn";

      continue;

    }

    AINFO << "Current lane id: " << lane_id;

   //存储node index和lane id的关系

    node_index_map_[lane_id] = graph_.node_size();

   // 从road_id_map里找lane id，在创建节点时指定道路id,没找到则道路id为空，创建节点引

   //用函数下一节具体介绍

    const auto iter = road_id_map_.find(lane_id);

    if (iter != road_id_map_.end()) {

      node_creator::GetPbNode(lane, iter->second, routing_conf_,

                              graph_.add_node());

    } else {

      AWARN << "Failed to find road id of lane " << lane_id;

      node_creator::GetPbNode(lane, "", routing_conf_, graph_.add_node());

    }

  }

 //又遍历pbmap_车道

 for (const auto& lane : pbmap_.lane()) {

    const auto& lane_id = lane.id().id();

  //禁止车道则跳过

    if (forbidden_lane_id_set_.find(lane_id) != forbidden_lane_id_set_.end()) {

      ADEBUG << "Ignored lane id: " << lane_id

             << " because its type is NOT CITY_DRIVING.";

      continue;

    }

   //对于遍历的每一个车道其对应的节点去创建边，AddEdge函数下一节介绍

    const auto& from_node = graph_.node(node_index_map_[lane_id]);

   //增加当前节点向前的边

    AddEdge(from_node, lane.successor_id(), Edge::FORWARD);

   //如果车道长度小于最小变道长度，则直接进入下一个循环

    if (lane.length() < FLAGS_min_length_for_lane_change) {

      continue;

    }

    //如果该车道有左边界且允许变道，增加向左边邻居变道的边

    if (lane.has_left_boundary() && IsAllowedToCross(lane.left_boundary())) {

      AddEdge(from_node, lane.left_neighbor_forward_lane_id(), Edge::LEFT);

    }

    //如果该车道有右边界且允许变道，增加向右边邻居变道的边

    if (lane.has_right_boundary() && IsAllowedToCross(lane.right_boundary())) {

      AddEdge(from_node, lane.right_neighbor_forward_lane_id(), Edge::RIGHT);

    }

  }

 //如果输出路径下的文件不是以bin && txt为后缀名，则显示错误信息

  if (!absl::EndsWith(dump_topo_file_path_, ".bin") &&

      !absl::EndsWith(dump_topo_file_path_, ".txt")) {

    AERROR << "Failed to dump topo data into file, incorrect file type "

           << dump_topo_file_path_;

    return false;

  }

  //输出拓扑地图graph_到bin,txt  

  auto type_pos = dump_topo_file_path_.find_last_of(".") + 1;

  std::string bin_file = dump_topo_file_path_.replace(type_pos, 3, "bin");

  std::string txt_file = dump_topo_file_path_.replace(type_pos, 3, "txt");

  if (!cyber::common::SetProtoToASCIIFile(graph_, txt_file)) {

    AERROR << "Failed to dump topo data into file " << txt_file;

    return false;

  }

  AINFO << "Txt file is dumped successfully. Path: " << txt_file;

  if (!cyber::common::SetProtoToBinaryFile(graph_, bin_file)) {

    AERROR << "Failed to dump topo data into file " << bin_file;

    return false;

  }

  AINFO << "Bin file is dumped successfully. Path: " << bin_file;

  return true;

}

2.3 建图引用的创建节点及边的函数

2.3.1 创建节点函数

void GetPbNode(const hdmap::Lane& lane, const std::string& road_id,

               const RoutingConfig& routingconfig, Node* const node) {

  InitNodeInfo(lane, road_id, node);    //初始化节点信息

  InitNodeCost(lane, routingconfig, node);  //初始化节点代价

}

2.3.2 初始化节点信息函数

//实际上就是对节点消息的各个参数初始化赋值

void InitNodeInfo(const Lane& lane, const std::string& road_id,

                  Node* const node) {

  double lane_length = GetLaneLength(lane);    //长度

  node->set_lane_id(lane.id().id());     //车道id

  node->set_road_id(road_id);       //道路Id

  //根据lane的边界添加能够变道的路段,每个车道节点的左出口，右出口

  AddOutBoundary(lane.left_boundary(), lane_length, node->mutable_left_out());

  AddOutBoundary(lane.right_boundary(), lane_length, node->mutable_right_out());

  node->set_length(lane_length);  //车道长度

  node->mutable_central_curve()->CopyFrom(lane.central_curve());  //中心线

  node->set_is_virtual(true);          //是否虚拟,默认是虚拟车道虚拟节点

  if (!lane.has_junction_id() ||         //没有juntion id或左邻居数量大于0,右邻居数量大于0,就设

                                                       //置成非虚拟节点

      lane.left_neighbor_forward_lane_id_size() > 0 ||

      lane.right_neighbor_forward_lane_id_size() > 0) {

    node->set_is_virtual(false);

  }

}

2.3.3 节点的代价计算函数

void InitNodeCost(const Lane& lane, const RoutingConfig& routing_config,

                  Node* const node) {

  double lane_length = GetLaneLength(lane);

  double speed_limit =

      lane.has_speed_limit() ? lane.speed_limit() : routing_config.base_speed();

  double ratio = speed_limit >= routing_config.base_speed()

                     ? std::sqrt(routing_config.base_speed() / speed_limit)

                     : 1.0;

  double cost = lane_length * ratio; //根据车道长度和速度限速比来计算代价，越接近限速代价

                                                       //越大

  if (lane.has_turn()) {                     //如果有转弯掉头代价就加上惩罚项

    if (lane.turn() == Lane::LEFT_TURN) {

      cost += routing_config.left_turn_penalty();

    } else if (lane.turn() == Lane::RIGHT_TURN) {

      cost += routing_config.right_turn_penalty();

    } else if (lane.turn() == Lane::U_TURN) {

      cost += routing_config.uturn_penalty();

    }

  }

  node->set_cost(cost);

}

2.3.4创建边函数

void GetPbEdge(const Node& node_from, const Node& node_to,

               const Edge::DirectionType& type,

               const RoutingConfig& routing_config, Edge* edge) {

 // 设置起始，终止车道和类型

  edge->set_from_lane_id(node_from.lane_id());

  edge->set_to_lane_id(node_to.lane_id());

  edge->set_direction_type(type);

 //边的默认代价为0，直接向前开

  edge->set_cost(0.0);

 //如果边的类型为左，右转，则边的代价的计算为变道的弧长？？？

  if (type == Edge::LEFT || type == Edge::RIGHT) {

    const auto& target_range =

        (type == Edge::LEFT) ? node_from.left_out() : node_from.right_out();

    double changing_area_length = 0.0;

    for (const auto& range : target_range) {

      changing_area_length += range.end().s() - range.start().s();

    }

   //定义一个比例，若变道区域长度小于设定的基本变道长度，按照一定规则增大cost

    double ratio = 1.0;

    if (changing_area_length < routing_config.base_changing_length()) {

      ratio = std::pow(

          changing_area_length / routing_config.base_changing_length(), -1.5);

    }

    edge->set_cost(routing_config.change_penalty() * ratio);

  }

}