[nav2_mppi] Add optional CUDA-accelerated backend for Jetson/Edge devices

[functionhx-art]

Feature request

Feature description

Currently, nav2_mppi heavily relies on CPU for trajectory sampling and evaluation. While efficient on desktop-class CPUs, this becomes a significant bottleneck on edge computing platforms like NVIDIA Jetson Orin. In high-density obstacle environments or with a high number of sampled trajectories ($K &gt; 500$), the CPU utilization spikes significantly, often limiting the control frequency to sub-optimal levels (e.g., < 20Hz), which restricts the robot's high-speed maneuvering capabilities.

I propose adding an optional CUDA-accelerated backend to offload these computations to the GPU, significantly improving real-time performance on ARM-based SoC architectures.

Implementation considerations

I suggest implementing this as an optional plugin-based optimization. Key technical points include:

• Parallel Computing: Use cuRAND for parallel noise generation and custom CUDA kernels for trajectory rollouts and scores.
• Memory Optimization: Leverage Unified Memory (managed memory) to minimize host-to-device data transfer overhead, specifically targeting the shared-memory architecture of Jetson devices.
• Build System: The CUDA backend will be gated behind a CMake flag (e.g., -DENABLE_CUDA=ON), ensuring 100% backward compatibility and no additional dependencies for non-NVIDIA users.
• Pros: - Much higher sampling density (e.g., $K &gt; 2000$).
  • Significantly lower latency and higher control frequency.
  • Reduced CPU overhead for other critical tasks like perception or localization.
• Cons: - Additional build-time dependency on the CUDA Toolkit for developers who explicitly enable this feature.

Recent research, such as "MPPI-Generic: A CUDA Library for Stochastic Trajectory Optimization" (arXiv:2409.07563), has already demonstrated the feasibility and performance gains of such an approach. I am a Robotics Algorithm Engineer and I am willing to contribute the implementation and a PR for this feature.

[SteveMacenski]

Currently, nav2_mppi heavily relies on CPU for trajectory sampling and evaluation. While efficient on desktop-class CPUs, this becomes a significant bottleneck on edge computing platforms like NVIDIA Jetson Orin. In high-density obstacle environments or with a high number of sampled trajectories ( K > 500 ), the CPU utilization spikes significantly, often limiting the control frequency to sub-optimal levels (e.g., < 20Hz), which restricts the robot's high-speed maneuvering capabilities.

Where is this coming from? We have samples at 30hz with 2000 samples (or 50hz with 1000 samples) on Nvidia Jetson platforms. Typically on my mobile AMD CPU, I can optimize 2000 samples in 10 milliseconds. I don't argue that perhaps CUDA could help use the Jetson platforms to the fullest, I would like to understand if you've tested this package before and found it deficient in some way on a Jetson.

But assuming so, I think this would be a very good contribution -- what strategy would you have to keep the current implementation in-tact but also support GPU optimization in key portions? Perhaps also some of the critics (namely Cost / Align Path) are also quite heavy that could potentially benefit from a GPU.

---

From their website/paper:

The next implementation we will compare against is ROS2’s MPPI [5]. As of ROS Iron, there is a CPU implementation of MPPI in the ROS navigation stack This CPU implementation is written in C++ and looks to make heavy use of AVX instructions or vectorized instructions to improve performance. There is a small selection of dynamics models (Differential Drive, Ackermann, and Omni-directional) and cost functions that are focused around wheeled robots navigating through obstacle-laden environments. This implementation will only become more widespread as ROS2 adoption continues to pick over the coming years, making it an essential benchmark. Unfortunately, it does have some drawbacks as it is not possible to add new dynamics or cost functions without rewriting the base code itself, has no implementation of Tube-MPPI or RMPPI, and is only available in ROS2 Iron or newer. This means that it might not be usable on existing hardware platforms that are unable to upgrade their systems.

I don't think this is a generous or accurate description... I wish I was consulted on this... the citation is especially egregious as this has nothing to do with Open Robotics.

not possible to add new dynamics

Not true

... or cost functions

Especially not true, these are plugins that anyone can make without even forking the repo

is only available in ROS2 Iron or newer.

Also not true, its in Humble, albeit a different tensor library backbone, but totally serviceable.

All the written drawbacks are verifiably false :(

[tonynajjar]

All the written drawbacks are verifiably false :(

I had nothing to do with writing MPPI and even I'm offended by the BS in the paper

[Yuchen-byte]

Hi Steve，

I sincerely apologize for this. Before opening this issue, I was indeed misled by the performance claims and descriptions in a recent paper (arXiv:2409.07563). I took their conclusions at face value without performing thorough local benchmarking first, which was my mistake.

I have just conducted real-world tests on my Jetson Orin. The results confirm exactly what you mentioned: thanks to the existing vectorized optimizations, the CPU performance is excellent, maintaining well over 30Hz even with 1000-2000 samples. In most standard use cases, adding a CUDA backend does not seem necessary or urgent at this stage.

I am sorry for the oversight and for relying on inaccurate information from that publication. To avoid further misleading the community or wasting your time, I would like to withdraw this proposal.

I will close this issue now.

[tonynajjar]

Before opening this issue

I'm confused, @Yuchen-byte and @functionhx-art are the same person?

[Yuchen-byte]

Yes, sorry for the confusion. @functionhx-art and @Yuchen-byte are both me

[mini-1235]

Thanks for the clarification, I will go ahead and close this