do not depend on velocity for approach scaling

[Aposhian]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3045
Primary OS tested on	Ubuntu
Robotic platform tested on	Not yet

---

Description of contribution in a few bullet points

Depend only on the path length left on whether to scale velocity on approach, since depending on lookahead distance results in a cyclic dependency

Description of documentation updates required from your changes

Added a new parameter.

---

Future work that may be required in bullet points

Need to test this and build it still.

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.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

[mergify]

@Aposhian, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• Check that any new parameters added are updated in navigation.ros.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

[SteveMacenski]

Beyond the obvious points, this seems good to me!

[mergify]

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

[SteveMacenski]

@Aposhian any update? It would be great to have this in before the next sync & close out #3045

[Aposhian]

I may be able to get to this on Saturday, but I have been a little swamped.

[Aposhian]

Here is how it ends up performing with the default 1m threshold. I think that is probably not a good default. I will try to figure out how to set this parameter to have roughly the same behavior as before.

[SteveMacenski]

Sounds good! Toggling since CI didn't trigger again after the changes

TODO

• Default for similar behavior
• Add docs to navigation.ros.org
• Add comment in readme (and navigation.ros.org) about sizing of the new parameter w.r.t. the costmap size

[Aposhian]

Ok I just updated the default to be one costmap cell less than the forward costmap extent, so that should approximate the old behavior (where scaling was enabled when lookahead distance was shorter than requested). Here is the behavior with that now on my robot sim:

One problem with looking at integrated distance of the transformed plan is that it jumps by the robot jumping between poses (this is with a sparse path). However, I think this is a far better behavior than what is existing, and this is also an issue that the velocity smoother could help remedy, correct?

[SteveMacenski]

Ok I just updated the default to be one costmap cell less than the forward costmap extent, so that should approximate the old behavior

Uh, wasn't it before that it started to slow down at the lookahead distance away, e.g. by default about 0.6m? This is much higher than that if its 1/2 the costmap size. The added comment in the readme also doesn't say that you should never set it to be higher than that -- what I meant by my comments in threads above was we need a warning not to exceed, not that this is the recommended value.

But maybe its a better value, but I suppose you didn't mention that you tried the old setting (~0.6) and found that the longer distance was better. Maybe I need a clarification here on what you tried to come to that conclusion (and still need that warning in the readme / in the code that it checks and never exceeds that + logs a warning about it and sets to the max valid possible value if they tried to exceed it)

Yes, the vel smoother will work out the jaggedness.

[Aposhian]

Uh, wasn't it before that it started to slow down at the lookahead distance away, e.g. by default about 0.6m

Well, I figured there wasn't a good way to recreate the average lookahead distance, since the velocity is unknown. But maybe the default could be calculated based off lookahead time and desired linear velocity?

what I meant by my comments in threads above was we need a warning not to exceed, not that this is the recommended value.

Ok, I misunderstood, but that makes sense. I think I can add a bounds check for that.

[SteveMacenski]

Well, I figured there wasn't a good way to recreate the average lookahead distance, since the velocity is unknown. But maybe the default could be calculated based off lookahead time and desired linear velocity?

Why not just use the value of the parameter lookahead_dist as the default? Though, if Adaptive behavior is being used, I suppose just max_lookahead_dist would be fine too, but if I had to pick one, I'd pick lookahead_dist since that should be no matter what type of PP was being used. Just needs to be documented that it defaults to lookahead_dist unless otherwise overridden (and should never exceed costmap size / 2).

Its imperfect, but it sounds reasonable. The other alternative is to just make them unrelated by default to anything else and just set it to 0.6 like it was before. I suppose there's no specific reason we need to make the default tied to the lookahead distance anymore as long as the default behavior is similar. I don't care very much, I think both are rational options, so I leave it to implementer's choice 😆

[Aposhian]

Docs PR: ros-navigation/docs.nav2.org#336

[Aposhian]

I decided to go with just making it an independent default, but going to 0.6 to match previous behavior. Unit tests are updated. I'm just going to do one more test in simulation with this and then I will be happy with it.

[Aposhian]

With some tuning this is what the approach velocity scaling ends up looking like (on a sparse path)

I think we can probably do better, but this at least removes the oscillating behavior because of the cyclic dependency.

[Aposhian]

@SteveMacenski I think all tasks should be addressed. Let me know if there is anything else.

[Aposhian]

If you weren't using adaptive lookahead distance before, then this change will be a regression: it will result in a less smooth scaling. I think we can tweak this algorithm to make it scale linearly, regardless of path density. I just need to brainstorm for a bit.

[SteveMacenski]

Toggling for CI, Circle's been flaky lately. Will merge once builds

[Aposhian]

Here is what it looks like with the smoothed version: