But currently we project using the velocity command, not the current velocity

Correct, because we're checking if our newly computed velocity command is safe. That is the point, I believe that is straight forward?

and ignore the angular velocity except for pure rotation ... since we check the collision in rotation only in pure rotation

We are always using angular velocity https://github.com/ros-planning/navigation2/blob/main/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L436 for our collision checks. This is always taken into account. That's why when you visualize the RPP arc you will see an "arc" not a straight line https://github.com/ros-planning/navigation2/blob/main/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L446. We are projecting forward the entire velocity command, linear and angular, over timesteps to check at each cell marching forward in both normal path tracking and pure rotations . Does this make sense now?

if the purpose is to check that there is no obstacle on the path, then we should project the poses along the path.

That is not the purpose, because while the robot will generally speaking follow the path, this algorithm does not exactly mimick each point of the path. So we need to check that our requested velocity command is going to be in collision if we follow that velocity for some time, using the parameter max allowed time to collision as that amount of time we project forward by to make sure we're always N seconds from collision at all times.

if the path does a 90° turn with a small radius, the current implementation triggers collision detections + drawing

Indeed, that I believe is what we're looking to solve or mitigate! Obviously your image would be of a very large max allowed time to collision, but I understand the image is to illustrate the point, which it does a good job of!

So what is the purpose of the collision check of RPP ? :)

To make sure the robot won't collide with something while trying to follow the path, inexactly but roughly. Often local costmaps are higher resolution than global and contain a bit more information from live sensors, so also to make sure its still safe given the current state of the world.

So the original thing I had was to collision check between the lookahead point and the robot along the arc that makes up the velocity command. That's essentially what we're doing now, except the length of that arc we check is determined by the velocity command and this max time to collision.

That way if we are moving very slowly and we have a very far look ahead point (2 meters), we only check the little bit that is actually important to us in that cycle. Conversely, if we're moving very fast and we have a short look ahead point (1 meter), we check further ahead to make sure it is still valid.

I suppose though that if we removed the time to collision option and relied on the lookahead point, that could solve the issue of significantly overshooting the look ahead point and into the obstacle that it would never actually drive into (from future updates to the look ahead location). Perhaps, we should the collision checking be forceably limited to the lookahead point, at max? In your image, that would make sure it is always an arc "mimicking" the path, and never continuing to overshoot in the direction long after the lookahead point is done.

However, that would also mean that we could be less than N seconds from a potential collision to a dynamic obstacle that is in the way of the path before a new path plan comes in -- which then couldn't be avoided if a user was moving very fast and set very close lookahead point... but maybe that's a reasonable compromise if documented? We just drop support for guaranteed maximum time to collision and instead just check between the robot and lookahead point along the new velocity arc?

Then maybe also provide some design advise that you should set your look ahead point to at least maximum desirable time to collision * maximum speed meters? That way you could effectively have the same behavior just by being smart about your parameters.

Thoughts?

RPP Warnings at goals near obstacles on approach #2670

Description

Metadata

Metadata

Assignees

Labels

Type

Projects

Milestone

Relationships

Development

Issue actions