[RPP/DWA/TEB] When NavThroughPose has an intersecting loop, it can occasionally pick the wrong path segment

[SteveMacenski]

I suspect its for one of a few reasons

• If it does "skip", its because the path is fully contained within the local costmap, so the pruning doesn't get rid of the future-looping section and then the "cross" point later in the path is technically closer to the lookahead requested point than the next sequential point based on some very small amount closer (<1mm)
• The fact that it jumps to that part once the robot hits that "cross" point makes me think its actually an issue in transforming the path, where the point closest to the robot becomes the other side of the "cross" and so it uses that (<1mm).

There's a couple of potential fixes:

• When we find the points closest to the robot to start transforming the path from, there should be some tolerance, like 0.01 or something, that if there are multiple points within that distance range of each other, use the first in the path rather than the closest one. That should help it pick the right part of the cross
• Rather than only checking when it leaves the costmap to prune til, also add a path distance element, once the path length from the closest pose is larger than N meters, also prune past those points. That way if there's a cross in the local costmap, as long as we pick the first point, we should prune out the rest

I suppose this part in erasing the passed parts should be a solution if there is no replanning (so we don't infinitely loop cauased by selecting the "first" part of the intersection in the path). So I think doing those 2 points should cover that edge case completely.

I'm pretty sure that DWA and TEB would have this exact same problem, but its something we can do something about in RPP much more easily.

[SteveMacenski]

oddbehaviour_galactic.mp4

[KavenYau]

What about using accumulation distance rather than the radius?

https://github.com/ros-planning/navigation2/blob/8c80af04b92ef537b8fc74e7f8f3b07f869a587c/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L377-L380

[SteveMacenski]

accumulation distance

Can you elaborate?

[KavenYau]

Calculating the distance between every two points of the path and adding them up.

For example:

  double accumulation_distance = 0.0;
  auto goal_pose_it = transformed_plan.poses.begin();
  auto temp_it = transformed_plan.poses.begin();

  for(; goal_pose_it != transformed_plan.poses.end(); ++goal_pose_it) {
    if (goal_pose_it == transformed_plan.poses.begin()) {
      accumulation_distance += hypot(goal_pose_it->pose.position.x, goal_pose_it->pose.position.y);
    } else {
      accumulation_distance += hypot(
        goal_pose_it->pose.position.x - temp_it->pose.position.x, 
        goal_pose_it->pose.position.y - temp_it->pose.position.y);
    }
    temp_it = goal_pose_it;

    if (accumulation_distance >= lookahead_dist) {
      break;
    }
  }

But if the accumulation distance of intersecting loop is shorter than lookahead_dist, it will still occasionally pick the wrong local goal.

[SteveMacenski]

We want a distance away, not integrated over path length away. Although that does bring up a good question "Why?", all the PP algorithm implementations / papers I see use distance, I've never heard of a proposed method using the integrated path-distance away. There's nothing in stability analysis that would make that an invalid approach. I wonder if functionally it just doesn't work as well or its one of those "so obvious no one's bothered to think about it". We'd need to do some experimentation since that would fundamentally alter the nature of the plugin's behavior

[SteveMacenski]

I'm changing the scope of this ticket to cover all of the controller plugins since they all have this issue. Seems like something we should implement as a nav2_util and apply to the controller plugins

[dpm-seasony]

I've seen this issue even when not in NavThroughPoses mode if the path intersects with itself. From what the potential fixes seem to be, I guess they should resolve the issue also when not in NavThroughPoses mode

[SteveMacenski]

@mradams-bastian writes:

If I understand everything correctly, I would argue that this is not only a problem in how controllers handle overlapping paths, but it is also a consequence of the default goal checker used by the controller server being the Simple Goal Checker which determines that a goal has been reached solely based on whether the current pose of the robot matches the final pose of the path. If the final pose of the path happens to overlap the trajectory of the robot, the loop in controller_server.cpp ends at that moment and publishZeroVelocity() is called.

Maybe it would make more sense to have the Stopped Goal Checker be the default? Or even create a more sophisticated goal checker?

---

Steve: That's a fair point to consider. However if we better pruned the paths going to the controllers then they wouldn't see this anyway. Maybe we should be doing something where we only send the next N meters of the path to the local trajectory planner from the controller server each iteration in the while loop. As the robot progresses, it will unlock more of the path. That would prevent looping issues, but could still be problematic if you have long stretches of colinear paths

[mitchradams]

Would it be possible to additionally check if that pruned segment crosses or overlaps itself, and if so, only send the path up to the point of intersection?

[SteveMacenski]

I don't think that would be very robust, especially in the colinear situation where there may be several overlaps close to each other or large sections of pure overlap

[mitchradams]

What are some example situations you're thinking of where it might fail? I am imagining that while traveling overlapping sections, the first time the robot passes over the section, the controller would not even realize it is an overlapping segment because the portion of the path that wraps back around and overlaps will have been pruned. When it passes back over the overlapping section a second time, the completed portion of the path will have been pruned away, so once again it will not realize that there was an overlap to start.

The biggest potential issue I can think of that if a path makes a 180 degree turnaround, the 'return' portion of the path would not become visible until the robot reaches the very end of the 'to' portion of the path, and as a consequence it would stop at the end of the 'to' portion of the path with no more path to travel before the new segment becomes available immediately after. The robot would have to come to a stop either way it it were to abruptly turn around like that, so I don't think that would be an issue. But are there any other issues situations like this would cause in the controller, etc?

[SteveMacenski]

If you have multiple intersections due to roughly co-linear paths such that each segment before a "cross" is very short, resulting in extremely poor path tracking abilities since it can't see in the future far enough. Imagine they're in the same pixels for 4 pixels and then 1 is off by 1 cell, and then back on the same route. That small deviation would split up the paths into a tiny 5 point segment.

Also imagine you have 2 colinear segments, one that leads to the other with a point that marks the change between them. As the robot approaches that point, it will start seeing sections of the other side of it causing the same issue as before if we only prune based on integrated path-distance away. Removing segments based on only directional changes is probably going to be error prone and also has the issue I describe in the paragraph above where there's no limit for how large or small they could be. They could be single points or 3-4 points making very small segments resulting in very poor control behavior & slowing the robot virtually for controllers that slow on approach with few points left (all of them).

Overall, I think the co-linear problem is another altogether, the example you brought up is a very extreme case and I'm not sure there will be a generalized solution possible if you have path segments that are exactly or nearly overlapping for any consequential amount of time. No path planner should be giving you those results & NHP can give you it, but only if poorly selected points are used. There are other tools like the waypoint follower that could instead be used that issues only 1 point at a time.

There may be a solution for the colinear problem, but I can't think of one right now. Open for suggestions - but there are quite a few edge cases to consider.

Some solutions here:

• Prune path N meters of integral path distance away, to be set based on the controller algorithms being used as a parameter. This will solve at least a standard deviation amount of the problem. DWB already does this, we can pull it out into the server and use it for all controllers.
• Prune path after passed (if not already done, I think it is)
• See how the goal checker plays into this issue and make it not stop at a goal point if still having more path to track
• (Potentially) Try to detect pass crosses and prune at them (see issues with that in description above) and issue them between visitation
• (Potentially) Try to detect significant portions of path overlap (see issues with that in description above) and do (?) about it