hybridAStar_dubinsshot

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