Benchmark unicycle_2d state cost function

[efernandez]

This benchmarks the analytic vs automatic differentiation cost functions for the unicycle_2d state motion model implemented in #115, giving the following results:

$ sudo cpupower frequency-set --governor performance
Setting cpu: 0
Setting cpu: 1
Setting cpu: 2
Setting cpu: 3
Setting cpu: 4
Setting cpu: 5
Setting cpu: 6
Setting cpu: 7
Setting cpu: 8
Setting cpu: 9
Setting cpu: 10
Setting cpu: 11

$ rosrun fuse_models benchmark_unicycle_2d_state_cost_function
2019-11-21 11:29:36
Running /home/efernandez/dev/ws/cpr_ws/devel/lib/fuse_models/benchmark_unicycle_2d_state_cost_function
Run on (12 X 2201 MHz CPU s)
CPU Caches:
  L1 Data 32K (x6)
  L1 Instruction 32K (x6)
  L2 Unified 256K (x6)
  L3 Unified 9216K (x1)
Load Average: 0.99, 1.25, 1.38
----------------------------------------------------------------------------------------------------------
Benchmark                                                                Time             CPU   Iterations
----------------------------------------------------------------------------------------------------------
Unicycle2DStateCostFunction/AnalyticUnicycle2DCostFunction             384 ns          384 ns      1587923
Unicycle2DStateCostFunction/AutoDiffUnicycle2DStateCostFunction       1491 ns         1491 ns       472094

refs CORE-15104

For this to get compiled you need to pass -DCATKIN_ENABLE_BENCHMARKING=1 to cmake or catkin --cmake-args.

You also need the benchmark debian installed in your system for this to build successfully. You probably need to add it to your CI buildfarm for the build test to pass in this PR.

The benchmark shows the analytic version is almost 4 times faster than the automatic differentiation one. This is too good to be true, so maybe I'm not testing things properly, i.e maybe some initialization stuff is being benchmark for the automatic differentiation version, but I think I'm testing it right.

We could also try using plain arrays in the analytic version. I'd like to try that and benchmark it, but I'm not sure how to organize the code to do so. Should I create a new class that implements the analytic jacobians computation with plain array, so I have two classes (apart from the automatic differentiation) that I can benchmark simultaneously in the same workspace?

FYI @svwilliams

[efernandez]

Add some comments to guide the review, mostly with some design concerns I had.

[efernandez]

I just fixed all roslint errors but the checks still failed. I suspect it's because the benchmark deb is not available/installed in your buildfarm. @svwilliams Could you past the error here and confirm, please?

[svwilliams]

Cannot locate rosdep definition for [benchmark]

I don't see benchmark in the standard rosdep definition here

[efernandez]

FYI @svwilliams I've created ros/rosdistro#23163 to request adding benchmark to the official rosdep.

[gavanderhoorn]

Should this be a test_depend?

[efernandez]

Good point. IMHO it's good to have the ability to run the benchmark on the robot platforms, so in that sense it's better to have the benchmark executable available not only in a test build. Note that in the CMakeLists.txt I also added an install rule for it.

Either way, this is behind another cmake flag, so it actually doesn't compile by default and it wouldn't be available. -DCATKIN_ENABLE_BENCHMARKING=1 must be passed to cmake or catkin build --cmake-args for that.

That being said, I don't have a strong opinion, so I can change it if you think there's no point on having a benchmark generally available.

[gavanderhoorn]

Either way, this is behind another cmake flag, so it actually doesn't compile by default and it wouldn't be available. -DCATKIN_ENABLE_BENCHMARKING=1 must be passed to cmake or catkin build --cmake-args for that.

true, but the dependency is installed regardless. Seems strange to do that if DCATKIN_ENABLE_BENCHMARKING isn't also set by default.

The contributed rosdep rule only adds a mapping for Ubuntu, so on other systems users either need to skip-keys or edit the manifest to make rosdep happy.

In my experience benchmarks are considered part of the tests, and those don't typically get built nor installed. Unless the user asks for them (by setting an option to true or on fi).

[ayrton04]

IMHO it's good to have the ability to run the benchmark on the robot platforms, so in that sense it's better to have the benchmark executable available not only in a test build.

So it sounds like this isn't being built from source on the robot platform in question, or you could just put the new CATKIN_ENABLE_BENCHMARKING block inside the CATKIN_ENABLE_TESTING block, I think.

Would expanding the rosdep platforms for the dependency address your concern, @gavanderhoorn?

[gavanderhoorn]

you could just put the new CATKIN_ENABLE_BENCHMARKING block inside the CATKIN_ENABLE_TESTING block

That would indeed perhaps be the best thing to do.

Would expanding the rosdep platforms for the dependency address your concern,

As we don't have optional dependencies in ROS package manifests, anything you add will become a required dependency. I'm not too worried about this one in particular, but it will make setting up a build environment for fuse a little more complex.

[efernandez]

I see your concerns. I think in brief you're asking to keep things simply, so what about:

1. Change <depend> into <test_depend> for benchmark
2. Remove the CATKIN_ENABLE_BENCHMARKING block and move the benchmark inside CATKIN_ENABLE_TESTING. It won't run, but it'll get build in that case.
3. With the benchmark inside the CATKIN_ENABLE_TESTING block, I can remove the install rule for the benchmark. I should be able to find where the executable ends up, so there must be a way to call it.

This means that in order to run the benchmark on a robot you'll have to:

1. Create a workspace with fuse
2. Run catkin build in order to get the build/fuse_models folder. Not sure if there's a faster way.
3. Go into the build/fuse_models folder and from there there should be a make target for the benchmark, that we can build alone
4. Finally, we should be able to run the benchmark

If you agree on this, I can make the changes to unblock this PR. 😃

[svwilliams]

Works for me. The executable should live in: {catkin_workspace}/devel/lib/fuse_models
At least if you are using catkin build.

[efernandez]

Updated. Now:

1. There's a <test_depend> on benchmark as opposed to a <depend>
2. I updated the comment in the package.xml with the link to the existing PR in the official rosdistro, which has been approved but not merged yet.

• I'll remove that entire comment and uncomment the <test_depend> when that gets merged, although it also means you need to sync the rosdistro in your buildfarm for it to work.

• This will make the benchmark compile, but it won't run as a test. I guess we could consider that this way we are at least building the benchmark, i.e. testing it builds, which is better than nothing.

3. I removed the CATKIN_ENABLE_BENCHMARKING block and moved the benchmark inside (at the botom) the CATKIN_ENABLE_TESTING block.

• However, catkin build seems to default to CATKIN_ENABLE_TESTING=ON, so it tries to build the benchmark. Note that because it's not a gtest, the benchmark target is built immediately. There's no need for make run_tests. For this reason, if the benchmark package is NOT available in the system it'd fail to build.

4. To solve that issue, I made the find_package on benchmark not REQUIRED (now is QUIET), and the benchmark is built only if benchmark_FOUND is set to TRUE.

I tested this by removing the pkgconfig and cmake benchmark stuff from my system, so the benchmark package could NOT be found. It worked:

• No cmake errors/warnings because benchmark wasn't found, thanks to the QUIET mode in find_package
• The benchmark wasn't built

Then I brought the removed files back, touched the CMakeLists.txt file to force a re-build and catkin build again. After that, the benchmark was built. The resulting executable goes here (one is a symlink):

~/ws/devel$ find | grep benchmark
./lib/fuse_models/benchmark_unicycle_2d_state_cost_function
./.private/fuse_models/lib/fuse_models/benchmark_unicycle_2d_state_cost_function

And it can be run with rosrun fuse_models benchmark_unicycle_2d_state_cost_function.