Independent tests of localization and navigation?

[banerjs]

What's the recommended way of testing navigation and localization independently on Flatland?

For context, I'm trying to tune move_base parameters using flatland. However, when I run flatland at >10x real time, cartographer (I'm testing cartographer as a replacement to AMCL) loses its ability to localize correctly on the map (likely due to asynchronous optimizations and updates within cartographer). I'd like to be able to test the effects of various parameter values in move_base independent of cartographer's issues. Is there a way, without starting flatland_viz, to get the map, the odom, and the transforms from map to base_link from flatland?

[josephduchesne]

I think you're going to want to have two TF trees that both originate from the default map frame, then compare them.

• one generated by the diff drive plugin (with randomly generated noise) provided odom, or by move base via odometry/filtered and corrected by cartographer standing in for AMCL. I think the standard for this is map->odom provided by AMCL and odom->base link provided by move base/flatland.
• One generated by the model TF publisher (acting as "ground truth").

rqt_plot or custom python+matplotlib graphs can be useful, as well as rviz to see when localization diverges from expected ground truth positions.

I'm not sure what is causing the desync at 10x real time. Flatland is fairly synchronous: it uses a single thread for all updates, with the laser plugin parallelizing laser scans in a recent update, but still in the same synchronous plugin update time slot as it iterates through all plugin updates.

It might also make sense to extend the "bad odom" simulation in diff drive to better emulate real wheel slip. The existing noise model just adds Gaussian noise and may or may not provide a good test case for what you're testing. I'd imagine that slip in the direction of acceleration/jerk of each wheel might be a better model.

Internally at Avidbots we use real world bag data for this purpose as well as simulations so that we can get a better idea of how things react in the real world, but I believe that runs into many of the constraints you seem to be hitting.

In my (limited) experience with Cartographer, it didn't work very well if it got behind processing wise. It might be good to try at lower speeds, but in parallel (docker containerize and run multiple concurrent slower simulations rather than one faster one). We have used servers with low speed (2-2.5Ghz) but high thread (32+) cpus to great effect for similar purposes to what you're trying to do.

[banerjs]

Oops. Forgot to close the issue after you responded; sorry! Your explanation makes sense, thanks!

I guess I wasn't clear on the desync - it's on cartographer's end. It keeps a queue of optimizations it should do when localizing and that appears to be computing resource constrained rather than /clock constrained. So running flatland at high frequencies is filling up the queue more quickly than cartographer can process it (I think).

Running at lower frequencies is probably our best option in the short term :)