Add script to run benchmarks

[ivanpauno]

Related to #35.

Summary

Adds a script that allows running a rosbag repeatedly, configuring amcl with different number of particles.
It records another bagfile with the result, the results of timem, and optionally perf events data.

The missing thing is the postprocessing steps.

qq: does the current rosbag provide a groundtruth topic?
We will need that to use something a tool like evo.

Checklist

• Read the contributing guidelines.
• Configured pre-commit and ran colcon test locally.
• Signed all commits for DCO.
• Added tests (regression tests for bugs, coverage of new code for features).
• Updated documentation (as needed).
• Checked that CI is passing.

[nahueespinosa]

@ivanpauno Left some comments!

does the current rosbag provide a groundtruth topic?

I didn't record an extra topic for ground truth since odom-to-base_link is the ground truth in the perfect_odometry bagfile, but +1 to having ground truth in bagfiles. Note that for this case, in evo you can compare odom-to-base_link and map-to-base_link.

[ivanpauno]

in the perfect_odometry bagfile

Lol, I didn't pay attention to the name of the bag haha.
Ok, that's enough to get evo to work.

[ivanpauno]

Note that for this case, in evo you can compare odom-to-base_link and map-to-base_link.

I tried to make some progress on this.
The problem is that evo doesn't support tf topic for ROS 2, it only does for ROS 1.
I tried to convert the bag using rosbags which doesn't require ROS 1 installed.
The problem is that evo does when using the rosbag reader.

It seems that the easy solution is to use a bag with a ground truth topic (or check how to add tf support to evo).

[olmerg]

maybe we can use the Odometry message ground_thruth_topic which generate the flatland plugin to obtain the ground truth, which is suuported by evo

[ivanpauno]

maybe we can use the Odometry message ground_thruth_topic which generate the flatland plugin to obtain the ground truth, which is suuported by evo

Yes, I think that's the easier thing to do, I will record a new bag that includes the odometry topic.

[ivanpauno]

Results:
benchmark.zip

The folder benchmark was compiled in Release mode.
The folder benchmark with profiling was compiled using RelWithDebInfo and -fno-omit-frame-pointer.

I'm failing to generate results for nav2_amcl, for some reason it's segfaulting now ......

To see the timem results, use:

/ws/src/scripts/benchmarking/timem_metrics_results /path/to/folder/with/timem/json/output

To get ape metrics:

evo_ape bag2 benchmark_1000_particles_output/rosbag/ /odometry/ground_truth /pose

[ivanpauno]

The issue I'm running into is ros-navigation/navigation2#3311.
I think it was fixed for rolling (see here), but it seems it wasn't backported to humble.

[ivanpauno]

I was finally able to record the same cases for nav2 (it doesn't seem to fail if commenting out the recovery_alpha_fast/recovery_alpha_slow, so I did that).

benchmark_nav2.zip

(edit: uploaded new zip, as I forgot to record the /amcl_pose topic)

[ivanpauno]

I run both again with some changes:

• beluga
  • Used covariance 0 for the initial pose, i.e. all initial particles in the same state.
    That's what nav2 does if you use the parameters.
• amcl
  • Changed recovery_alpha_fast/recovery_alpha_slow to the default, i.e. disabled.
    It avoids the crash mentioned above. I cannot disable this in beluga, but I think it shouldn't affect much the comparison.
  • Run tests again without using google meet at the same time 😄

benchmark.zip

I'm working in an script to easily compare both results

[hidmic]

I don't think it's useful for the comparison

Not for a comparison, but I wonder why the APE maximum is consistently higher for beluga. Is it a transient that is quickly corrected? Is it a larger variance in the distribution (ie. beluga being less consistent in its performance than nav2_amcl)?

[ivanpauno]

Not for a comparison, but I wonder why the APE maximum is consistently higher for beluga. Is it a transient that is quickly corrected? Is it a larger variance in the distribution (ie. beluga being less consistent in its performance than nav2_amcl)?

Yes, I guessed it was that.
I will probably add that in another PR to limit the scope of this one, but I think it's a good idea to have that available as well.

[ivanpauno]

I will probably add that in another PR to limit the scope of this one, but I think it's a good idea to have that available as well.

I see that's already provided by evo, see 1a50027.

---

The maximum APE error seems to happen just before the bagfile ends (i.e. before shutdown).

It happens pretty consistently for beluga, and not for nav2_amcl.
It doesn't seem to be something to worry much about ...

[nahueespinosa]

@ivanpauno I have only minor comments.

[nahueespinosa]

@ivanpauno LGTM!