Required Info:
- Operating System: Ubuntu 22.04
- ROS2 Version: Humble
- DDS implementation: Cyclone DDS
I'd like to start by explaining why I needed this, with a BT example
<RecoveryNode number_of_retries="10">
<FollowPath name="x" path="{x}" controller_id="x" goal_checker_id="x"/>
<Sequence>
<ClearEntireCostmap name="ClearingActionsLocalCostmap" service_name="local_costmap/clear_entirely_local_costmap"/>
<ComputePathToPose name="x" goal="{goal}" path="{x}" planner_id="x"/>
<PublishPathMsg topic="x" path="{x}"/>
<IsPathValid name="x" path="{x}" enable_truncate="true" distance_forward="0.50" distance_backward="0.0"/>
</Sequence>
</RecoveryNode>
In this BT, because of how RecoveryNode works, we can't reset number_of_retries until FollowPath returns SUCCESS.
My real goal here is to stop the robot from constantly regenerating a path in dynamic environments, since replanning over and over in a dynamic environment can slow things down. With dynamic obstacles, maneuvering (replanning) usually doesn't make sense you can just wait a bit and carry on, because the obstacle moves out of the way itself. With static obstacles, on the other hand, no matter how long you wait in front of them, you eventually have to generate a new path.
The problem is this: on the way from A to B, I can't know in advance how many obstacles I'll run into, or whether they'll be dynamic or static. So a fixed retry value doesn't really mean much.
If I set it high (say 100), then at a static obstacle I'd have to wait 101 cycles; depending on system speed that's a bit excessive and dummy. And even if I assume only dynamic obstacles, the first moving obstacle burns 10 attempts and leaves 90 so I can react to at most 10 dynamic obstacles (and only if they all behave the same way).
Whereas from long-RUNNING nodes like FollowPath we can get feedback.
I added a way to reset this retry counter not only when the child returns SUCCESS, but also when the robot actually makes progress. The "progress" metric here is the distance the robot travels, which I measure from TF. I added two ports, both disabled by default:
reset_distance (meters), which resets the counter once the robot has moved that far since the last reset, and reset_time (seconds), which resets it once that much time has elapsed.
Since RecoveryNode is Nav2's own node, I added the ports to it directly. RetryUntilSuccessful, however, is a BehaviorTree.CPP built-in, so it can't be extended in place within Nav2; I provided the same behavior with a small Nav2 decorator that mirrors it (same num_attempts semantics), added under a different name.
Same BT, now with the port:
<RecoveryNode number_of_retries="10" reset_distance="0.5">
<FollowPath name="x" path="{x}" controller_id="x" goal_checker_id="x"/>
<Sequence>
<ClearEntireCostmap name="ClearingActionsLocalCostmap" service_name="local_costmap/clear_entirely_local_costmap"/>
<ComputePathToPose name="x" goal="{goal}" path="{x}" planner_id="x"/>
<PublishPathMsg topic="x" path="{x}"/>
<IsPathValid name="x" path="{x}" enable_truncate="true" distance_forward="0.50" distance_backward="0.0"/>
</Sequence>
</RecoveryNode>
Now the 10-retry budget resets every 0.5 m of real progress, so the robot only gives up after 10 tries when it is genuinely stuck
Required Info:
I'd like to start by explaining why I needed this, with a BT example
In this BT, because of how RecoveryNode works, we can't reset number_of_retries until FollowPath returns SUCCESS.
My real goal here is to stop the robot from constantly regenerating a path in dynamic environments, since replanning over and over in a dynamic environment can slow things down. With dynamic obstacles, maneuvering (replanning) usually doesn't make sense you can just wait a bit and carry on, because the obstacle moves out of the way itself. With static obstacles, on the other hand, no matter how long you wait in front of them, you eventually have to generate a new path.
The problem is this: on the way from A to B, I can't know in advance how many obstacles I'll run into, or whether they'll be dynamic or static. So a fixed retry value doesn't really mean much.
If I set it high (say 100), then at a static obstacle I'd have to wait 101 cycles; depending on system speed that's a bit excessive and dummy. And even if I assume only dynamic obstacles, the first moving obstacle burns 10 attempts and leaves 90 so I can react to at most 10 dynamic obstacles (and only if they all behave the same way).
Whereas from long-RUNNING nodes like FollowPath we can get feedback.
I added a way to reset this retry counter not only when the child returns SUCCESS, but also when the robot actually makes progress. The "progress" metric here is the distance the robot travels, which I measure from TF. I added two ports, both disabled by default:
reset_distance (meters), which resets the counter once the robot has moved that far since the last reset, and reset_time (seconds), which resets it once that much time has elapsed.
Since RecoveryNode is Nav2's own node, I added the ports to it directly. RetryUntilSuccessful, however, is a BehaviorTree.CPP built-in, so it can't be extended in place within Nav2; I provided the same behavior with a small Nav2 decorator that mirrors it (same num_attempts semantics), added under a different name.
Same BT, now with the port:
Now the 10-retry budget resets every 0.5 m of real progress, so the robot only gives up after 10 tries when it is genuinely stuck