Slam_toolbox crashes when loading a serialised map

[abylikhsanov]

Required Info:

• Operating System:
  • Ubuntu 18.04
• Installation type:
  • Source
• ROS Version
  • ROS melodic
• Version or commit hash:
  • Melodic latest

Steps to reproduce issue

During async_slam, I have created a small node that calls /slam_toolbox/serialize service every 5 seconds:

int main(int argc, char** argv)
{
    ros::init(argc, argv, "position_publisher");
    ros::NodeHandle nh;

    sleep(5);

    ros::Rate rate(0.2);

    ros::ServiceClient service_client = nh.serviceClient<slam_toolbox::SerializePoseGraph>("/slam_toolbox/serialize_map");
    slam_toolbox::SerializePoseGraph serialize;
    serialize.request.filename = "/media/psf/Home/Desktop/master-wheel/ros_ws/src/slam_toolbox/config/posegraph";

    while (ros::ok())
    {
        service_client.call(serialize);
        //if (service_client.call(serialize)) ROS_INFO("Pose Graph has been saved");
        //else ROS_INFO("ERROR: Could not save serialize pose graph");
        rate.sleep();
    }

}

Interestingly, it only works once and does not call the service again after rate.sleep(), even though it should.

When I then start a localization node with loading a saved serialized map, I get this error (right after getting a first scan):

[ INFO] [1583670131.651204177]: Got scan!
WARNING: Logging before InitGoogleLogging() is written to STDERR
F0308 13:22:11.652395 22328 problem_impl.cc:572] Parameter block not found: 0x5567da4a9a80. You must add the parameter block to the problem before it can be set constant.
*** Check failure stack trace: ***

Expected behavior

The service should be called every 5 seconds from the simple code above.
The crash should not happen after 1st scan

Actual behavior

Code from the above calls the service only once and only once the map is saved.
The crash appears after starting a localization node with a map above.

Additional information

[SteveMacenski]

right after getting a first scan

Does this only happen when you do it right after the first scan? I'd merge a PR that keeps a boolean around to check if its processed once before serialization. That is a strict requirement due to some performance optimizations and is why that's happening I suspect.

I understand why that's happening - I never expected someone would want to serialize the file on that frequent of a basis since serialization blocks the SLAM process and takes a bunch of memory. You may expect crashes if your system doesn't have alot of staticly available memory since serialization will require all that data to be on the stack and if you don't have enough memory available, it's going to be a bad time. I'd recommend backing this off to every few minutes, not seconds.

Have you tried this after waiting for a few scans to enter in and the optimizer to be called? If it works after that point, then we know the issue with your use. Also, make sure that you're using the 18.04 rosdep Ceres install. If you build it from source or otherwise have built/installed another version (as cartographer may require), I will not support that. There are various differences between them and I don't have the cycles to debug anything but the supported versions.

The crash appears after starting a localization node with a map above.

I'm not sure I understand this comment. There is no serialization available in localization mode.

Interestingly, it only works once and does not call the service again after rate.sleep(), even though it should.
Code from the above calls the service only once and only once the map is saved.

I don't understand this request. Are you asking me to debug your code?

[abylikhsanov]

Yes, it crashes right after getting a first scan. Are you saying that during SLAM whenever I call a service to serialize my current map, there is a chance that the service can serialize without the scan being processed and whenever I load a pose graph and the node deserializes it crashes?

I am not planning to serialize a map every 5 seconds rather it was just a test but I am planning to do so once every 1-2 minutes as the environment is quite dynamic and I want to keep my map up to date automatically. Yes, the localization mode cannot serialize a map by default but I just added this feature to always save the latest map in a dynamic environment (like warehouses).

I don't understand this request. Are you asking me to debug your code?
No, I just gave an example, you can ignore it.

[SteveMacenski]

Are you saying that during SLAM whenever I call a service to serialize my current map, there is a chance that the service can serialize without the scan being processed and whenever I load a pose graph and the node deserializes it crashes?

This sentence doesn't make sense. Please rephrase into multiple sentences and around a single idea.

What I think your seeing is the following:

• Scan goes into SLAM
• SLAM processes and creates nodes in graph
• graph sits around
• Enough nodes come in and a loop closure is triggered
• graph is optimized and the first time we optimize a map, we setup some stuff that is used in all further situations.

But since you're serializing before that graph optimization occurs, you're not setting up that stuff, and then things are failing. Did you try, as I suggest above?

Have you tried this after waiting for a few scans to enter in and the optimizer to be called

If it works after that point, that's root cause and then you can submit a PR:

I'd merge a PR that keeps a boolean around to check if its processed once before serialization.

and just return failed if not able to serialize. There's really no value in a serialized map before a graph optimization has been run anyhow.

[abylikhsanov]

Ok, I think I get it now. Please correct me if I am wrong, whenever optimization occurs, karto does some sort of a correction (if needed) to the robot pose and because at the very beginning, my robot did not move (which is required by your code in order to make processScan to return True), no optimization occurred and I was saving incomplete pose graph.

So If I make sure that whenever someone requests to serialize a map during SLAM and the scan has not been processed, the service simply rejects a request.

If that is correct then the PR would be quite simple with just a few lines of code which I can submit

[SteveMacenski]

and I was saving incomplete pose graph.

Correct up to this point. It is complete. Again, there are optimizations afoot. Don’t worry about the specifics but you can’t save a pose graph without at least 1 loop closure attempt in the optimizer.

[abylikhsanov]

#182

[abylikhsanov]

Closing this issue as the problem root has been determined and a possible solution is being suggested (pull request above)