Required Info:

• Operating System:
  • Ubuntu 24.04
• Computer:
  • i7-13850HX Laptop
• ROS2 Version:
  • Jazzy binaries
• Version or commit hash:
  • ros-jazzy-navigation2 1.3.8-1noble.20250915.220228
• DDS implementation:
  • Default (rmw_fastrtps_cpp)
• Nav2 Package:
  • nav2_mppi_controller

I’m wondering whether an alternative implementation of FurthestReachedPoint() would be interesting or worth considering. If it’s useful, I’d be happy to contribute a PR.

I’d really appreciate your input on this. I may be overlooking something in my setup, so I want to make sure I’m not misattributing the issue. Any pointers on where to look would be greatly appreciated.

Details

My application requires coverage of an area, tight path tracking, and dynamic obstacle avoidance.

Steps to reproduce issue

Send a path to MPPI with a "tight" U-Turn (e.g. 0.6 m)

Expected behavior

MPPI's optimal trajectory closely follows the path sent.

Actual behavior

When approaching the U-Turn, MPPI's optimal trajectory leans towards the end of the "pruned path" (videos below).

Additional information

My understanding is that this is caused by the interaction of:

• findPathFurthestReachedPoint() (in include/nav2_mppi_controller/tools/utils.hpp)

With my nav config:

• Long "prediction horizon" ( time_steps * model_dt = 80 * 0.1 = 8 secs)
• Long prune_distance (5 meters)

Given the way FurthestReachedPoint() works, the end point of some of the "sampled control sequences" will effectively be on the "way back" of the U-Turn, so the selected point will already be on the way back, instead of continuing following the path.

Reducing prune_distance (and time_steps) would help; however, that would affect the obstacle avoidance capabilities (not long enough horizon time to result in control_sequences that make progress while avoiding obstacles).
Note: In my system, once the coverage path is calculated, it is sent to the controller (MPPI), which is responsible for dynamic obstacle avoidance (also static obstacles that were not in the costmap when the coverage path was computed).

Controller config (main params):

    # MPPI plugin configuration
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 80
      model_dt: 0.1
      batch_size: 1200
      vx_std: 0.25
      vy_std: 0.0
      wz_std: 0.35
      vx_max: 0.5
      vx_min: -0.4
      vy_max: 0.5
      wz_max: 0.50 
      ax_max: 0.625     
      ax_min: -0.40   
      az_max: 1.0
      iteration_count: 1
      prune_distance: 5.0
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      ...
      critics: ["ConstraintCritic", "GoalAngleCritic", "GoalCritic", "ObstaclesCritic", "PathAlignCritic", "PathAngleCritic", "PathFollowCritic", "PreferForwardCritic", "VelocityDeadbandCritic"]

      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        threshold_to_consider: 0.5
        offset_from_furthest: 20
        max_path_occupancy_ratio: 0.02
        use_path_orientations: false
        trajectory_point_step: 2
      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0 
        offset_from_furthest: 4
        threshold_to_consider: 0.5
        max_angle_to_furthest: 1.0
        mode: 2
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0 
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      ...
      [other critics]
      ...

Videos: