[Nav2] Best way of alternating between autonomous navigation and assisted teleop using `follow_waypoints` action server
Hi everyone!
I'm currently using Nav2 on a robot that has to perform long navigation tasks (>1000m) across large environments (~5sqkm); I'm relying on the follow_waypoints
action server for passing long sequences of sparse waypoints. I don't have a static 2d gridmap of the environment but rather a graph of sparsely interconnected nodes.
For different reasons there are small sections of the environment where the robot cannot drive autonomously. These sections are already pre-mapped in the edges between certain nodes on the aforementioned graph. See the below picture for a clearer explanation
On the picture above, considering the marked robot route (dotted arrow), what I would like to achieve is to tick the usual navigate_to_pose
behavior tree for getting from node 2
to node 3
and from node 3
to node 4
. When navigating from node 4
to node 5
I would like to trigger the assisted teleop behavior server to manually drive the robot. When reaching node 5
I would like to switch back to the navigate_to_pose
bt to get to node_6
and so on.
I have thought of making a custom behavior tree node to switch between a navigate_to_pose
branch and an assisted_teleop
branch according to the goal_id that is to be reached. Also I would have to integrate a goal checker into the assisted_teleop
server to SUCCEED
the branch when the given goal is reached manually.
However I'm aware that some discussion has been held around this kind of "semantic navigation" to change the behavior of nav2 according to semantic labels defined on a sparse graph, so I wanted to ask the community about this beforehand.
Have someone faced the same problem? Would you recommend a different alternative? Is there any standard way of doing this?
Thanks in advance for your help and any insights you can provide :)