Basic Info

Description of contribution in a few bullet points

This pull request is a backport of #5636 to the humble_main branch.
This issue leads to nan costs when wz is zero with ackermann enabled, and results in the robot moving only in circles.

Description of how this change was tested

I used a test nav2 setup with a dummy local costmap and global costmap with all values equal to 0.
Motion model is set to Ackermann.

Before the fix to any goal the optimal trajectory correspond to high vx and wz and the robot going only in circles, failing after a while. The constraint critic compute NaN costs, verified by adding prints in the code.

After the fix, the previous behaviour does not happen anymore and the optimal trajectory is correct.

Future work that may be required in bullet points

• [mppi] fix division by zero, clean raw pointers code #5636 also adds a minor change on applycontraints() to avoid using raw pointers and use instead Eigen operations. This change was not added to the PR.

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-*.