MPPI PathAlignCritic: Add a minimum dist for occupancy check

[adivardi]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	-
Primary OS tested on	Ubuntu
Robotic platform tested on	Simulation & real robot
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

The occupancy check is used to disable the critic if a certain percentage of the coming segment of the path is in collision.
Previously, the segment length was defined by furthest_reached_path_point.

This PR adds a minimum distance for the occupancy check in the PathAlignCritic, so if furthest_reached_path_point is too short, the occupancy check distance is extended up to the min dist.
This solves a behavior of MPPI where in presence of an obstacle straight ahead, the robot slows down instead of overtaking the obstacle. This is because when slowing down, furthest_reached_path_point gets shorter, so the PathAlignCritic is never disabled. With this change, even when the robot slows down, the critic is disabled, freeing the controller to plan around the obstacle.

The main downside is, if the distance is too big, the critic may be disabled too early, which may cause deviation from path (if for example, the reference point for PathFollowCritic is beyond a curve on the path).

Description of documentation updates required from your changes

• Added a new param

Description of how this change was tested

Tested in both simulation and our real robot.
The effect is impressive. Without the change, our robot cannot overtake an obstacle wider than ~1m. With the change, it can overtake obsatcles up to 2m wide easily.

Without the change:

pathalign_nochange_s.mp4

With the change:

pathalign_change_s.mp4

pathalign_change_2m_s.mp4

---

Future work that may be required in bullet points

I would appreciate advice on setting the default value of this param. For our robot, it is set to 5m. But our robot is big and drives fast. I figured most robots are small, so I set it to 2m.

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]

The main downside is, if the distance is too big, the critic may be disabled too early, which may cause deviation from path (if for example, the reference point for PathFollowCritic is beyond a curve on the path).

So basically: don't make it too large? Good tuning advice for the documentation / readme 😉

We have something similar for disabling other critics in the case of occupancy, no? Should we use this there as well?

Finally, I'm curious if you had to tune your path follower critic (or modify?) to use a further path point in this case to overtake so nicely. Maybe something to consider for further improvements on this behavior?

Something's weird with CI. Can you rebase/pull in main to try again? I've been seeing some outages though on CircleCI lately in the docs, so maybe they're just having trouble in general.

[SteveMacenski]

Conceptually sound, just some details :-)

[adivardi]

So basically: don't make it too large? Good tuning advice for the documentation / readme 😉

Yes, though it depends a bit on robot's speed and kinematics. The slower the robot can react, the bigger it should be.
Should I add it to README or only to the doc page?

Question: What about offset_from_furthest? Does that not matter? I don't think so, but did you consider it?

In this critic the offset_from_furthest is different because it doesn't advance the reference point but instead disabled the critic if the trajectories are not far enough. I didn't play with it much, but I think it can't replace the min distance since it is also used in other situations (slow drive, hgh bearing to path), so I don't want to increase it too much.
Does that make sense?

Question: Do we need to retune the value for max_path_occupancy_ratio to be larger now since we're checking a much smaller portion of the path?

I also thought so, but it is working quite well for me with the current value of 0.05 (with occupancy check distance of 5m), so I left it as is.

We have something similar for disabling other critics in the case of occupancy, no? Should we use this there as well?

• PathFollowCritic skips obstacles but isn't disabled. I think that makes sense, since if we disable that, the robot is kinda lost without a direction to go to.
• PathAngleCritic does nothing, but I will open a PR soon to make it behave as PathFollowCritic, so they drive the robot to a similar point.

Finally, I'm curious if you had to tune your path follower critic (or modify?) to use a further path point in this case to overtake so nicely. Maybe something to consider for further improvements on this behavior?

It works surprisingly well for me. Once this critic is disabled, PathFollow takes over and drives really well.
The main issues are:

• if furthest_reached_path_point jumps beyond a u-turn: This is improved a lot by MPPI: add arc-length path progress to fix path skipping on curved paths #6055
• For a really big obstacle (in my test it was a whole length of a car), the robot ends up perpendicular to the path and the horizon is not long enough to "overtake" the obstacle => no trajectory results in any advancement on the path, so it gets kinda lost. This is something I want to work on more, and happy to received inputs (but maybe in such case, the global path should be replanned)

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
...nav2_mppi_controller/critics/path_align_critic.hpp	100.00% <ø> (ø)
..._mppi_controller/src/critics/path_align_critic.cpp	94.20% <100.00%> (+0.35%)	⬆️

... and 65 files with indirect coverage changes

🚀 New features to boost your workflow:

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

[SteveMacenski]

Should I add it to README or only to the doc page?

Both :-)

PathFollowCritic skips obstacles but isn't disabled. I think that makes sense, since if we disable that, the robot is kinda lost without a direction to go to.

I guess I'm suggesting to take a look at that with your fresh eyes from this work and see if there's improvements we can/should make there. For example, maybe its worth setting some offset after this distance rather than using the first free-point in order to lob the follow-path goal further out. I think that may help by setting the optimization goal further away to give more freedom in the immediate time horizon to avoid the obstacle and not try to "hook" back into it immediately after the sensor data reported blockage (since sensor data only sees the convex hull visible of the obstacle).

For a really big obstacle (in my test it was a whole length of a car), the robot ends up perpendicular to the path and the horizon is not long enough to "overtake" the obstacle => no trajectory results in any advancement on the path, so it gets kinda lost. This is something I want to work on more, and happy to received inputs (but maybe in such case, the global path should be replanned)

This is actually something I'm going to start thinking about more at the start of next week, so happy to :-) My thoughts above on the PathFollow critic are in that direction. Got a video of what you mean by this?

Have you tried a longer horizon? Even if perhaps you can't do it on your vehicle's computer, it would be good to know if making that longer is even the full solution or there's more to be done in the critics

I'm also trying to replicate your results by putting a box in front of a robot in simulation and I'm not seeing the same behavior as you where it goes around with the configuration below. Any suggestion? Or am I starting too 'big' with a pallet sized obstacle in the path's way 😉

      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        max_path_occupancy_ratio: 0.05
        min_distance_occupancy_check: 0.5
        trajectory_point_step: 4
        threshold_to_consider: 0.5
        offset_from_furthest: 20
        use_path_orientations: false

[adivardi]

I'm also trying to replicate your results by putting a box in front of a robot in simulation and I'm not seeing the same behavior as you where it goes around with the configuration below. Any suggestion? Or am I starting too 'big' with a pallet sized obstacle in the path's way

What's the robot speed and horizon?
If the distance param = horizon, then the robot should not slow down before the critic is disabled.

For example, maybe its worth setting some offset after this distance rather than using the first free-point in order to lob the follow-path goal further out

Yes, I think that makes sense. Often the reference pt ends up inside the obstacles since only the "shell" can be seen. It would be interesting to look at it, though I can't get to it in the very near future. Would it be helpful to open an issue/feature request?

This is actually something I'm going to start thinking about more at the start of next week, so happy to :-) My thoughts above on the PathFollow critic are in that direction. Got a video of what you mean by this?

Yes, here:
https://github.com/user-attachments/assets/93717750-07c5-477e-8c5c-8ff439c388f0

I haven't tried tuning more for this case. It's something I would like to do when I get more time.
I think increasing the horizon could help, though when the robot slows down so does the horizon, so it's limited. But it needs more thought for sure

[SteveMacenski]

What's the robot speed and horizon?

More or less the default Nav2 parameters, though I did try with model_dt of 0.1 so that the 56 timesteps would be longer. I'm wondering if maybe its tuning too - how far are you from the default MPPI params for these demos? Maybe it would be useful to share your complete config.

I had the parameter set to 5 which is much higher than the slow down distance. Didn't do anything.

Yes, I think that makes sense. Often the reference pt ends up inside the obstacles since only the "shell" can be seen.

Exactly :-) I quickly prototyped by increasing it by the same size as the collision offset (i.e. keeping a counter in the while loop, then add the counter's size to the offset_idx to essentially "double" the visible size).

I didn't see really any impacts, but I also can't reproduce your results yet, so maybe worth a try from your side and/or revisiting once your results can be reproduced

---

On my testing, I put together an incredibly crude demonstration that seemed to work somewhat well. I created a new obstacle bypass critic that essentially (a) identified if an obstacle was blocking the path, if so (b) identified the lateral direction that was shortest to be cleared as the direction to turn (i.e. should we turn left or right), (c) scored a penalty analog to the path angle critic towards this laterally offset point to incentivize turning towards the easiest direction to get around & an analog path follow critic to incentivize drivings towards a laterally offset point the furthest_path_point_reached + offset away (basically just Path Follow with the lateral offset).

It was pretty unstable in terms of the trajectory shape and behavior (like I said: crude), but it actually worked. I only went this direction though because I couldn't reproduce even close to what you showed in your the robot ends up perpendicular to the path video. For a realistic version of this, we'd want to disable the path follow when this obstacle bypass critic is in use. Perhaps have the "obstacle bypass" as a feature within the path follow / align critics, perhaps. Or optimally do something less heavy handed without this hand-created behavior -- if I can get something like what you show working, work.

With respect to that video, I think what's happening here is the path follow / align / angle critics (angle probably?) is making it want to drive back to the relative path heading since the angle grew too high. What happens if you disable the path angle critic (not permanently, you want that, but to see if that's the cause). If its not the cause, disable critics until you can see what is.

Also with respect to your video, if I can get something like that to work on my end without this complexity of new critics, I can almost certainly make quick work of fixing that behavior you see I believe :-)

[adivardi]

I'm also trying to replicate your results by putting a box in front of a robot in simulation and I'm not seeing the same behavior as you where it goes around with the configuration below. Any suggestion? Or am I starting too 'big' with a pallet sized obstacle in the path's way

I have tried it a bit more with the turtlebot configuration. I also can't get it as nicely as mine, though the Gazebo box is really big compared to the turtlebot. With the ball & the capsule it works well.
The biggest improvements I got for the turtle bot was when I increased the horizon, the prune distance and the local costmap size, perception & inflation radius. I suspect that for the out-of-the box turtlebot, the big obstacle is seen too late when the min distance has no effect, and by that point all the trajectories lead to collision.
I am not sure if there are other factors related to our robot being steered.

So these changes improved the behavior:

      prune_distance: 7.0
      time_steps: 100
local_costmap:
      width: 10
      height: 10
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 0.7 
        inflation_radius: 5.0
      voxel_layer:
          raytrace_max_range: 10.0
          raytrace_min_range: 0.0
          obstacle_max_range: 9.5
          obstacle_min_range: 0.0

and my configuration is:

    PathHandler:
      plugin: "nav2_controller::FeasiblePathHandler"
      prune_distance: 10.0
      enforce_path_inversion: false
      enforce_path_rotation: false
      inversion_xy_tolerance: 0.2
      inversion_yaw_tolerance: 0.4
      minimum_rotation_angle: 0.785
      reject_unit_path: false
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 100
      model_dt: 0.05
      batch_size: 2000
      open_loop: true
      retry_attempt_limit: 3
      ax_max: 1.7
      ax_min: -1.7
      ay_max: 0.0
      ay_min: 0.0
      az_max: 0.6061
      vx_max: 0.8
      vx_min: -0.4
      vy_max: 0.0
      wz_max: 0.8534
      vx_std: 0.5
      vy_std: 0.2
      wz_std: 0.4
      iteration_count: 1
      temperature: 0.3
      gamma: 0.015
      motion_model: "Ackermann"
      regenerate_noises: true
      Visualization:
        publish_critics_stats: false
        publish_optimal_trajectory_msg: false
        publish_transformed_path: true
        publish_trajectories_with_total_cost: false
        publish_trajectories_with_individual_cost: true
        publish_optimal_footprints: false
        publish_optimal_path: true
        trajectory_step: 15
        time_step: 15
        footprint_downsample_factor: 3
      TrajectoryValidator:
        plugin: "mppi::DefaultOptimalTrajectoryValidator"
        collision_lookahead_time: 2.0  # Time to look ahead to check for collisions
        consider_footprint: true
      AckermannConstraints:
        min_turning_r: 1.0 # 5-A-1 with 4 wheels steering, slightly higher than right turn (0.9856m) from calculate_model_parameters.py
      critics:
        [
          "ConstraintCritic",
          "CostCritic",
          "GoalCritic",
          "GoalAngleCritic",
          "PathAlignCritic",
          "PathFollowCritic",
          "PathAngleCritic",
          "PreferForwardCritic",
        ]
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 1.4
      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 0.5
      PreferForwardCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 0.5
      CostCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.81
        near_collision_cost: 253
        critical_cost: 300.0
        consider_footprint: true
        collision_cost: 1000000.0
        near_goal_distance: 1.0
        trajectory_point_step: 2
      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        max_path_occupancy_ratio: 0.05
        occupancy_check_min_distance: 4.5
        trajectory_point_step: 4
        threshold_to_consider: 0.5
        offset_from_furthest: 20
        use_path_orientations: true
        visualize_furthest_point: true
        visualize_occupancy_check_distance: true
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 1.4
        visualize_furthest_point: true
      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
        visualize_furthest_point: true
      VelocityDeadbandCritic:
        cost_power: 1
        cost_weight: 35.0
        deadband_velocities: [0.05, 0.0, 0.001] # Deadband Velocity x, y, wz

---

Regarding the robot ends up perpendicular to the path, I actually also can't replicate it often in simulation. Mostly it just getting stuck driving back and forth with the obstacle in front.

[adivardi]

I added a readme paragraph for the new param. Let me know if it's too long or unclear.

[SteveMacenski]

  prune_distance: 7.0

Ah got it. I will try that tomorrow, that could explain it. Do you use a ~2x horizon?

I do need to be able to test this making a change before merging. But luckily for you I'm also really interested in this same area so I can dedicate some real time to it (both to test your changes to reproduce + update Nav2 defaults + take it more steps forward if necessary).

If possible, can you send me your parameters for the local costmap as well (I guess just the footprint + inflation params) so I can try to replicate? if I can start with what works, I can make changes to narrow into how to get this to be good for the TB3/4 + my testing rigs. If you can do that in the next ~half day, I can spend my 9-5 tomorrow on this to evaluate, test, and refine if needed the TB3/4 + my testing parameters (and see if my work I previously mentioned is even needed at all).

Regarding the robot ends up perpendicular to the path, I actually also can't replicate it often in simulation. Mostly it just getting stuck driving back and forth with the obstacle in front.

I think I know why this is (or at least 2 strong hypotheses), but I'd need to sit in front of it with some debugging prints to know which or any is the cause + solution.

---

The docs are good. I forget if you had a configuration guide entry for MPPI yet for this PR. If not, add that there too :-)

[adivardi]

Sure:

local_costmap:
      width: 30
      height: 30 
      resolution: 0.1
      footprint: "[ [2.230, 0.65], [-1.4312, 0.65], [-1.4312, -0.65], [2.230, -0.65] ]"
      footprint_padding: 0.1

      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 0.7    # r=5.0 -> f=1.2. r=15.0 -> f=0.375
        inflation_radius: 5.0

btw, I also noticed the turtlebot only goes up to 0.5m/s. That may also make the horizon quite shorter, so the trajectories spread less to the sides

[SteveMacenski]

Can you open that docs PR?

Compiling now trying to test this. I didn't quite get to it until later this afternoon as another MPPI issue took me longer than expected

[SteveMacenski]

pallet_demo.mp4

So I was able to reproduce but had some issues/concerns:

• In this video, I was able to make it work when the costmap had not yet seen the obstacle. When it sees it, I see the behavior work very well where it navigates around the obstacle. I had to increase the costmap size and adjust the inflation parameters to there was a potential field to follow, but I think those are reasonable adjustments based on enabling this and its genuine needs. Probably mentioning people a sufficient long time_step*model_dt horizon for it as well. Documentation in the config guide/README in this would be key for others to reproduce / get this behavior themselves
• However, I did notice that if I tried again or I backed the robot to the starting pose and tried again, it always fails when it sees the obstacle in the local costmap ahead of time. It approaches the obstacle and slows down without trying to go around it. Sometimes it starts to go around without confidence but then just kind of .... wanders away into oblivion which I think is a similar behavior to what you saw with the perpendicular video

So the 2 issues there are:

• Why isn't it seeing the obstacles in the way and trying to go around before slowing down? I imagine this is at least a tuning thing and maybe some of these parameters should be grounded so tuning is easier. Hence why I'm asking you because I assume you saw this yourself when tuning to know the trade offs that I should make and should be documented :-) I'm thinking that the CostCritic is causing a slow down which affects the furthest point reached for checking for occupancy and so forth.
• Why is it... wondering away perpendicularly when it is unable to start driving away before slowing down using this bypass? Perhaps this PR needs some additional conditions to handle such situations (unless this happens already without this PR -- in which case maybe not your problem to solve but something we should look into). Something seems not quite right.

I'm curious if you've seen anything similar to the latter, since I think that's a pretty key thing to either fix or pre-tune for users if its a tuning thing + provide advice.

I'm using timesteps = 56/model_dt = 0.1, min_distance_occupancy_check to 2.0, increased the prune distance to 7.0, local costmap size to 10, and the inflation scale factor to 0.7 & inflation radius to 0.5 as you have them. I see you have occupancy_check_min_distance: 4.5 but that parameter was renamed so its not doign anything and the default is 2.0

[adivardi]

Can you open that docs PR?

ros-navigation/docs.nav2.org#912

Documentation in the config guide/README in this would be key for others to reproduce / get this behavior themselves

I added a sentence about it, please let me know if that's clear.

I did notice that if I tried again or I backed the robot to the starting pose and tried again,

hmm weird. I don't see how that should be different. I have not seen that.
To be honest, I didn't need to re-tune anything for this change to work. It worked really well as soon as I did it with my previous tuning.

Are you still using the raytrace_max_range: 3.0? Maybe it populates the map too late? but I don't see why it would matter much.
My local costmap populates its whole size of 30m, so in many of my tests (but not all) the obstacle is seen from the initial position.

Why isn't it seeing the obstacles in the way and trying to go around before slowing down?

So I think for big obstacles, if the trajectories are not spread far enough, they all get invalidated by the CostCritic, which is why it doesn't even try to go around. It needs a valid trajectory to push the distribution to one side. And it needs to happen early enough for the overtake to still be possible.
Before making the change in this PR, I was planning to try increasing wz_std and maybe wz_max (though that's not really valid in practice) to force the trajectories to be more spread. Once I found this change, I didn't need it anymore, but maybe I got lucky and my robot's kinematics just works.
But I am not so sure what would be a solution for this, apart for increasing the perception & horizon, which is often not feasible.
You mentioned a by-pass critic, maybe something like this could guide the trajectories out of the local minima of slowing down straight?

wondering away perpendicularly when it is unable to start driving away before slowing down using this bypass?

looking more closely at my video, just before it turns, there are a few trajectories valid to the right. I think they are valid because car is not exactly perpendicular, and probably also because of the orientation. Somewhere around this:

One frame later, the whole distribution changes

The thing is, if I try it in simulation with a perpendicular, it gets stuck. So maybe the video is more of a fluke? Though you are saying you also have that?

I am using min_distance_occupancy_check: 4.5. It is roughly equal to my horizon length at full speed of 0.8m/s. It works well down to 3.0m, though then it slows down before overtaking. But my robot's steering is really slow and has a huge delay, so it really takes a big distance to steer around the obstacle.

[SteveMacenski]

Sorry for the delay, this is something I needed to test and play around with and finding that time took longer than expected. I'm on this today though.

The slowing down / stopping I'm looking into after simplifying my setup (I think I was overcomplicating some things and got lost in those details). Maybe this is just a configuration / setup / tuning item that I need to understand better and then write some guide docs for.

Are you still using the raytrace_max_range: 3.0? Maybe it populates the map too late? but I don't see why it would matter much.

Interestingly I would assume that would cause my "slow down and stop" issue since it would see it ahead of time like I mentioned, but I will try that & also keep in mind as I'm trying to understand that behavior better.

I am using min_distance_occupancy_check: 4.5. It is roughly equal to my horizon length at full speed of 0.8m/s. It works well down to 3.0m, though then it slows down before overtaking. But my robot's steering is really slow and has a huge delay, so it really takes a big distance to steer around the obstacle.

Your horizon is 4m from the configs given, so actually setting this above that number makes alot of sense. The bypass starts before the collision critics even have a chance to score poorly.

Question: When its down to ~3.0, does that work well always? sometimes? just as well as 4.5?

I'm still not able to get this to reliably work on my end, except when the obstacle range is low so that the obstacle pops up on the local costmap 'suddenly' over an existing full-speed trajectory. If the robot sees the obstacle while approaching it, I can never reproduce your results :(

---

The wondering away perpendicularly issue we both see though is something worth probably addressing in some way. That's definitely not good and we probably want to find what condition that happens in to prevent it

What other, if any, issues are you still seeing?

[SteveMacenski]

P.S. Here's my ObstacleBypassCritic that when married with your changes I get pretty good behavior. There's a couple of discontinuities I'm trying to assess (1x I'm pretty sure is right, 1x I'm trying to characterize to understand) but I think this is worth you trying and seeing if it helps you

https://github.com/ros-navigation/navigation2/tree/obstacle_bypass_critic

[SteveMacenski]

@adivardi here's my MVP prototype that I think works quite well. Can you give it a whirl? #6144