Basic Info

Description of contribution in a few bullet points

• Fix a bug where the motion_model is not reset when the optimizer is reset. This causes parameters changes to not be updated in the motion model, even though they are updated in the optimizer.
• I noticed the MPPI behavior getting worse when dynamically changing control constraints (in my case az_max)

Description of documentation updates required from your changes

None

Description of how this change was tested

The old fashion way - using prints:
Added this print after the line changed in this PR.

  std::cout << "Optimizer::reset: " << "\n settings_.constraints.az_max: "
            << settings_.constraints.az_max
            << "\n settings_.base_constraints.az_max: " << settings_.base_constraints.az_max
            << "\n motion_model_.az_max " << motion_model_->control_constraints_.az_max << std::endl;

The call ros2 param set /controller_server FollowPath.az_max 1.0
Without the change, the last line value is not updated to the new value requested.
You can also add prints in places such as optimizer and motion_model

I am happy to hear suggestions on how to test this more properly.

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