美文网首页
apollo routing

apollo routing

作者: 犭虫彳亍口苗 | 来源:发表于2019-10-24 17:32 被阅读0次

Routing模块中的有向图

1. 路网

将路网抽象为有向图,目的是为了方便后续路线规划算法使用。
路网抽象为有向图有以下几个需要注意的地方:

  1. 在Apollo所使用的高精地图数据中,车道的打断与车道标线的打断位置不是相同的。即,一个车道的右侧可能关联多个打断的车道标线。

2. 有向图

  1. 车道抽象为TopoNode,车道之间的连接关系抽象为TopoEdge。TopoNode与TopoEdge组成TopoGraph如下所示:
class TopoGraph {
 public:
  TopoGraph() = default;
  ~TopoGraph() = default;
  bool LoadGraph(const Graph& filename);
  const std::string& MapVersion() const;
  const std::string& MapDistrict() const;
  const TopoNode* GetNode(const std::string& id) const;
  void GetNodesByRoadId(
      const std::string& road_id,
      std::unordered_set<const TopoNode*>* const node_in_road) const;
 private:
  void Clear();
  bool LoadNodes(const Graph& graph);
  bool LoadEdges(const Graph& graph);
 private://关注此部分
  std::string map_version_;
  std::string map_district_;
  std::vector<std::shared_ptr<TopoNode> > topo_nodes_; //图中的节点
  std::vector<std::shared_ptr<TopoEdge> > topo_edges_;//图中的有向边
  std::unordered_map<std::string, int> node_index_map_;
  std::unordered_map<std::string, std::unordered_set<const TopoNode*> >
      road_node_map_;
};
  1. TopoNode与TopoEdge之间相互关联如下:
  • TopoNode的部分私有成员如下
/**
* in_from 表示进入该节点的边
* out_to 表示从该节点出去的边
* left 、right 表示TopoEdge的类型。详见TopoEdge
**/
  std::unordered_set<const TopoEdge*> in_from_all_edge_set_; //所有进入边
  std::unordered_set<const TopoEdge*> in_from_left_edge_set_;//所有左转进入边
  std::unordered_set<const TopoEdge*> in_from_right_edge_set_;//所有右转进入边
  std::unordered_set<const TopoEdge*> in_from_left_or_right_edge_set_;//所有左转与右转的进入边
  std::unordered_set<const TopoEdge*> in_from_pre_edge_set_;//所有直连进入的边
  std::unordered_set<const TopoEdge*> out_to_all_edge_set_;
  std::unordered_set<const TopoEdge*> out_to_left_edge_set_;
  std::unordered_set<const TopoEdge*> out_to_right_edge_set_;
  std::unordered_set<const TopoEdge*> out_to_left_or_right_edge_set_;
  std::unordered_set<const TopoEdge*> out_to_suc_edge_set_;

  std::unordered_map<const TopoNode*, const TopoEdge*> out_edge_map_;
  std::unordered_map<const TopoNode*, const TopoEdge*> in_edge_map_;
  • TopoEdge的部分私有成员
/**
* TopoEdge有类型区分,分别为Forward,Left,Right
* 例:如由A车道至B车道无需变道,则连接A车道与B车道之间的TopoEdge类型为Forward;A车道至B车道需
* 要向左变道,则TopoEdge类型为Left,当然此时两车道之间的车道标线应为虚线,实线是不能变道的。
**/
  Edge pb_edge_;//TopoEdge的Proto,包含Edge的类型等属性
  const TopoNode* from_node_ = nullptr;//起始节点
  const TopoNode* to_node_ = nullptr;//终止节点

下面看一看TopoGraph是如何生成的:

  • TopoGraph的Proto如下:
syntax = "proto2";

package apollo.routing;

import "modules/map/proto/map_geometry.proto";

message CurvePoint {
  optional double s = 1;
}

message CurveRange {
  optional CurvePoint start = 1;
  optional CurvePoint end = 2;
}

message Node {
  optional string lane_id = 1;
  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;
}

message Edge {
  enum DirectionType {
    FORWARD = 0;
    LEFT = 1;
    RIGHT = 2;
  }

  optional string from_lane_id = 1;
  optional string to_lane_id = 2;
  optional double cost = 3;
  optional DirectionType direction_type = 4;
}

message Graph {
  optional string hdmap_version = 1;
  optional string hdmap_district = 2;
  repeated Node node = 3;
  repeated Edge edge = 4;
}

可以看出TopoGraph的proto就是Graph。一个Graph由多个Node和Edge组成,我们要先了解Graph如何创建的。
Graph的创建是通过以下几个类完成的。大致过程为,从源文件中加载地图数据,然后构建Graph,最后将Graph序列化保存成二进制文件。在规划时,将Graph反序列化,再动态创建TopoGraph用于规划使用!


image.png

接下来就看一下Graph的整个创建过程:
创建的过程在grpah_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;
}

}  // namespace

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) {}

bool GraphCreator::Create() {
// 1. 从xml或者已经序列化的二进制文件中加载地图数据至pbmap。
  if (EndWith(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 {
    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();
// 2. 常规的一些赋值操作
  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();

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

  InitForbiddenLanes();
  const double min_turn_radius =
      VehicleConfigHelper::GetConfig().vehicle_param().min_turn_radius();
//3. 由车道生成Edge与Node,此处为关键部分。
//3.1 第一遍循环,创建Node
  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;
    }
    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_map_[lane_id] = graph_.node_size();
    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());
    }
  }
//3.2 第二遍循环创建Edge
  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;
    }
    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;
    }
//此处存在疑惑,为什么这里判断AllowedToCross的方法是,只要存在不可变道区域就认为不可cross。
//而在上面GePbNode()生成node设置可变道区域时是认为只要存在可变道区域就是可以out。
//两处互相矛盾。
    if (lane.has_left_boundary() && IsAllowedToCross(lane.left_boundary())) {
//如果左侧标线全是虚实线,就生成一个左转的edge。也就是只要存在部分实线区域就不能左转。
      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);
    }
  }
//4. 构建完毕,保存成txt或者二进制bin文件。
  if (!EndWith(dump_topo_file_path_, ".bin") &&
      !EndWith(dump_topo_file_path_, ".txt")) {
    AERROR << "Failed to dump topo data into file, incorrect file type "
           << dump_topo_file_path_;
    return false;
  }
  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;
}
  • 再来详细看一下如何通过Lane创建Node的。进入到node_creator.cc
bool IsAllowedOut(const LaneBoundaryType& type) {
  if (type.types(0) == LaneBoundaryType::DOTTED_YELLOW ||
      type.types(0) == LaneBoundaryType::DOTTED_WHITE) {
    return true;
  }
  return false;
}

double GetLengthbyRate(double cur_s, double cur_total_length,
                       double target_length) {
  double new_length = cur_s / cur_total_length * target_length;
  return std::min(new_length, target_length);
}

double GetLaneLength(const Lane& lane) {
  double length = 0.0;
  for (const auto& segment : lane.central_curve().segment()) {
    length += segment.length();
  }
  return length;
}

//设置可变道出去的区域。
void AddOutBoundary(const LaneBoundary& bound, double lane_length,
                    RepeatedPtrField<CurveRange>* const out_range) {
// 通过此段可以看出,只要车道标线存在虚实线部分,就将这一部分设置成可Out区域。
  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);
    }
  }
}

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());
  node->set_road_id(road_id);
  AddOutBoundary(lane.left_boundary(), lane_length, node->mutable_left_out());//设置Node的可左变道区域
  AddOutBoundary(lane.right_boundary(), lane_length, node->mutable_right_out());//设置Node的可右变道区域
  node->set_length(lane_length);
  node->mutable_central_curve()->CopyFrom(lane.central_curve());
  node->set_is_virtual(true);
//如果车道不是路口内车道,或者存在左右同向车道,那么就不是虚拟车道。
  if (!lane.has_junction_id() ||
      lane.left_neighbor_forward_lane_id_size() > 0 ||
      lane.right_neighbor_forward_lane_id_size() > 0) {
    node->set_is_virtual(false);
  }
}

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())
                     ? (1 / sqrt(speed_limit / routing_config.base_speed()))
                     : 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);
}

}  // namespace

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

子有向图

  1. 什么是子有向图?
    一个TopoNode可以分段划分成多个SubNode,由此导致TopoEdge也会生成过个SubEdge。SubNode与SubEdge共同组成了子有向图
  2. 为什么需要生成子有向图?
    Apollo所使用的高精度数据,在车道标线发生虚实变化时,车道并未打断。

A*算法


bool AStarStrategy::Search(const TopoGraph* graph,
                           const SubTopoGraph* sub_graph,
                           const TopoNode* src_node, const TopoNode* dest_node,
                           std::vector<NodeWithRange>* const result_nodes) {
  Clear();
  AINFO << "Start A* search algorithm.";

  std::priority_queue<SearchNode> open_set_detail;

  SearchNode src_search_node(src_node);//search node为topo node增加一个f值
  src_search_node.f = HeuristicCost(src_node, dest_node);//启发函数
  open_set_detail.push(src_search_node);//open队列,以search node的f值排序,最小的在队首,先把起点node放进去。

  open_set_.insert(src_node);//同样是open表,为什么不直接用一个open 表呢?
  g_score_[src_node] = 0.0;//第一段起点node的score值设为0
  enter_s_[src_node] = src_node->StartS();//第一段起点node的开始位置。

  SearchNode current_node;
  std::unordered_set<const TopoEdge*> next_edge_set;
  std::unordered_set<const TopoEdge*> sub_edge_set;//没啥太大作用,仅仅是用来临时存储
  while (!open_set_detail.empty()) {
    current_node = open_set_detail.top();//open队列中取f值最小的node
    const auto* from_node = current_node.topo_node;//把当前节点设置为新的搜索起点node
    if (current_node.topo_node == dest_node) {//如果当前node就是终点node,说明规划成功
      if (!Reconstruct(came_from_, from_node, result_nodes)) {//生成规划路线,调整一下路线,优先走最长的节点。
        AERROR << "Failed to reconstruct route.";
        return false;
      }
      return true;
    }
    open_set_.erase(from_node);//从open集中删除当前搜索起点node
    open_set_detail.pop();//从open队列中弹出当前搜索起点node

    if (closed_set_.count(from_node) != 0) {//
      // if showed before, just skip...
      continue;
    }
    closed_set_.emplace(from_node);//把搜索过的点放入close集

    // if residual_s is less than FLAGS_min_length_for_lane_change, only move
    // forward
    const auto& neighbor_edges =    //如果可以变道,就找出from node所有edge,如果不满足变道条件,就只找出直行edge
        (GetResidualS(from_node) > FLAGS_min_length_for_lane_change &&
         change_lane_enabled_)
            ? from_node->OutToAllEdge()
            : from_node->OutToSucEdge();
    double tentative_g_score = 0.0;
    next_edge_set.clear();
    for (const auto* edge : neighbor_edges) {//取出每个edge的subedge,车道在虚实线
      sub_edge_set.clear();
      sub_graph->GetSubInEdgesIntoSubGraph(edge, &sub_edge_set);
      next_edge_set.insert(sub_edge_set.begin(), sub_edge_set.end());
    }

    for (const auto* edge : next_edge_set) {
      const auto* to_node = edge->ToNode();
      if (closed_set_.count(to_node) == 1) {
        continue;
      }
      if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) {
        continue;
      }
      tentative_g_score =
          g_score_[current_node.topo_node] + GetCostToNeighbor(edge);
      if (edge->Type() != TopoEdgeType::TET_FORWARD) {
        tentative_g_score -=
            (edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2;
      }
      double f = tentative_g_score + HeuristicCost(to_node, dest_node);
      if (open_set_.count(to_node) != 0 && f >= g_score_[to_node]) {
        continue;
      }
      // if to_node is reached by forward, reset enter_s to start_s
      if (edge->Type() == TopoEdgeType::TET_FORWARD) {
        enter_s_[to_node] = to_node->StartS();
      } else {
        // else, add enter_s with FLAGS_min_length_for_lane_change
        // 若是以向左或向右方式抵达相邻结点to_node,则将to_node的进入距离更新为
        // 当前结点from_node的进入距离加上最小换道长度,并乘以相邻结点to_node长度
        // 与当前结点from_node长度的比值(这么做的目的是为了归一化,以便最终的代价量纲一致)。
        // 以上为参考,此处暂不明白归一化作用。
        double to_node_enter_s =
            (enter_s_[from_node] + FLAGS_min_length_for_lane_change) /
            from_node->Length() * to_node->Length();
        // enter s could be larger than end_s but should be less than length
        to_node_enter_s = std::min(to_node_enter_s, to_node->Length());
        // if enter_s is larger than end_s and to_node is dest_node
        if (to_node_enter_s > to_node->EndS() && to_node == dest_node) {
          continue;
        }
        enter_s_[to_node] = to_node_enter_s;
      }

      g_score_[to_node] = f;
      SearchNode next_node(to_node);
      next_node.f = f;
      open_set_detail.push(next_node);//没啥好说的,待搜索的候选节点依次加入open队列中
      came_from_[to_node] = from_node;
      if (open_set_.count(to_node) == 0) {
        open_set_.insert(to_node);
      }
    }
  }
  AERROR << "Failed to find goal lane with id: " << dest_node->LaneId();
  return false;
}

相关文章

网友评论

      本文标题:apollo routing

      本文链接:https://www.haomeiwen.com/subject/ljxdvctx.html