[Optimization] Enhancing D* Lite performance using heapq and lazy deletion

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.

Comienza en PathPlanning/DStarLite e inspecciona cómo se ordena la lista U durante update_vertex y compute_shortest_path. Compara el comportamiento actual de la cola con heapq de Python y con el enfoque propuesto de eliminación diferida mediante entry_finder, incluida la lógica de animación. Se considera completado cuando las actualizaciones de la cola evitan ordenar repetidamente la lista y preservan el comportamiento y la animación existentes de D* Lite.

Escrito por el modelo de indexación a partir del texto del issue.