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