posegraph_viewer died

[ResearcherYan]

Hi, there. I think there is a bug in the posegraph_viewer node. When I subscribe to the topic /fw01/kimera_distributed/posegraph_viewer/loop_edges (fw01 is robot 0's name) in rviz, posegraph_viewer dies everytime when inter-robot loop closures are detected. I added some debug code into the pose_graph_tools/src/visualizer.cpp to check what's happening.

Debugging code

First, I output all the nodes in its received pose graph.

Then, I output all the loop closure edges in Visualizer::visualize().

Lastly, in the position parser function Visualizer::getPositionFromKey, if a position is successfully parsed, it will be printed.

posegraph_viewer throws std::out_of_range

Then I launch kimera-multi with two robots named fw01 and zhan66. The fw01 frontend window's output is shown below.

When no loop closure is detected, the posegraph_viewer is still alive. But when 11 inter-robot loop closures are detected, it dies quickly. According to the debugging code above, the code throws std::out_of_range when it tries to call getPositionFromKey(robot2, key2). Specifically, the error comes from this line.

According to the terminal output, it shows that Robot0: 0, Robot1: 1, Key0: 1, Key1: 6. And I guess it died because robot 0 didn't receive any node of robot 1. The former terminal output confirms that hypothesis, since there is no such message like Successfully added node for robot 1, key xxx., only Successfully added node for robot 0, key xxx.

And the rqt_graph further proves my opinion. It seems that fw01's posegrapher_viewer only subscribes to its own pose_graph. No wonder it reports std::out_of_range when it tries to visit another robot's position.

My question

According to the analysis I made, I want to ask why robot 0's posegraph_viewer doesn't hear from robot 1. It makes sense to listen to another robot to successfully publish the loop clousre edge.

Looking forward to your generous reply. Thanks a lot!