/// <summary>
        /// Plan between two nodes
        /// </summary>
        /// <param name="start"></param>
        /// <param name="goal"></param>
        /// <param name="nodes"></param>
        /// <param name="time"></param>
        public void Plan(INavigableNode start, INavigableNode goal, out List <INavigableNode> nodes, out double time)
        {
            aStar astar = new aStar(start, goal, new List <NavigableEdge>());

            nodes = astar.Plan();
            time  = astar.TotalTime;
        }
 /// <summary>
 /// Plan between two nodes
 /// </summary>
 /// <param name="start"></param>
 /// <param name="goal"></param>
 /// <param name="nodes"></param>
 /// <param name="time"></param>
 public void Plan(INavigableNode start, INavigableNode goal, out List<INavigableNode> nodes, out double time)
 {
     aStar astar = new aStar(start, goal, new List<NavigableEdge>());
     nodes = astar.Plan();
     time = astar.TotalTime;
 }