This pull request is in conflict. Could you fix it @Ayush1285?

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

Let me know here when you want me to take a look again! I'm quite excited for this work - even if for no reason than to move to Eigen + 10% performance boost, since Eigen's release and support is much more known than xtensor's

Let me know here when you want me to take a look again! I'm quite excited for this work - even if for no reason than to move to Eigen + 10% performance boost, since Eigen's release and support is much more known than xtensor's

Sure, I'm trying a few optimizations and will push changes once I'm done.

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

Let me know here when you want me to take a look again! I'm quite excited for this work - even if for no reason than to move to Eigen + 10% performance boost, since Eigen's release and support is much more known than xtensor's

I've completed the migration to Eigen. But we need to make sure that functionality-wise everything is correct or not. I'll run tests and ensure that all of them are passing. Meanwhile, you can take a look at the latest changes.

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

This pull request is in conflict. Could you fix it @Ayush1285?

From the ARM testing folks at Kiwibot:

We tested the fix you mentioned on simulation and on a robot today and it seems to be working fine (including the PathAlignCritic), except for some oscillations that we think might require some tuning to fix. I will be working on that. Keep you posted.

It looks like that fix did the job. I'm helping them with some more tests and metrics gathering but its looking good :-)

Thanks for this PR! We (Kiwibot) tested it on one of our robots running ROS 2 Iron on Ubuntu 22.04 (Jetson AGX, ARM64). The compilation issues we previously encountered with xtensor are now resolved.

Using the default bringup parameters, we observed the following performance metrics:

17.1 ms average controller execution time
55–65% CPU usage on a single core during navigation
25% CPU usage on a single core when idle
0.5% overall memory usage
As @SteveMacenski mentioned, the controller’s performance has been good, and the issue with the PathAlignCritic is now fixed.

I've heard enough good news at this point (only 2 weeks late from EOY)!

@Ayush1285 please rebase / pull in main. If CI looks good, I'll merge! Thanks so much for your tireless work and effort here, this is a big improvement in performance and long term maintainability. Even better, this enables this to work well and effectively on Jetson board which were not previously possible due to their relatively weak CPUs. The metrics that @jdgalviss reports from Kiwibot are a major improvement and make it possible to use MPPI even on those boards for production without using insane amount of resources or only able to run at sub-20hz.

I can't be happier with how this came out and I look forward to any other contributions you'd be interested in helping out with in Nav2! 😄

I've heard enough good news at this point (only 2 weeks late from EOY)!

@Ayush1285 please rebase / pull in main. If CI looks good, I'll merge! Thanks so much for your tireless work and effort here, this is a big improvement in performance and long term maintainability. Even better, this enables this to work well and effectively on Jetson board which were not previously possible due to their relatively weak CPUs. The metrics that @jdgalviss reports from Kiwibot are a major improvement and make it possible to use MPPI even on those boards for production without using insane amount of resources or only able to run at sub-20hz.

I can't be happier with how this came out and I look forward to any other contributions you'd be interested in helping out with in Nav2! 😄

I've merged it with the latest main. There are two unrelated test failures. One in collision monitor test:
"[ERROR] [1736830701.557335092] [collision_monitor]: Error while getting parameters: parameter 'observation_sources' is not initialized. Another one in the docking server test:
[opennav_docking-1] [INFO] [1736830869.904053683] [docking_server]: Creating bond (docking_server) to lifecycle manager. [INFO] [lifecycle_manager-2]: sending signal 'SIGINT' to process[lifecycle_manager-2]. It seems flaky failure to me.

I can't be happier with how this came out and I look forward to any other contributions you'd be interested in helping out with in Nav2! 😄

Sure, I'm open to more contributions.

This pull request is in conflict. Could you fix it @Ayush1285?

Ugh, I'm sorry to do this to you, but it looks like there are merge conflicts from #4812 which changed the use of path to the actual goal pose to fix a bug in MPPI using the transformed path's end goal as the global goal and shortcutting some critics.

Otherwise, I tested and this is good to go 😄

Sure, I'm open to more contributions.

This was a pretty big undertaking - what's your feeling about working on MPPI some more or switching gears into another part of the codebase / new algorithms?

On the MPPI front, there are a few open tickets that would be really nice to hvae someone look at:

• Issue with Bidirectional Acceleration and Deceleration Settings in MPPI Controller #4601 - MPPI acceleration and deceleration profiles being priorly applied when moving in reverse (would be a natural small project from this PR)
• Full footprint collsion distance in MPPI obstacle critic and use of collision_margin_distance. #4361 - there's some discussion that the obstacle / cost critics can be simplified or issues removed. I haven't had the cycles to look at it again to solve it
• Potential MPPI Controller Improvements #3351 - This is a general laundry list I've been keeping of potential MPPI updates to improve run-time performance or trajectory generation quality

On other fronts:

• I have some thoughts on some ideas for other controllers based on sampling of spirals and other feasible primitives that I haven't really had the time to flush out but could be really nice potential algorithms for users to have access to. For example: the graceful controller computes a smooth spiral from the robot pose to a goal pose from a set of hyper parameters. We could instead sample those hyper parameter space with the same fixed start and goal poses to find the 'best' spiral to get us from A->B minimizing some cost function of curvature, cost, length, etc. We could even have multiple time-steps of sampling as well to create more sophisticated maneuvers
• There have been regular interests in creating AI-based costmap layers for object detection and semantic segmentation, but none of these efforts have culmanated in a ready-to-merge PR with a tested capability. This would be a high value and ROSCon worthy talk subject
• [collision_monitor] Add temporal axis to min_points behavior #4364 - the collision monitor currently uses single-measurements to decide if the robot could be soon in collision, its been requested that it use a buffer of data so that a single erroneous measurement doesn't trigger the system. This is a pretty straight-forward sounding task that would probably involve some clever architectural changes and modifications if you're interested in a jigsaw puzzle

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

Ugh, I'm sorry to do this to you, but it looks like there are merge conflicts from #4812 which changed the use of path to the actual goal pose to fix a bug in MPPI using the transformed path's end goal as the global goal and shortcutting some critics.

I've fixed the merge conflicts. Let's wait for the CI to run. The optimizer_benchmark was missing a change; it was not updated with the latest evalControl() API changes.

This was a pretty big undertaking - what's your feeling about working on MPPI some more or switching gears into another part of the codebase / new algorithms?

I would like to contribute on other fronts. Playing with new control algorithms and improvising the costmap layer seems exciting ideas to me. Currently, I'm a little short on bandwidth but we can discuss/explore potential ideas for now.

@Ayush1285, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

I would like to contribute on other fronts. Playing with new control algorithms and improvising the costmap layer seems exciting ideas to me. Currently, I'm a little short on bandwidth but we can discuss/explore potential ideas for now.

Understood - want to chat a bit on the Nav2 Slack? By no means feel limited to those thoughts, the world's our oyster 😆

@Ayush1285 only small thing that would make a nice followup PR: We talked about compressing some of the rollout stuff a bit more cleanly as so (as a rough hand-written-untested template)

  float last_yaw = initial_yaw;
  for(size_t i = 0; i != n_size; i++) {
    float & curr_yaw = traj_yaws(i);
    curr_yaw = last_yaw + wz(i) * settings_.model_dt;
    last_yaw = curr_yaw;
  }

Please ping me in Slack / email and lets chat about next projects when you have some bandwidth -- I can't thank you enough for this great work. I know this took alot of time and effort and it is well worth it for the global impact this has

@Ayush1285 only small thing that would make a nice followup PR: We talked about compressing some of the rollout stuff a bit more cleanly as so (as a rough hand-written-untested template)

  float last_yaw = initial_yaw;
  for(size_t i = 0; i != n_size; i++) {
    float & curr_yaw = traj_yaws(i);
    curr_yaw = last_yaw + wz(i) * settings_.model_dt;
    last_yaw = curr_yaw;
  }

Yes, I'll fix it in a follow-up PR.

Please ping me in Slack / email and lets chat about next projects when you have some bandwidth -- I can't thank you enough for this great work. I know this took alot of time and effort and it is well worth it for the global impact this has

I'll ping you on Slack.

Will this change merge into branch humble?

No, this cannot be backported. It is a complete ABI/API breakage

Will this change be backported to Jazzy?

No, it is a complete ABI rewrite. It is only available in Main, Kilted, and newer. Note that Main compiles against Jazzy if you want to, but won’t replace the build farm binaries.