Hi everyone,
I wanted to share a motion-planning project that I’ve been working on independently for about two years (although I actually had the original idea when I was a student!) and hopefully get some feedback, criticism, or ideas for possible extensions.
The project is called FMTX: Lazy Wavefront Search for Dynamic Replanning. Alongside the planner itself, I ended up building a common C++17/ROS 2 framework for benchmarking dynamic replanning algorithms under the same environments and measurements.
One thing to keep in mind is that in the current version I mainly use ROS 2 for RViz visualization. I removed Gazebo from the actual benchmarking loop because I did not want simulator latency and timing effects to interfere with the planner measurements and reproducibility. For academic benchmarking I wanted the environment changes to be controlled as directly as possible.
The original motivation came from something that bothered me when thinking about extending Fast Marching Tree (FMT*) to dynamic environments.
FMT* is built around the V_unvisited, V_open, and V_closed sets, and much of its original reasoning and analysis assumes that structure.
One possible way of making FMT* dynamic is to reconstruct those sets after the environment changes: put some affected nodes back into V_unvisited, seed some nodes into V_open, and continue the expansion.
For obstacle additions, this can work fairly naturally because the invalidated part of the existing tree can be identified.
What I found much less straightforward was obstacle removal. When an obstacle disappears, a node that has already been visited may suddenly have access to a lower-cost path. But then the question becomes: which already-visited nodes should become “unvisited” again?
I could not find a clean way of doing that without introducing another heuristic or repair mechanism.
So instead, I started questioning whether the V_unvisited condition itself was really fundamental to the FMT* expansion, or whether the expansion could be expressed directly using a simple cost-improvement criterion.
That sounds like a small change, but it changes the behavior of the algorithm and introduces additional consistency issues that need to be handled carefully, hence the paper 
The work eventually resulted in two related planners:
- D-FMT*, a fixed-graph version mainly intended to isolate the dynamic repair mechanism. I also found this version very useful for transferring some of the reasoning and theoretical analysis from the original FMT* setting.
- FMTX, which adds online sampling and graph densification for anytime replanning.
The framework currently includes comparisons with:
- D* Lite on PRM*
- RRTX
- LLPT*
The experiments cover geometric and kinodynamic planning, changing obstacles, partial observability, and repeatable benchmark configurations with RViz visualization.
I also tried to structure the code as a framework where additional replanning algorithms can be added and evaluated under the same scenarios. If there is a good replanner out there that I’ve missed, please let me know. I’d be very interested in adding more relevant comparisons.
When I get the chance, I’d also like to take one branch of the framework closer to practical robotics, for example by adding state estimation and noisy measurements around the localization, integrating higher-DOF dynamics and richer simulation. For now though, I think it works quite well as an academic benchmarking framework where the replanning algorithms can be isolated and compared more cleanly.
I also don’t know if something like FMTX would actually be useful for Nav2 or not. For the usual 2D mobile robot problems people solve every day, maybe adding this extra replanning complexity is not really the way to go. But if anyone here works a lot with Nav2, I’d be curious to hear what you think.
This has been a solo effort, so I’d really appreciate any feedback on the work. Technical criticism, related papers I may have missed, other replanners worth adding, comments on the benchmark setup, implementation suggestions, possible extensions, or really anything that stands out to you.
Paper / arXiv v2:
C++ / ROS 2 implementation and demos: