Routing模块中的有向图
1. 路网
将路网抽象为有向图,目的是为了方便后续路线规划算法使用。
路网抽象为有向图有以下几个需要注意的地方:
- 在Apollo所使用的高精地图数据中,车道的打断与车道标线的打断位置不是相同的。即,一个车道的右侧可能关联多个打断的车道标线。
2. 有向图
- 车道抽象为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_;
};
- 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);
}
子有向图
- 什么是子有向图?
一个TopoNode可以分段划分成多个SubNode,由此导致TopoEdge也会生成过个SubEdge。SubNode与SubEdge共同组成了子有向图 - 为什么需要生成子有向图?
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;
}
网友评论