Hi,

The current implementation of the D* Lite algorithm in PathPlanning/DStarLite uses a standard Python list for the priority queue U, which is sorted using .sort() during every update_vertex and compute_shortest_path call. This leads to a time complexity of approximately $O(n \log n)$ for queue operations in each iteration.

I have developed an optimized version that utilizes a Min-Heap (heapq) and an Entry Finder dictionary to implement Lazy Deletion. This reduces the complexity of priority queue updates to $O(\log n)$ and lookups to $O(1)$.

Reason for Change

1. Scalability: The current sorting-based approach becomes significantly slow as the grid size increases or when the map has high-frequency obstacle updates.
2. Efficiency: Using heapq with a dictionary for node tracking is the standard, high-performance way to implement incremental search algorithms.

Proposed Changes

• Replace the list.sort() mechanism with heapq.
• Introduce an entry_finder to handle node priority updates efficiently.
• Maintain consistency in the animation logic with the existing implementation.

I have already implemented and tested these changes locally and would like to submit a Pull Request.