Added function ComputeShortestPath

Usage:
queue -- the priority queue, assuming the elements in the queue are the
indices
graph -- the graph containing all the explored nodes
s_start -- index of the start node
s_goal -- index of the goal node
km