如混合A星部分的代码：
while (!open_pq_.empty()) {
// take out the lowest cost neighboring node
const std::string current_id = open_pq_.top().first;
open_pq_.pop();
std::shared_ptr current_node = open_set_[current_id];
// check if an analystic curve could be connected from current
// configuration to the end configuration without collision. if so, search
// ends.
const double rs_start_time = Clock::NowInSeconds();
if (AnalyticExpansion(current_node)) {
break;
}
const double rs_end_time = Clock::NowInSeconds();
rs_time += rs_end_time - rs_start_time;
close_set_.emplace(current_node->GetIndex(), current_node);
for (size_t i = 0; i < next_node_num_; ++i) {
std::shared_ptr next_node = Next_node_generator(current_node, i);
// boundary check failure handle
if (next_node == nullptr) {
continue;
}
// check if the node is already in the close set
if (close_set_.find(next_node->GetIndex()) != close_set_.end()) {
continue;
}
// collision check
if (!ValidityCheck(next_node)) {
continue;
}
if (open_set_.find(next_node->GetIndex()) == open_set_.end()) {
explored_node_num++;
const double start_time = Clock::NowInSeconds();
CalculateNodeCost(current_node, next_node);
const double end_time = Clock::NowInSeconds();
heuristic_time += end_time - start_time;
open_set_.emplace(next_node->GetIndex(), next_node);
open_pq_.emplace(next_node->GetIndex(), next_node->GetCost());
}
//如果next_node在open_set_中，不应该比较其新的g值并更新更小的g值和父节点吗？
}
}