Node3D* Algorithm::hybridAStar(Node3D& start,
const Node3D& goal,
Node3D* nodes3D,
Node2D* nodes2D,
int width,
int height,
CollisionDetection& configurationSpace,
float* dubinsLookup,
Visualize& visualization) {
// PREDECESSOR AND SUCCESSOR INDEX
int iPred/*前任*/, iSucc/*继任*/;
//*g:表示起点到当前节点的cost的累加(通常是dijkstar的代价)
float newG;
// Number of possible directions, 3 for forward driving and an additional 3 for reversing
int dir = Constants::reverse ? 6 : 3;
// Number of iterations the algorithm has run for stopping based on Constants::iterations
int iterations = 0;
// VISUALIZATION DELAY
ros::Duration d(0.003);
// OPEN LIST AS BOOST IMPLEMENTATION
typedef boost::heap::binomial_heap<Node3D*,
boost::heap::compare<CompareNodes>
> priorityQueue;
priorityQueue O;
//*h:表示当前节点到目标点估计出来的代价(通过启发函数)
// update h value
updateH(start, goal, nodes2D, dubinsLookup, width, height, configurationSpace, visualization);
// mark start as open
start.open();
// push on priority queue aka open list
O.push(&start);
iPred = start.setIdx(width, height);
nodes3D[iPred] = start;
// NODE POINTER
Node3D* nPred;
Node3D* nSucc;
// float max = 0.f;
// continue until O empty
while (!O.empty()) {
// // DEBUG
// Node3D* pre = nullptr;
// Node3D* succ = nullptr;
// std::cout << "\t--->>>" << std::endl;
// for (priorityQueue::ordered_iterator it = O.ordered_begin(); it != O.ordered_end(); ++it) {
// succ = (*it);
// std::cout << "VAL"
// << " | C:" << succ->getC()
// << " | x:" << succ->getX()
// << " | y:" << succ->getY()
// << " | t:" << helper::toDeg(succ->getT())
// << " | i:" << succ->getIdx()
// << " | O:" << succ->isOpen()
// << " | pred:" << succ->getPred()
// << std::endl;
// if (pre != nullptr) {
// if (pre->getC() > succ->getC()) {
// std::cout << "PRE"
// << " | C:" << pre->getC()
// << " | x:" << pre->getX()
// << " | y:" << pre->getY()
// << " | t:" << helper::toDeg(pre->getT())
// << " | i:" << pre->getIdx()
// << " | O:" << pre->isOpen()
// << " | pred:" << pre->getPred()
// << std::endl;
// std::cout << "SCC"
// << " | C:" << succ->getC()
// << " | x:" << succ->getX()
// << " | y:" << succ->getY()
// << " | t:" << helper::toDeg(succ->getT())
// << " | i:" << succ->getIdx()
// << " | O:" << succ->isOpen()
// << " | pred:" << succ->getPred()
// << std::endl;
// if (pre->getC() - succ->getC() > max) {
// max = pre->getC() - succ->getC();
// }
// }
// }
// pre = succ;
// }
// pop node with lowest cost from priority queue
nPred = O.top();
// set index
iPred = nPred->setIdx(width, height);
iterations++;
// RViz visualization
if (Constants::visualization) {
visualization.publishNode3DPoses(*nPred);
visualization.publishNode3DPose(*nPred);
d.sleep();
}
// _____________________________
// LAZY DELETION of rewired node
// if there exists a pointer this node has already been expanded
if (nodes3D[iPred].isClosed()) {
// pop node from the open list and start with a fresh node
O.pop();
continue;
}
// _________________
// EXPANSION OF NODE
else if (nodes3D[iPred].isOpen()) {
// add node to closed list
nodes3D[iPred].close();
// remove node from open list
O.pop();
// _________
// GOAL TEST//到达目标结束运算
if (*nPred == goal || iterations > Constants::iterations) {
// DEBUG
return nPred;
}
// ____________________
// CONTINUE WITH SEARCH
else {
// _______________________
// SEARCH WITH DUBINS SHOT//杜宾斯到点结束运算
if (Constants::dubinsShot && nPred->isInRange(goal) && nPred->getPrim() < 3) {
nSucc = dubinsShot(*nPred, goal, configurationSpace);
if (nSucc != nullptr && *nSucc == goal) {
//DEBUG
// std::cout << "max diff " << max << std::endl;
return nSucc;
}
}
// ______________________________
// SEARCH WITH FORWARD SIMULATION
//从不同的方向搜索
for (int i = 0; i < dir; i++) {
// create possible successor
nSucc = nPred->createSuccessor(i);
// set index of the successor
//创建可能的继任者
iSucc = nSucc->setIdx(width, height);
// ensure successor is on grid and traversable
//确保继任者在网格上且可遍历
if (nSucc->isOnGrid(width, height) && configurationSpace.isTraversable(nSucc)) {
// ensure successor is not on closed list or it has the same index as the predecessor
//确保继任者不在关闭列表中,或其索引与前任相同
if (!nodes3D[iSucc].isClosed() || iPred == iSucc) {
// calculate new G value
//计算起点到当前节点的cost的累加
nSucc->updateG();
newG = nSucc->getG();
// if successor not on open list or found a shorter way to the cell
//如果继任者不在打开列表中或找到了通向单元格的较短路径
if (!nodes3D[iSucc].isOpen() || newG < nodes3D[iSucc].getG() || iPred == iSucc) {
// calculate H value
//计算当前节点到目标点估计出来的代价
updateH(*nSucc, goal, nodes2D, dubinsLookup, width, height, configurationSpace, visualization);
// if the successor is in the same cell but the C value is larger
//如果后继单元格位于同一单元格中,但C值较大
if (iPred == iSucc && nSucc->getC() > nPred->getC() + Constants::tieBreaker) {
delete nSucc;
continue;
}
// if successor is in the same cell and the C value is lower, set predecessor to predecessor of predecessor
//如果后继项位于同一单元格中,并且C值较低,请将前置项设置为前置项的前置项
else if (iPred == iSucc && nSucc->getC() <= nPred->getC() + Constants::tieBreaker) {
nSucc->setPred(nPred->getPred());
}
if (nSucc->getPred() == nSucc) {
std::cout << "looping";
}
// put successor on open list
nSucc->open();
nodes3D[iSucc] = *nSucc;
O.push(&nodes3D[iSucc]);
delete nSucc;
} else { delete nSucc; }
} else { delete nSucc; }
} else { delete nSucc; }
}
}
}
}
if (O.empty()) {
return nullptr;
}
return nullptr;
}
hybridAStar
最新推荐文章于 2024-04-08 15:55:55 发布