[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.

Inizia in PathPlanning/DStarLite e analizza come viene ordinata la lista U durante update_vertex e compute_shortest_path. Confronta il comportamento attuale della coda con heapq di Python e con l’approccio proposto della cancellazione differita tramite entry_finder, includendo la logica dell’animazione. Il lavoro è completato quando gli aggiornamenti della coda evitano di ordinare ripetutamente la lista, preservando il comportamento e l’animazione esistenti di D* Lite.

Scritto dal modello di indicizzazione a partire dal testo della issue.