[Nav2] Best way of alternating between autonomous navigation and assisted teleop using `follow_waypoints` action server

asked 2022-09-23 09:44:40 -0500

Pepis
130 ●6 ●13 ●15

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

$C:\fakepath\Screenshot from 2022-09-23 09-13-25.png$

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 :)

edit retag flag offensive close merge delete

add a comment

answered 2022-09-23 12:31:45 -0500

stevemacenski

8272 ●34 ●503 ●129 https://www.linkedin.c...

So a few things, my first gut instinct would be to have your BT have some logic surrounding the path. If you have a path from a to b to c to d to ... and you know some of these edges can be done autonomously and others not, I'd break apart in a root-node the segments that are autonomous to do their thing. Then once you hit the end of those segments, switch to another branch which does the assisted teleop and maybe some general error checking or goal checking nodes so that the BT logic would automatically return once within proximity to the goal (though, if you have a bad driver, that could be problematic if there are strict bounds, so having a backup way of exiting teleop would be good).

A slight variation of that, which is where I would probably start, is treating each segment as different a to b b to c, and so on. That way its simpler to think about in your BT root logic before branching to the autonomy vs teleop subtrees.

The last option would be to have the teleop logic outside of the BT entirely, and have that built into whatever autonomy system you have set up that is calling Nav2 to navigate autonomously. So the only requests the BT are handling are the autonomous ones. The teleop commands are sent by the autonomy application itself, which then exposes you some more direct options to natively receive feedback or perform actions on the goal handles based on operator's feedback.

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

I'd want to understand a bit more about this. If you're 'manually' driving, why not have the operator just cancel the action? That would return the teleop action to continue on its merry way. But if 'manual' driving is actually not a human in the loop but something a bit different, having something automated to switch once an objective is reached might be a good idea. We could add in a plugin interface for that. But if there's actually someone with a joystick / web interface / etc driving the robot, then its easy to add a button to 'end' the session which would return the action.

edit flag offensive delete link

Comments

Wow, thanks for your detailed answer Steve! addressing some of your points,

If you're 'manually' driving, why not have the operator just cancel the action?

There will probably be a button available to operators to switch back to autonomous mode, however I need to make sure that they are able to click it only when the robot has actually reached the waypoint where it can resume normal navigation. Otherwise operators would be able to switch back mid-way or drive the robot to a completely off route position which would mess up all the path sequence. I will probably implement a goal checker that needs both to reach the goal and to get the user confirmation to report the goal was succeeded.

Pepis ( 2022-09-23 13:56:59 -0500 )edit

A slight variation of that, which is where I would probably start, is treating each segment as different a to b b to c

I think I will pursue this approach, specially because the follow_waypoints server already wraps the successive execution of the behavior tree for each goal, and as you say it would probably be easier to think of the switching logic this way. From the follow_waypoints server perspective both the autonomous and teleop branch would look the same: the robot will be in movement (RUNNING) until it reaches the next goal (where either branch of the tree should return SUCCESS). Do you see any perks of having the BT run all the waypoints successively on a single server call instead of using the follow_waypoints server to call it one time per waypoint?

Thanks again for your answer!

Pepis ( 2022-09-23 14:00:57 -0500 )edit

Mostly just easier book-keeping, I'm not sure it would be a big difference either way.

stevemacenski ( 2022-09-23 17:58:57 -0500 )edit

add a comment

[Nav2] Best way of alternating between autonomous navigation and assisted teleop using `follow_waypoints` action server

1 Answer

Comments

Question Tools

Stats

Related questions

[Nav2] Best way of alternating between autonomous navigation and assisted teleop using `follow_waypoints` action server edit

1 Answer

Comments

Question Tools

Stats

Related questions

[Nav2] Best way of alternating between autonomous navigation and assisted teleop using `follow_waypoints` action server