Fix nav2 MPPI Trajectories — Foxglove

Fixes the zero timestamps on nav2 MPPI trajectory markers so they transform correctly in Foxglove's 3D panel.

Two ways to use it — pick whichever you prefer:

• Extension (this repo's src/) — a packaged plugin you build and install once. Best if you want it always-on across layouts. See below.
• User Script (user-script/fix-trajectories-stamp.ts) — paste-and-go, no build or install. Best for a quick fix or if you can't install extensions. See User Script alternative.

Both apply the same fix; the only difference is packaging.

The problem

The nav2 MPPI controller publishes its candidate trajectories as a visualization_msgs/msg/MarkerArray, but every marker's header.stamp is left at {sec: 0, nsec: 0}. With a zero stamp the 3D panel can't look up a /tf transform for the marker's frame_id, so the trajectories don't render in the correct frame.

The fix

This extension registers a topic converter (docs) that reads the incoming MarkerArray and republishes it on a new topic (/trajectories/stamped) with each marker's header.stamp rewritten to the message's receive time. frame_id is left untouched, so once the stamp is valid the TF lookup resolves.

receiveTime is the correct value, not a hack: MPPI computes and publishes these trajectories "now", so their true time really is ≈ receive time — and this holds for both live connections and recorded data.

The whole converter is in src/index.ts.

Configure for your setup

Three constants at the top of src/index.ts:

Constant	Default	Change if…
INPUT_TOPIC	/trajectories	nav2 publishes on a different / namespaced topic (e.g. /controller_server/trajectories). Read the real name off your topic list.
OUTPUT_TOPIC	/trajectories/stamped	you want a different name for the fixed topic.
MARKER_ARRAY_SCHEMA	visualization_msgs/msg/MarkerArray	your data source reports a different schema name for that topic (nav2 is ROS 2, so it should be this).

Build & install

Requires Node.js 16+.

npm install
npm run local-install   # builds and installs into your local Foxglove

Then restart / reload Foxglove. To package a distributable .foxe instead (e.g. to share with teammates), run npm run package and double-click the resulting *.foxe to install it.

Use it

In the 3D panel, add the output topic /trajectories/stamped — not the original /trajectories. The trajectories will now transform into the correct frame.

The output topic only appears once a panel subscribes to it; the converter is created lazily on first subscription.

Verifying

Build and packaging are verified to succeed in this repo. The runtime behavior must be confirmed inside Foxglove: after installing, display /trajectories/stamped in the 3D panel and confirm the candidate trajectories appear and sit in the expected frame (e.g. odom / map) as the robot moves — that's the signal that the TF lookup is now succeeding.

User Script alternative

If you'd rather not build/install an extension, use the User Script in user-script/fix-trajectories-stamp.ts instead. It does the same thing with no tooling:

1. Open the User Scripts panel in Foxglove and paste the whole file in.
2. If your trajectories aren't on /trajectories, edit the inputs line.
3. In the 3D panel, add the output topic /foxglove_script/trajectories_stamped — not the original /trajectories.

Reload isn't needed; User Scripts run as soon as you save them. The trade-off vs. the extension: a User Script lives inside one Foxglove layout rather than being installed app-wide.

Project layout

src/index.ts                        the extension's topic converter (the fix)
user-script/fix-trajectories-stamp.ts  standalone User Script (same fix, no build)
package.json                        extension manifest + build scripts
tsconfig.json                       extends create-foxglove-extension's base config