search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.52k stars 1.27k forks source link

BT XML Idea: Stop replanning when near the goal if path is valid #4003

Open SteveMacenski opened 10 months ago

SteveMacenski commented 10 months ago

When using feasible planners, it might not be sensible to replan when things are clear within X meters of the goal. It can actually be detrimental to do so if the finite change in path tracking error makes exact goals not possible once within < 1m to the goal result in some "looping" behaviors. This is totally unnecessary to achieve the goal successfully given the positional and rotational tolerances that the exact planners don't know about. Or due to correctable finite error in state estimation.

So, it seems very reasonable to use the goal distance BT node to check if we're with x meters of the goal and if so, select not to replan if the path continues to be totally valid (e.g. no occopancy). This could be added as either a new BT or modify existing BTs to take advantage of this knowledge

VArt3m commented 8 months ago

That's a very sensible thing to do. Was thinking about doing something like that to prevent loops when near goal myself - it may be possible to use GoalReached BT node with high (1-3m) threshold for that purpose. Although I suspect that having a "path length" condition would be more correct - but there's no node for that yet.

P.S. I find it a bit strange that GoalReached BT node doesn't leverage goal checker plugins from Controller

SteveMacenski commented 8 months ago

@shannonwarren Lets discuss here:

Making it based on a timer seems excessive, we could simply add a control flow node under the current rate timer which uses a condition node isGoalReached -- or more accurately would be to make a new one using the nav2_util for getting the integrated path distance.

If we're within the distance to the goal spatially or by integrated path length, then we can use isPathValid to check if the path is collision free. If its in collision, then replan. If not, pass on replanning.

We should be able to do this with no changes to existing BT nodes and possibly just creating 1 new BT node which just wraps an existing nav2_util function. Then its just organizing those stages using the basic control flow nodes

shannonwarren commented 8 months ago

Sounds good. I'll work on it.

padhupradheep commented 6 months ago

Did something similar for another project..

however, when the robot is in the goal radius with an obstacle, I just wait and not go for replanning..

https://github.com/ros-planning/navigation2/assets/20242192/60a72ab1-0507-4162-b836-2e836dddf428