Poses from valid nodes being retrieved as null

[felipeduque]

Here's what's been bugging me. I have built a map with roughly 130 nodes. Let's say the robot is spawned at a certain location, and correctly localized itself at node 90. Now I want it to navigate to the node whose ID is 10. I know it's a valid ID because previously I had called RTAB-Map's get3DMap() method and checked all valid IDs.

But if I call /rtabmap/set_goal and plug in the desired ID, it says that the node is not in map's graph. For some other nodes, however, no issue is raised. Why is that? Is it something related to the distance to the current location?

I also tried to send the pose goal programatically via goalCommonCallback(), with desired ID = 0, empty label and desired pose = rtabmap_.getPose(10), and I found out that the pose is taken to be null.

Tinkering around a little bit, I incidentally found out that get3DMap() actually returns all the nodes' poses!, but getPose() doesn't. Why is that? Is there any drawback if I just use get3DMap() to retrieve all the poses? I believe getPose() would be more intuitive!

Thanks!

[matlabbe]

Hi,

get3DMap() returns all nodes, including those not in the graph. For example, the nodes not added to graph when the robot was not moving. They are all saved in database by default for debugging purpose. You should look at the poses in the returned  "poses" argument. You will see that "poses" may not have all ids, it contains only those ones in the graph, thus only those ones you can send a goal on.

getPose returns null if the node is not in the current graph. It should return all poses included in "poses" of get3DMap (when calling get3DMap with global=false).

If you call /rtabmap/set_goal with an ID and it fails, it means the node is unreachable, because it doesn't exist or it is not linked directly or indirectly to current node in the graph (the node is from an older session not linked to current session or that the node has been ignored as explained above).

In rtabmapviz, with the GraphView, we can mouse over the nodes to know their ids. All ids shown in the graph should be accessible for sending goal.

In rviz, you can add a MarkerArray with /rtabmap/labels topic along the rtabmap_ros/MapGraph display to see the ids of the nodes in the graph.

EDIT: I updated the code to not return invalid nodes, so now the only reason you cannot plan a path to a node in poses or signatures of get3DMap is because its graph is not linked to current one (like trying to plan to a node in map 1 while in map 2 and there is no yet a loop closure between the maps).

cheers,
Mathieu

[felipeduque]

Thanks for the quick reply! Just some more questions:

1) What are the differences between the nodes currently in the graph and nodes not in the graph? From what I read on your papers, I thought all nodes in WM would automatically be in the graph, and thus eligible for being sent to as a goal - I checked that all nodes returned from get3DMap() are in WM.

2) How can I send any node as a goal via /rtabmap/set_goal? Surrogate question: how to make sure every node is properly connected so set_goal won't complain about it?

3) What do you mean with "nodes in different maps"? In my case, the environment was mapped in a single mapping session, so I assume there is only one map, correct?

[matlabbe]

Hi,

1) When I say "Nodes in the graph", they are all nodes in WM or LTM that are directly or indirectly linked to latest node. Nodes that are not in the graph are those saved for debugging purpose but not added to graph because the robot didn't move for example. Nodes from previous sessions that are not linked to current map are also not in the current graph. In the update I did in the previous post, if you have only one session, all nodes sent by get3DMap are valid to send goal on.

2) You can send a goal to any node contained in /rtabmap/mapGraph topic.

3) Yes, you have only one map, so all nodes returned by get3DMap should now (see Edit previous post) be valid to send a goal on.

cheers,
Mathieu

[felipeduque]

Thanks, Matthieu! Now I can send any node as a goal.