Astar算法我们在上一篇routing模块(4)-全局路线的生成 已经介绍了,接下来就是全局路线的生成最后一部分了.最后一部分就是GeneratePassageRegion函数
GeneratePassageRegion函数
bool ResultGenerator::GeneratePassageRegion(
const std::vector<NodeWithRange>& nodes,
const TopoRangeManager& range_manager,
routing::RoutingResponse* const result) {
std::vector<PassageInfo> passages;
if (!ExtractBasicPassages(nodes, &passages)) {
return false;
}
ExtendPassages(range_manager, &passages);
CreateRoadSegments(passages, result);
return true;
}nodes:就是我们通过上一篇SearchRouteByStrategy函数获取到的result_nodes
range_manager:中主要成员是range_map_这个是在routing模块(2)-全局路线的生成 介绍的在topo_range_manager_中range_map_主要是表示黑名单lane/road节点及不可行驶区间
result:需要返回的出参
ExtractBasicPassages函数
bool ResultGenerator::ExtractBasicPassages(
const std::vector<NodeWithRange>& nodes,
std::vector<PassageInfo>* const passages) {
ACHECK(!nodes.empty());
passages->clear();
std::vector<NodeWithRange> nodes_of_passage;
nodes_of_passage.push_back(nodes.at(0));
for (size_t i = 1; i < nodes.size(); ++i) {
auto edge =
nodes.at(i - 1).GetTopoNode()->GetOutEdgeTo(nodes.at(i).GetTopoNode());
if (edge == nullptr) {
AERROR << "Get null pointer to edge from " << nodes.at(i - 1).LaneId()
<< " to " << nodes.at(i).LaneId();
return false;
}
if (edge->Type() == TET_LEFT || edge->Type() == TET_RIGHT) {
auto change_lane_type = LEFT;
if (edge->Type() == TET_RIGHT) {
change_lane_type = RIGHT;
}
passages->emplace_back(nodes_of_passage, change_lane_type);
nodes_of_passage.clear();
}
nodes_of_passage.push_back(nodes.at(i));
}
passages->emplace_back(nodes_of_passage, FORWARD);
return true;
}
遍历nodes,先将第一个node存入nodes_of_passage,每当遇到变道边就用当前nodes_of_passage和变道类型构建一个passage,如果没有变道边最后就构建一个直行passage
ExtendPassages函数
向前扩展当前 Passage 的最后一个节点，使其“可行驶区间”尽可能对齐下一 Passage 的可行驶区间,换句话说，它负责连接两个 Passage, 确保：当前 passage 的末尾可行驶区间和下一 passage 的开始可行驶区间之间是平滑连接、长度一致、没有断层的。
void ResultGenerator::ExtendPassages(const TopoRangeManager& range_manager,
std::vector<PassageInfo>* const passages) {
int passage_num = static_cast<int>(passages->size());
for (int i = 0; i < passage_num; ++i) {
if (i < passage_num - 1) {
ExtendForward(range_manager, passages->at(i + 1), &(passages->at(i)));
}
if (i > 0) {
ExtendBackward(range_manager, passages->at(i - 1), &(passages->at(i)));
}
}
for (int i = passage_num - 1; i >= 0; --i) {
if (i < passage_num - 1) {
ExtendForward(range_manager, passages->at(i + 1), &(passages->at(i)));
}
if (i > 0) {
ExtendBackward(range_manager, passages->at(i - 1), &(passages->at(i)));
}
}
}ExtendForward函数
auto& back_node = curr_passage->nodes.back();取当前 Passage 的最后一个 NodeWithRange,如果 curr_passage 最后的 node 的可行驶区间未覆盖整个节点（也就是 EndS < FullLength）那它需要扩展。但是否能扩展，要看是否在黑名单里,若在黑名单中，那 EndS 必须保持不动.
如果当前节点不存在黑名单lane/road说明可以扩展可行驶区域
if (!IsCloseEnough(back_node.EndS(), back_node.FullLength())) {
if (!range_manager.Find(back_node.GetTopoNode())) {
if (IsCloseEnough(next_passage.nodes.back().EndS(),
next_passage.nodes.back().FullLength())) {
back_node.SetEndS(back_node.FullLength());
} else {
double adjusted_end_s = next_passage.nodes.back().EndS() /
next_passage.nodes.back().FullLength() *
back_node.FullLength();
if (adjusted_end_s > back_node.StartS()) {
adjusted_end_s = std::min(adjusted_end_s, back_node.FullLength());
back_node.SetEndS(adjusted_end_s);
ADEBUG << "ExtendForward: orig_end_s[" << back_node.EndS()
<< "] adjusted_end_s[" << adjusted_end_s << "]";
}
}
} else {
return;
}
}如果 next_passage 的最后一个节点完全可用（即可行驶范围覆盖整个车道），说明该 passage 能完整使用到底，因此 curr_passage 的最后一个节点也可以放心地扩展到其全长，从而在车道末端提供完整的衔接能力（例如支持在末端变道或直行）。
如果 next_passage 的最后一个节点仅部分可用，说明该 passage 在末端受限（可能因静态禁行区、道路截断等），此时若将 curr_passage 的末端也推到车道尽头，可能导致无法有效连接到 next_passage。因此，按相同比例缩短 curr_passage 的末端范围，使得两段 passage 在拓扑进度上对齐，提高后续路径优化的成功率。
bool allowed_to_explore = true;
while (allowed_to_explore) {
std::vector<NodeWithRange> succ_set;
for (const auto& edge :
curr_passage->nodes.back().GetTopoNode()->OutToSucEdge()) {
const auto& succ_node = edge->ToNode();
// if succ node has been inserted
if (ContainsKey(node_set_of_curr_passage, succ_node)) {
continue;
}
// if next passage is reachable from succ node
NodeWithRange reachable_node(succ_node, 0, 1.0);
if (IsReachableFromWithChangeLane(succ_node, next_passage,
&reachable_node)) {
const auto* succ_range = range_manager.Find(succ_node);
if (succ_range != nullptr && !succ_range->empty()) {
double black_s_start = succ_range->front().StartS();
if (!IsCloseEnough(black_s_start, 0.0)) {
succ_set.emplace_back(succ_node, 0.0, black_s_start);
}
} else {
if (IsCloseEnough(reachable_node.EndS(),
reachable_node.FullLength())) {
succ_set.emplace_back(succ_node, 0.0, succ_node->Length());
} else {
double push_end_s = reachable_node.EndS() /
reachable_node.FullLength() *
succ_node->Length();
succ_set.emplace_back(succ_node, 0.0, push_end_s);
}
}
}
}
if (succ_set.empty()) {
allowed_to_explore = false;
} else {
allowed_to_explore = true;
const auto& node_to_insert = GetLargestRange(succ_set);
curr_passage->nodes.push_back(node_to_insert);
node_set_of_curr_passage.emplace(node_to_insert.GetTopoNode());
}
}这段代码的逻辑是如果 curr_passage 最后的 node直行出边,可以到达next_passage中的某个节点,那么就继续扩展curr_passage中的节点.
const auto* succ_range = range_manager.Find(succ_node);
if (succ_range != nullptr && !succ_range->empty()) {
double black_s_start = succ_range->front().StartS();
if (!IsCloseEnough(black_s_start, 0.0)) {
succ_set.emplace_back(succ_node, 0.0, black_s_start);
}
}如果出边所到节点在黑名单中则做如下操作:
如果某个 successor node（后继节点）最前面一段是“可行驶区域”，但后面遇到黑名单lane/road（不可用区域），那么先把前面“可用的部分”记录下来。
else {
if (IsCloseEnough(reachable_node.EndS(),
reachable_node.FullLength())) {
succ_set.emplace_back(succ_node, 0.0, succ_node->Length());
} else {
double push_end_s = reachable_node.EndS() /
reachable_node.FullLength() *
succ_node->Length();
succ_set.emplace_back(succ_node, 0.0, push_end_s);
}
}否则,按照出边succ_node节点与next_passage中可到达的节点reachable_node的可行驶区间范围比例进行修改succ_node可行驶区间范围,存入扩展的succ_node及可行驶区间到succ_set
if (succ_set.empty()) {
allowed_to_explore = false;
} else {
allowed_to_explore = true;
const auto& node_to_insert = GetLargestRange(succ_set);
curr_passage->nodes.push_back(node_to_insert);
node_set_of_curr_passage.emplace(node_to_insert.GetTopoNode());
}当succ_set不为空时,获取succ_set中可行驶区间最大的节点,扩展到curr_passage节点中,然后继续循环,直到找不到符合条件succ_node为止.
ExtendBackward函数
auto& front_node = curr_passage->nodes.front();
// if front node starts at middle
if (!IsCloseEnough(front_node.StartS(), 0.0)) {
if (!range_manager.Find(front_node.GetTopoNode())) {
if (IsCloseEnough(prev_passage.nodes.front().StartS(), 0.0)) {
front_node.SetStartS(0.0);
} else {
double temp_s = prev_passage.nodes.front().StartS() /
prev_passage.nodes.front().FullLength() *
front_node.FullLength();
front_node.SetStartS(temp_s);
}
} else {
return;
}
}首先是获取curr_passage中的第一个节点front_node,首先会判断front_node可行驶区间的起点是否接近0,并且front_node节点不在黑名单中,如果prev_passage的第一个节点的可行驶区间起点为0,则curr_passage中的第一个节点front_node可行驶区间起点也设为0,否则按照prev_passage的第一个节点的可行驶区间起点占整个节点的比例进行设置,目的就是保证换道后车进入的位置,与车在换道前行驶过的比例衔接上.
bool allowed_to_explore = true;
while (allowed_to_explore) {
std::vector<NodeWithRange> pred_set;
for (const auto& edge :
curr_passage->nodes.front().GetTopoNode()->InFromPreEdge()) {
const auto& pred_node = edge->FromNode();
// if pred node has been inserted
if (ContainsKey(node_set_of_curr_passage, pred_node)) {
continue;
}
// if pred node is reachable from prev passage
NodeWithRange reachable_node(pred_node, 0, 1);
if (IsReachableToWithChangeLane(pred_node, prev_passage,
&reachable_node)) {
const auto* pred_range = range_manager.Find(pred_node);
if (pred_range != nullptr && !pred_range->empty()) {
double black_s_end = pred_range->back().EndS();
if (!IsCloseEnough(black_s_end, pred_node->Length())) {
pred_set.emplace_back(pred_node, black_s_end, pred_node->Length());
}
} else {
pred_set.emplace_back(pred_node, 0.0, pred_node->Length());
}
}
}
if (pred_set.empty()) {
allowed_to_explore = false;
} else {
allowed_to_explore = true;
const auto& node_to_insert = GetLargestRange(pred_set);
curr_passage->nodes.insert(curr_passage->nodes.begin(), node_to_insert);
node_set_of_curr_passage.emplace(node_to_insert.GetTopoNode());
}
}首先获取curr_passage中的第一个节点,然后获取到所有的入边,再通过入边获取到pred_node,然后去在prev_passage中查找可以到达pred_node的节点,然后判断pred_node是否在黑名单中,如果在黑名单中,就把最后可用的部分先存到pred_set中,如果不在黑名单中,就把新找到的节点加入到pred_set中,然后获取pred_set中的可行驶区间最大的节点插入到curr_passage中,然后继续循环,直到找不到符合条件的pred_node为止
CreateRoadSegments函数
CreateRoadSegments函数的作用就是换道部分构成一个roadsegment,不同passage中的节点构成roadsegment中的一个passage,直行单独构成一个roadsegment.
这部分没有复杂的算法仔细的按照图示和图表认真的过一遍就了解了,然后所有的roadsegment都存储在CreateRoadSegments函数的出参result中,然后传回给GeneratePassageRegion的出参response中
result_generator_->GeneratePassageRegion(
graph_->MapVersion(), request, result_nodes, topo_range_manager_,
response)最最后就是返回给了
modules/external_command/command_processor/command_processor_base/motion_command_processor_base.h
routing_->Process(routing_request, routing_response.get())planning_command->mutable_lane_follow_command()->CopyFrom(
*routing_response);
planning_command_writer_->Write(planning_command);然后通过下面channel发出去
"/apollo/planning/command"然后在planning模块接收
modules/planning/planning_component/planning_component.cc
planning_command_reader_ = node_->CreateReader<PlanningCommand>(
config_.topic_config().planning_command_topic(),
[this](const std::shared_ptr<PlanningCommand>& planning_command) {
AINFO << "Received planning data: run planning callback."
<< planning_command->header().DebugString();
std::lock_guard<std::mutex> lock(mutex_);
planning_command_.CopyFrom(*planning_command);
});