Here’s a video of the behavior tree in action from the discussion on pausing near goal if obstacles are in the way @ggrigor. It shows 2 examples from @Pradheep_krishna:
-
“Dynamic” obstacle is in the way, the robot will stop, pause, and go around if it has not yet moved out of the way
-
“Dynamic” obstacle is in the way, then moves out of the way, the robot continues on the original route after it moves out of the way from a pause
For the general solution for this type of problem, the primary thing is to find metric(s) that decide which replans are due to this type of issue in order to pause the robot (or always pause the robot if replans are too “significant”? That might not be the most ideal, but something to consider). This BT only does this when in close proximity to the goal since those are one such situation when pausing might be helpful if being asked to go around another direction. Finding a metric for “this path goes around another direction” is, I think, the most important thing, which might involve some analysis not only on the plan comparisons but the map itself. Perhaps computing a voronoi diagram on the static map to cache (or similar) to classify differences in “route classes”.