Hybrid A* 算法基本流程

openset = PriorityQueue() // pop out node with least totoal cost (heuriscost + trajcost)
openset.push(start, 0)
closedset = {}
trajcost[start] = 0

while not openset.empty():
      current = openset.top()
      
      if analytic_expantion(current, goal): // using rs path 
         break;
      
      openset.pop()
      closedset.put(current)
      
      for next in kinematic_expantion(current):
          if (checkcollison(next)):
              delete next
              continue
          if find in closedset:
              delete next
              continue
          next.trajcost = current.trajcost +  calc_trajcost(current, next)
          if not find in openset:
             next.heuricost = calc_heuriscost(next, goal)
             next.totalcost = next.trajcost + next.heuriscost
             parent[next] = current
             openset.push(next)
          if find in openset and openset[next].trajcost > next.trajcost:
             openset[next].trajcost = next.trajcost
             openset[next].totalcost =  openset[next].trajcost + openset[next].heuriscost

reconstruct(goal)
delete start
delete goal