Map loading and Transform/Odom topic refactoring

[TirelessDev]

This PR covers two changes to the hdl_graph_slam master.

• Introduces an initial first pass of tying together the loading functionality of g2o and graph_slam within a service request endpoint.
• Refactors the code slightly so that published topic frames can be easier defined in the launch files.

To the first point. We have added a load service endpoint that takes a string of the path to a map previously dumped using the dump endpoint. This is a folder that contains pose graph information and keyframes.

{
  "path": "/tmp/dumped/map"
}

This loading should be performed on a fresh start of hdl_graph_slam, and before new point clouds are received. After loading new point clouds will be used to extend the loaded map.

Of particular importance to know is that the current odometry information from the previous map generation process is not retained, ie where it figured the Velodyne frame was within the Map frame. This means that after starting a new instance of hdl_graph_slam and after loading a previous map the "current estimated position" of the Velodyne will be at the origin. This is super important because if point clouds don't then arrive and describe the same local environment, the registration to the existing map will be incorrect. There may be ways to help alleviate this problem, perhaps by providing an initial position estimate to update the internal odom estimate. However, this is a topic for future work.

An example of the std output from the loading service call.

loading data from:/tmp/dumped/map
loading pose graph...
nodes  : 11
edges  : 10
kernels: 0
loaded 9 keyframes
loaded special nodes - anchor_node: 1 anchor_edge: -2 floor_node: 8
snapshot updated
loading successful

With respect to the second lot of changes, to the topic subscriptions. These basically involved some minor modifications to the nodelet code to allow additional parameters to be passed into the node to configure the transform frame names. Basically, just to remove hardcoded values like "/odom" so we can avoid conflicts when integrating with other systems on the robot.

[koide3]

Thanks a lot for the nice PR. I pushed a fix for a problem on the CI that wrongly failed. Could you merge master branch to re-run the CI?

The PR looks nice, and I'm going to merge it with slight modifications. But, please note that my review would be slow because I'm a bit busy at this moment for writing a paper...

[koide3]

Sorry for the very late response. The PR looks fine, but I would like to request to revert a few points to keep the default behavior unchanged because some changes can affect the systems of existing users.

[marcelinomalmeidan]

@koide3, I might be interested in these changes, so I can wrap up the work of merging this PR. Is this something that still interests you?
I was looking at the code in here, and I am wondering if you have any reasoning of what the anchor_edge means when we save or load a map. It seems that every edge that we create has an id = -2 (I added prints to my code), so it doesn't seem to be relevant for anything. Would you have a different interpretation for it?

[TirelessDev]

Hey guys, apologies for neglecting this. I moved into a different role and didn't have time to continue working on it / totally forgot. My bad :).

Though it does work, this PR was more a PoC than a serious attempt at getting it working robustly. My understanding at the time was somewhat surface level, so there is certainly more that can be done to get map reuse working to a level where it would be useful in practice. From memory, some helper processes to aid in roughly aligning the map on load would go a long way. The kidnapped robot problem is still real.

I had renamed the topics to add clarity for our use context and remove contentions with other systems we were running ( We used this on a Boston Dynamics Spot in conjunction with a number of other solutions ). However, I agree it makes sense to change back.

I don't have the capability to test this anymore, but I can make the requested changes and push a new commit to the PR.

[marcelinomalmeidan]

Hey @TirelessDev, thanks for coming back and making some new commits. Let me know when you're done with new commits, I can test them for you with some test bags.
And you did clarify the question on my mind, as this isn't solving the kidnapped robot problem yet. Still, it is a good step in the right direction. I will see if I can find some time to work on that once this PR finally merges

[TirelessDev]

@marcelinomalmeidan Not a prob; it should be up to date with master now and have those default names reverted.

Adding the extra /hdl_graph_slam prefix to the topics was simply a compatibility thing. I was testing numerous slam solutions at the same time. HDL graph slam was my favourite :)

Yeah, as mentioned in the PR, the robot needs to be in a very similar position and orientation when booting up for the map to converge correctly. Practically this wasn't an issue for us, as Spot always returned to the same spot to charge. However, one of the interesting drivers for map reuse for us was actually map sharing and map collaboration. While finding kidnapped robots in SLAM maps is an entire research topic, there are practical ways that the problem can be worked around. Having solid anchors ( QR codes ) in the map to help localise and provide an initial pose correction would probably be the easiest. However, the map as it stands would need to be extended to support this sort of anchor. Alternatively, such behaviour could be sideloaded to a different service quite easily.

So much potential, so little time.

[marcelinomalmeidan]

nit: this repo is mostly using camel_case for its variables, so I'd recommend updating maxDirectoryCount and keyframeDir to camel case

[TirelessDev]

Good suggestion, have updated now

[marcelinomalmeidan]

The new solution worked to me! It seems good to go, IMO
Unfortunately, I can't approve this PR, but hopefully we'll hear soon from @koide3

[koide3]

Wow, thanks guys for your work! I'll make time to review and approve the PR in a few days.

[marcelinomalmeidan]

there is a minor issue in this line. If either anchor_node, anchor_edge or floor_plane_node is a null pointer, then we cannot print ->id() for them (segfault). This happens if any of them is -1 in the loaded CSV file, and it happened to me.
I'd suggest only printing when they're not null pointers. I think that only anchor_node needs to be non-null to be able to load the graph successfully

[koide3]

The code now looks good to me, and I did a quick test in my environment . I'll merge this PR once the last issue pointed by @marcelinomalmeidan is fixed.

[marcelinomalmeidan]

@TirelessDev ping :-)

[koide3]

Thanks again @TirelessDev for your contribution! I also thank @marcelinomalmeidan for handling this PR!

[TirelessDev]

Not a problem. Sorry for missing that final semicolon. There is always something, looks like you caught it in the merge though.

[marcelinomalmeidan]

@TirelessDev thanks for finalizing the PR!
@koide3 you're very welcome!