D* Lite Planner for Incremental Replanning

[2006wu]

Description

Currently, the global planner uses SmacPlanner2D.

Traditional path planning algorithms such as Dijkstra and A* search for a path based on the current costmap. When the environment changes and the current path becomes blocked, these algorithms generally need to perform a new search.

This feature aims to investigate and implement a D Lite planner* for Nav2 and compare its behavior with Dijkstra and A*.

The main idea of D* Lite is to reuse information from the previous search and update only the affected parts of the planning problem when the costmap changes, instead of solving the entire planning problem from scratch.

Original Path
Start ─────────────────────→ Goal
                 │
             New Obstacle
                 │
                 ▼
        Update affected area
                 │
                 ▼
          Repair the path

The goal is to evaluate whether incremental replanning can improve replanning efficiency in Eurobot's dynamic environment.

---

Expected Behavior

For Dijkstra and A*, when the current path is affected by a new obstacle:

Costmap Update
      │
      ▼
Run Search Again
      │
      ▼
Updated Global Path

For D* Lite:

Costmap Update
      │
      ▼
Detect Changed Area
      │
      ▼
Update D* Lite Search
      │
      ▼
Repair Existing Search
      │
      ▼
Updated Global Path

Expected benefits:

• Reuse previous planning information
• Avoid unnecessary full replanning
• Update only the affected search states
• Faster response to local costmap changes
• Efficient replanning when obstacles appear or disappear

---

Nav2 Integration

Implement D* Lite as a Nav2 planner plugin:

Costmap
   │
   ▼
D* Lite Planner
   │
   ▼
Global Path
   │
   ▼
Controller

The planner should follow the nav2_core::GlobalPlanner interface so it can be selected like the existing planners.

For comparison, use Dijkstra, A*, and D Lite* with the same global costmap.

Example:

planner_server:
  ros__parameters:
    planner_plugins: ["Dijkstra", "AStar", "DStarLite"]

    Dijkstra:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      use_astar: false

    AStar:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      use_astar: true

    DStarLite:
      plugin: "dstar_lite_planner::DStarLitePlanner"

---

Comparison with Dijkstra and A*

D* Lite should be tested against Dijkstra and A*.

Compare:

• Initial planning time
• Replanning time
• Number of expanded or updated nodes
• Path length
• Path cost
• Costmap update response
• Performance with dynamic obstacles

The main comparison focuses on what happens when the environment changes:

                Costmap Changes
                       │
          ┌────────────┼────────────┐
          ▼            ▼            ▼
      Dijkstra        A*        D* Lite
          │            │            │
          ▼            ▼            ▼
     New Search   New Search   Update Search
                                  State
          │            │            │
          └────────────┼────────────┘
                       ▼
                 Updated Path

The main goal is to determine whether D Lite's incremental replanning provides useful performance advantages over Dijkstra and A** when only a small part of the costmap changes.

SmacPlanner2D remains the current global planner. The purpose of this comparison is not to replace it directly, but to evaluate whether D* Lite is suitable for future integration into the Eurobot navigation system.

---

Test Scenarios

The planners should be tested under the same start position, goal position, costmap, and obstacle configuration.

Static Environment

Compare the initial planning performance without costmap changes.

Start ─────────────────────→ Goal

New Obstacle

Add an obstacle that blocks the current path.

Start ─────────X────────────→ Goal
              New
            Obstacle

Compare how quickly each planner generates a new valid path.

Obstacle Removed

Remove an existing obstacle from the costmap and evaluate how efficiently each planner responds to the cost decrease.

Dynamic Obstacle

Simulate a moving rival robot that continuously changes the global costmap.

t0

Start ─────────────────────→ Goal
             X

t1

Start ─────────────────────→ Goal
                    X

This scenario can be used to evaluate repeated replanning performance in an environment closer to an actual Eurobot match.

---

Tasks

• Study D* Lite and the Nav2 planner plugin interface
• Implement D* Lite as a Nav2 planner plugin
• Support costmap changes and incremental replanning
• Configure Dijkstra and A* for comparison
• Test static and dynamic obstacle scenarios
• Compare Dijkstra, A*, and D* Lite performance
• Test locally and on the actual machines
• Evaluate the results and decide the next step