Multi-floor navigation with topological nav

[cburbridge]

I am setting up Bob to do multi-floor navigation using the lift. I think the tidiest solution will be to build on topological navigation instead of having a hard coded additional navigating state machine. However, this leads to number of potential issues:

(1) when the robot moves to a different floor it has different topological nodes. However, these nodes have a similar x,y,th position to the nodes on other floors unless the maps don't line up. This would result in the topological_localisation localising to the wrong floors nodes unless it knows which 2d_map is currently being used. But the metric_map field is not used at the moment right?

(2) I have made a map which has all floors in one map to avoid (1) and having to switch map. Each floor has a node in the lift, and all the inside-lift nodes are joined with edges across floors. I want to create an edge action server for these edges that waits for the lift doors to open at the right floor then teleports the robot to the correct node/metric position. However, only action types that can be used as edge actions are implementations of MovebaseAction. The action edge action server should be allowed to be anything, with parameters defined in the edge specification. The action server needs to know the target floor in this case, and hard coding a lookup table outside of the topological map is an ugly work-around.

(3) The robot needs human assistance to call the lift, so the edges linking nodes outside the lift to inside the lift entrance will call an action server that looks for people, and asks them for help calling the lift, then incorporates door passing to get into the lift. This requires (2) to be fixed, but aside from that is it ok for an edge action server to move the robot around in a non-direct path between nodes, possibly passing through other topological nodes? Or would this trigger a re-topological plan?

[Jailander]

Hi, I already have a system changing the maps acording to the map defined in the edge of the topological map it needs a bit of work before merging however, my plan was to do so when I'm back from holidays on August 19th I hope is not too late.

About the action server for the topological map, we are only using movebase goals msgs since its easier for monitored navigation, maybe we could make a work around it but I don't know if that is desirable maybe @bflacerda has something to say about it, in any case what would you need to define that is not defined in the movebase goal?

And for your last question yes passing through nodes will trigger replaning if you are using the mdp, however my question is how are you localising inside the lift, maybe that is the way to work around that in this case

[bfalacerda]

regarding the floor goal, the issue is that monitored navigation is independent of the topological map. The way I found to have it monitoring different actions servers was fixing the type of goal they re allowed to have to a MoveBaseAction.

We can think of a better way to do it once I'm back (September), for now my suggestion is using the "z" coordinate in the Pose to specify where to go.

Also for now the looking for people can be a problem with policy execution, he won't like going to different nodes. This can be taken care of by modelling a "look for people" action on the mdp, but again, holidays. Are you ok just waiting in front of the lift for now?

Btw, 👍 for starting to push for this :)

[Jailander]

For the people search you can also try to use the topological navigation action server directly which doesn't mind the passing through different nodes

[Jailander]

Hi I have a version of this working on this branch https://github.com/strands-project/strands_navigation/compare/indigo-devel...Jailander:icra16?expand=1 , it needs some testing and it needs to use message store map switcher it works having maps associated with edges in the topological map and it loads the corresponding metric map when traversing the edge.
If this is a good approach for you I'll do the testing and merge next week.