[RPP] Prevent collision check premature termination

[EricoMeger]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5595
Primary OS tested on	Ubuntu 24.04
Robotic platform tested on	Physical Robot (differential-drive)
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

• Fixes a bug in RPP where the robot would "creep" forward after stopping for an obstacle if min_lookahead was set lower than min_distance_to_obstacle
• Change isCollisionImminent logic to always cover at least up to min_distance_to_obstacle, even when carrot_dist shrinks.

Description of documentation updates required from your changes

None

Description of how this change was tested

Tested for a week on a physical differential-drive robot using a 270-degree LiDAR in a real-world environment. The tests included various routes with both static and dynamic obstacles placed in the robot's path. No negative side effects or degradation in general navigation behavior were observed.

---

Future work that may be required in bullet points

I don't like to create a entire new variable just that it receives carrot_dist if min_distance_to_obstacle is unused, but I can't think of a better way to do that. I'm open to suggestions.

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]

Just a nit for wording, LGTM. Check out the CI issues, it looksl ike you have linting violations that need to be resolved before merge

@doisyg please approve as well

[EricoMeger]

Just updated the title to add the [RPP] prefix, to make the PR more self-explanatory

[mergify]

@EricoMeger, 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).

[mini-1235]

When testing #5446, I noticed that I am unable to navigate with RPP after merging this PR. From the log, it says that

[WARN 1760532493.169995588] [controller_server]: RegulatedPurePursuitController detected collision ahead! (computeAndPublishVelocity() at /root/nav2_ws/src/navigation2/nav2_controller/src/controller_server.cpp:659)

My parameters (mostly copied from docs.nav2.org):

FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.6
      lookahead_dist: 0.6
      min_lookahead_dist: 0.3
      max_lookahead_dist: 0.9
      lookahead_time: 1.5
      rotate_to_heading_angular_vel: 1.8
      use_velocity_scaled_lookahead_dist: false
      min_approach_linear_velocity: 0.05
      approach_velocity_scaling_dist: 0.6
      use_collision_detection: true
      max_allowed_time_to_collision_up_to_carrot: 1.0
      use_regulated_linear_velocity_scaling: true
      use_fixed_curvature_lookahead: true
      curvature_lookahead_dist: 0.25
      use_cost_regulated_linear_velocity_scaling: false
      cost_scaling_dist: 0.3
      cost_scaling_gain: 1.0
      regulated_linear_scaling_min_radius: 0.9
      regulated_linear_scaling_min_speed: 0.25
      use_rotate_to_heading: false
      allow_reversing: true
      rotate_to_heading_min_angle: 0.785
      max_angular_accel: 3.2
      min_distance_to_obstacle: 1.5
      stateful: true

If I change the min_distance_to_obstacle to 1.0 or 0.0, it works properly. I believe that's why CI is passing, any idea @EricoMeger @SteveMacenski?

[EricoMeger]

Hmmm, ok. Thanks for the report @mini-1235

I think I know what's happening: I would assume this is because now RPP will always check up to 1.5m (your min_distance_to_obstacle), while before this PR, it would only check up to 0.9m (your max_lookahead_dist)

Annnd, I have to admit, max_lookahead_dist was something I didn't consider in the implementation. I focused so much on the min_lookahead_dist that I forgot entirely about it.

Should a condition be made that min_distance_to_obstacle must be < max_lookahead_dist? or should we just consider the greater of min_lookahead and min_distance, but cap it at max_lookahead?

What do you guys think? @mini-1235 @SteveMacenski

[SteveMacenski]

Should a condition be made that min_distance_to_obstacle must be < max_lookahead_dist? or should we just consider the greater of min_lookahead and min_distance, but cap it at max_lookahead?

That seems sane to me but I want to hear from @doisyg as the author of that in case he needed that to be further away. @EricoMeger can you open that PR ASAP to unbreak main? Maybe also log a warning if that condition is violated in the parameter handler so folks know its an error case?

[doisyg]

Should a condition be made that min_distance_to_obstacle must be < max_lookahead_dist? or should we just consider the greater of min_lookahead and min_distance, but cap it at max_lookahead?

That seems sane to me but I want to hear from @doisyg as the author of that in case he needed that to be further away. @EricoMeger can you open that PR ASAP to unbreak main? Maybe also log a warning if that condition is violated in the parameter handler so folks know its an error case?

Sorry for the delay.

min_distance_to_obstacle must be < max_lookahead_dist?

This seems fine and what originally expected :

navigation2/nav2_regulated_pure_pursuit_controller/README.md

Line 94 in 01580bd

| `min_distance_to_obstacle` | The shortest distance at which the robot is allowed to be from an obstacle along its trajectory. Set <= 0.0 to disable. It is limited to maximum distance of lookahead distance selected. |

I skimmed through the comments, but if you tag me in the PR to fix main I can check fully and test quickly.

[EricoMeger]

Opened #5616