RPP: when far off path but within costmap, min approach velocity is used erroneously

[Aposhian]

Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• ROS2 Version:
  • source from main
• Version or commit hash:
  • 3ecb4d0
• DDS implementation:
  • N/A

Steps to reproduce issue

1. Configure desired_linear_vel * lookahead_time to be less than half of the local costmap width and height
2. Place the robot to be as far away from a pre-planned path as possible, but still within the costmap

Expected behavior

This will yield a lookahead point that is further away than the desired lookahead distance. While this configuration may be a little odd, you would expect the robot to proceed to that point at the desired_linear_vel (subject to curvature and cost scaling if enabled).

Actual behavior

The robot proceeds at a steady pace of min_approach_linear_velocity. Here is why:

RegulatedPurePursuitController::applyConstraints assumes that dist_error will always be less than lookahead_distance, so that (dist_error / lookahead_dist) will always be less than one, and velocity_scaling will end up being a number between 0.0 and 1.0. This assumption breaks when sqrt(carrot_dist2) is greater than the lookahead_dist.

https://github.com/ros-planning/navigation2/blob/719b5455b43d75c0d7c8cf7c2d15f7152e555d78/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L613-L624

  if (dist_error > 2.0 * costmap_->getResolution()) { // this can be greater than two costmap cells, and still be greater than lookahead_dist
    double velocity_scaling = 1.0 - (dist_error / lookahead_dist); // when dist_error > lookahead_dist then this is negative
    double unbounded_vel = approach_vel * velocity_scaling;
    if (unbounded_vel < min_approach_linear_velocity_) { // always true because unbounded_vel is negative
      approach_vel = min_approach_linear_velocity_; // approach_vel is set to this, even though the carrot is very far away
    } else {
      approach_vel *= velocity_scaling;
    }

    // Use the lowest velocity between approach and other constraints, if all overlapping
    linear_vel = std::min(linear_vel, approach_vel); // min between 0.5 and 2.0
  }

Additional information

While it could be argued that these ratios of lookahead distances and costmap sizes is uncommon, the fact remains that it is supported by RPP, and this edge case introduces unexpected behavior.

[SteveMacenski]

While it could be argued that these ratios of lookahead distances and costmap sizes is uncommon
Place the robot to be as far away from a pre-planned path as possible, but still within the costmap

Actually, for this example the unusual thing is the use of a pre-planned route, allowing your robot to be far away from the path in the first place (versus dynamic planning / replanning). I only considered the robot to be relatively speaking close to the path, since this controller directly follows the path and as long as it started on the path, there shouldn't be a reason that it be able to get itself so far off of it for that ratio to be problematic. dist_error is supposed to measure the error between our theoretical lookahead point and the actual one used, as a proxy for being on approach to the goal (since if within lookahead_dist of the goal, the furthest lookahead point possible is shorter-and-shorter on approach).

So you bring up an interesting issue. With that context in mind, do you have a suggested change in mind?

The first place I'd go is to say that the distance error is the wrong metric to use because of this case. Instead, we could have it be a radius to goal metric with the same other logic. If the last point on the path is within lookahead_dist radius, apply the same logic with the same 'error' calculation. That would be a similar proxy, though inexact.

Another option would be to check the if condition in a signed fashion. Instead of fabs() the error in the method call, we pass it a signed value and then fabs() it inside of this function when we want to use it. Then we can only do this when the error is positive. When negative (e.g. carrot_dist2 >> lookahead_distance) it wouldn't trigger this condition.

I'm sure we can come up with other solutions too. I think my preference would be the second option to keep the behavior as similar as possible. I would be curious though if this didn't trigger and the robot was far away from the path how well it actually got back on the path. That feels more like a job for some splines (which I've thought before about adding as a controller, but never knew if anyone would actually want it)

[Aposhian]

So what the current algorithm is saying is: "If I request a lookahead distance, and the actual distance from the lookahead point is different enough, then I must be at the end of the path." I think we should just replace that logic with "If I request a lookahead distance, and the actual distance from the lookahead point is shorter enough, then I must be at the end of the path."

Another option would be to check the if condition in a signed fashion. Instead of fabs() the error in the method call, we pass it a signed value and then fabs() it inside of this function when we want to use it. Then we can only do this when the error is positive. When negative (e.g. carrot_dist2 >> lookahead_distance) it wouldn't trigger this condition.

So I think that would do the trick. I can make a PR for this when I get some time.

[SteveMacenski]

"If I request a lookahead distance, and the actual distance from the lookahead point is shorter enough, then I must be at the end of the path."

I think that's more or less what the sign check would do. It wouldn't be as contextually clear but a quick comment would clarify.

I'm happy to make the change, but I'm basically going to turn around and ask you if it fixes your problem, would you mind submitting a PR if it fixes your problem?

[SteveMacenski]

Whoops

[SteveMacenski]

Any update? No rush, but planning on branching off for Humble in the next week so it would be great to have this reflected in the initial Humble release. But this is a non-API breaking change so we can always make it after the fact and have it included in a sync.

[Aposhian]

I probably will not get around to this soon, with a lot of other things on my plate.

[SteveMacenski]

#3045 supersedes