Fix optimal plan frame id

[eegeugur]

The cmd is wrt to the base frame while the optimal_trajectory is wrt to global frame. Fix the frame id for the trajectory_msg.

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	/
Primary OS tested on	Ubuntu
Robotic platform tested on	Custom simulation
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
• Should this be backported to current distributions? If so, tag with backport-*.

[SteveMacenski]

Please explain. Isn't the cmd_vel simply a single timestep of the broader trajectory? That would put these into the same frame of reference.

[eegeugur]

Yes, cmd_vel is a body-relative instantaneous velocity command, so it's expressed in the base frame link of the robot. The optimal_trajectory is generated by integrating these instantaneous velocities starting from the robot’s current pose in the costmap's global frame. So the steps of the trajectory are computed at the costmap's global frame.

For example, assume a robot is at (10, 0) wrt to odom and facing east while cmd_vel only has linear.x as non-zero with 1.0. If we simulate 3 secs constant cmd_vel, the poses of the trajectory should be [(11, 0), (12, 0), (13, 0)] wrt odom while [(1, 0), (2, 0), (3, 0)] wrt to base_link.

[SteveMacenski]

LGTM after going through to track down frames. Thanks!

[SteveMacenski]

One linting error left:

whitespace/blank_line [2] (/opt/overlay_ws/src/navigation2/nav2_mppi_controller/src/controller.cpp:119)

[codecov]

Codecov Report

❌ Patch coverage is 0% with 4 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_mppi_controller/src/controller.cpp	0.00%	4 Missing ⚠️

Files with missing lines	Coverage Δ
nav2_mppi_controller/src/controller.cpp	88.46% <0.00%> (-5.42%)	⬇️

... and 3 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.