@sathak93, 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

Should I take a look at this or wait until you work on the DWB part?

This bug #2622 is due to either of

1. min_by function used to pick transformation_begin pose - when the path overlaps, sometimes it picks the later poses than immediate ones which leads to deleting of poses. in this pr i changed the range from global plan begin to look_ahead point instead of global plan begin to global plan end so that the immediate ones will be always selected.

2. find_if algorithm in getlookaheadpoint picks wrong pose if the transformed plan has some poses in front of the robot and some back to the robot. in this pr, look_ahead point in progressively reduced when nearing the cusp using the already defined function for reversing behavior. And the transformed_plan ending is selected using the integrated path distance instead of absolute distance

find_if and min_by algorithm was the main cause of the this bug. this was not suitable to overlapping path case of navthrough poses behavior.
this is from my understanding of it. i am open to any improvement or suggestion of new algorithm

@Aposhian please let us know your thoughts 😄

Here is the difference between the stock galactic controller, and building this PR for rolling. The green panels are the global plan (which is being updated by a custom planner), and the base_link TF can be seen cruising along, with the lookahead point being updated in red.

n
When I try this PR, the robot approaches the initial lookahead point, and then stops.

I think this may relate to the fact you're giving it a hilariously small path, this is handling the looping for you so you shouldn't need to give it something so small. Though this is clearly an issue, are you sure that the controller is getting those path updates?

@sathak93 why would this be?

@SteveMacenski i will check this pr for very small path. also i think it is best to validate RPP of this using main branch rolling. RPP in galactic branch and in rolling branch has many differences. using rolling we can focus problems related to this pr.

I think @Aposhian did use rolling's version of RPP to see that issue. I think he was just highlighting the difference with a baseline (no?)

Yes, the short path that is being given has been our workaround to the bug that we are trying to fix here 😄, so that it something that we should hopefully be able to remove, but I don't think that this fix should cause a regression for short paths. I will test on nav2 main to compare as well, and will try removing our intermediary path segment selection to see how this fix addresses our problem.

Here is the performance against the main branch: it doesn't exhibit the stalling behavior that I am seeing with this PR.

I have made some modifications on my own fork. But it has occured to me: if we implement an upper bound to prevent jumping to future points, then we are implicitly increasing our reliance on the lower bound provided by the global_plan pruning. What this means that if we make the change this way, it may break backward compatibility with some users' expectations by removing the controllers capability to "snap" to any point on the path, provided by the current behavior of just grabbing the closest pose to the robot.

"snapping" is fine and easy for paths that don't overlap, but with overlapping paths, you need to make sure that you always prioritize earlier portions of the path over later portions, and it may be hard to do that in a deterministic but also intuitive way.

There are a few ways I think we could solve this problem:

• add a parameter to give users control on whether they want "snapping" or being able to do overlapping paths (nonideal)
  • this would essentially work by either starting the local plan at the first pose in the global plan (enforced lower bound) or using the current behavior of snapping to the absolute closest waypoint
• instead of finding the absolute closest waypoint, maybe find the minimum (by index) waypoint within a predefined "search radius". This would support snapping (within reason)
  • this poses the following tradeoff: a greater search radius provides a larger snapping distance, but may not snap to the closest waypoint
• use a heuristic of minimum waypoint index and distance between robot and pose to determine which waypoint to snap to.

Take the following global plan for example:
blue are poses in the global plan, and red is the robot pose.

e

Now, assuming that nav2 has been incrementally pruning the start of the global plan, in this situation, the controller should snap to pose 2. But how do you describe that as an algorithm? Closest pose won't work, since that would snap to pose 10. If you do a simple search radius paired with minimum waypoint id, then if search_radius = 4, then it will snap to 2. But if search_radius = 5, then by the same algorithm, it would snap to pose 1, which is not ideal.

Another option would be be to get the closest waypoint between the start of global_plan and the 180 degree turnaround. That should snap to pose 2, and that should support many, but not all cases where people want to snap to paths that don't overlap.

@SteveMacenski I have made some modifications on my fork based off of @sathak93 's work. Should I reflect those as suggested changes in GitHub here? Or should I create another PR?

I'm not entirely sure I'm following you.

if we implement an upper bound to prevent jumping to future points, then we are implicitly increasing our reliance on the lower bound provided by the global_plan pruning

Correct, but that should be pretty good, no? I haven't run into any issues w.r.t. this. We're recursively updating quickly, so there should be little change between iterations since its being run at 10-1000 hz. So as long as we can detect the robot starting pose effectively (what the role of this PR is to improve), the pruning should be spot on. I think that's safe to rely on, and in fact we already do so there's really not much change w.r.t. that.

What this means that if we make the change this way, it may break backward compatibility with some users' expectations by removing the controllers capability to "snap" to any point on the path, provided by the current behavior of just grabbing the closest pose to the robot.

This is where maybe your wording is confusing me. It should always select the path point that it was tracking in time = t-1 plus 1-N waypoint markers depending on the speed and density of the path. It should be going sequentially.

If I'm understanding what you mean by "snapping", you mean that it jumps to another part on the path that is technically closer, but then skips some parts of the path. That is a bug, not desired behavior (and what we're trying to fix here). I don't think anyone expects that behavior for us to want to retain it for backward compatibility. Certainly that is not a feature that we advertise or I would ever think to be desirable. Especially for a pure pursuit who's goal is to purely follow.

The logic as written's goal is to find the closest point on the path that the robot is, for which it assumes perfect path tracking capability. If the path tracking is bad so that its actually closer to another part of the zig-zag, I don't think you want it to select a point that is significantly further along the route than you were in just 1 iteration prior based on that error.

always prioritize earlier portions of the path over later portions

maybe find the minimum (by index) waypoint within a predefined "search radius".

Isn't what this PR is doing? But instead of a radius search, its limiting the search for what path point is most close to the robot by changing the search condition from point 0 -> point len(path) to now point 0 -> point N where N is bounded by an integrated distance from point 0, which we're discussing how to find N in the comments above (probably based on the length of a side of the costmap or similar). So we're limiting the search by the integrated path distance from the point 0 which is recursively updated as essentially right behind the robot pose, essentially.

On your diagram: I think if you look at the PR, that's the point of L604-L611's changes. We're restricting the search via the local window because we know that we're running this at 1, 5, 10, 100 hz, so its frequently. Therefore we know we shouldn't be "very far" from the last iteration so we're recursively able to update knowing we're not "far" from the last update. There's no need for the radius based search because we can just prune to only look in a local region ahead which can be set by a parameter or costmap size.

The costmap size would be most useful, but that only works if you maneuvers aren't going to be squiggling multiple times within the 2x2 or 4x4 meter window. If you are making those small finite turns, we may want to instead make it based on a parameter so that it can be set lower. But for any normal system, you'd never expect to have something like what you're showing in your figure all bounded within the local costmap size. It would be a larger trend several times larger for rows / etc. But your diagram has no scale so I can't tell if you're talking about that squiggle being a 2x2m window or a 20x20m window.

For configurability, maybe its a good idea to make it a parameter, but inform users in the configuration guide that a good bet for it is the costmap side length, unless making very small maneuvers that might overlap like what you're showing.

Or should I create another PR?

Another PR would be good and point out what you changed so its easy for me to tell on review.

Sorry if my comments were confusing, since I was bringing up a few different alternatives. I do agree that parameterized closest pose is better than using a search radius, which is not as robust. I think the bound should be up to the local costmap OR the nearest turnaround point.

This is where maybe your wording is confusing me. It should always select the path point that it was tracking in time = t-1 plus 1-N waypoint markers depending on the speed and density of the path. It should be going sequentially.

I suppose most users would not experience this "snapping" behavior since they use planners that always start the global plan from the current robot pose. Our planner doesn't necessarily do this, and this is why we have observed the "snapping" behavior since it just grabs the closest pose.

My modifications are here: #2812

@sathak93 thank you kindly for jump starting this discussion and developing the core ideas to solve this problem. It was very clever and didn't occur to me and a few others so we really appreciate you jumping in and I hope to have your help again in the future!

I'm going to close this ticket since #2812 supersedes with a slight variation on your technique to work better with another user's experiences to make sure this can work for everyone's needs