Map server various improvements (see description)

[baronep]

This PR does the following ...

1. Removes a dependency on the Bullet library which was only included to do a trivial quaternion calculation. Instead, perform that calculation manually.
2. General cleanup and c++ modernization. Move away from raw pointers (as much as possible, still required for SDL library, use auto where appropriate, etc)
3. Removing dead code.
4. Removing (probably) unnecessary code. Deprecated (for ~10 years) command line interface and backwards compatibility with YAML 0.3.0 (released and replaced in 2012)
5. Removed MACRO and replaced with function (modern compilers will inline this anyways).
6. Improved locality of variable declarations (https://stackoverflow.com/a/3773458)

[mikeferguson]

Just taking a quick look at this, I think we should actually plan to target this at noetic (it's a little late in the release cycle to make such drastic changes in an existing LTS release like Melodic). Full review forthcoming.

[baronep]

@mikeferguson Makes sense to me. Let me know if you would like me to make any changes and/or split up this PR into multiple PRs.

[baronep]

@mikeferguson Unfortunately, it looks like the majority of the integration tests still use the deprecated map_server CLI and I don't think there is an easy way to port to the new API since the test framework relies on the roslaunch variables to get the path to the downloaded maps. Should the old API be preserved or do you have any other ideas for workarounds?

[baronep]

One alternative could be to make the map_server use rosparams instead of loading a yaml directly. Then the unit tests could manually specify the rosparams in the roslaunch file using the CATKIN env vars, and normal use would involve loading the existing YAML files into a map_server param namesapce. This would be a major API change though

[mikeferguson]

@baronep looking at the amcl/basic_localization_test the map files get downloaded into the test directory (See the CMakeListst - it uses catkin_download_test_data). Therefore, you should be able to put a yaml file in the test directory with the name of the file that will be downloaded and the other metadata, and then update the args so it finds the yaml file instead of the pgm/map file - no changes needed for map server.

(EDIT: honestly, I'm surprised someone bothered to download the map file using the catkin macros - I mean really, how big of a file is that? Usually that macro is really only used for bag files, which almost always are tens to hundreds of megabytes).

[baronep]

With the following YAML file in amcl/test

image: willow-full.pgm
resolution: 0.1
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0

I get the following error ...

[ERROR] [1585510555.699087338]: failed to open image file "/home/pbarone/catkin_ws/src/navigation/amcl/test/willow-full.pgm": Couldn't open /home/pbarone/catkin_ws/src/navigation/amcl/test/willow-full.pgm

However, if I use ...

image: /home/pbarone/catkin_ws/devel/share/amcl/test/willow-full.pgm

then everything resolves correctly. The files actually get downloaded to devel/share/amcl/test instead of src/amcl/test

[baronep]

After a little more investigation $(find amcl) does a little bit of magic which allows it to resolve files in any of these directories ...

pbarone@pbarone-t460p:~/catkin_ws$ catkin_find amcl
/home/pbarone/catkin_ws/devel/include/amcl
/home/pbarone/catkin_ws/devel/lib/amcl
/home/pbarone/catkin_ws/devel/share/amcl
/home/pbarone/catkin_ws/src/navigation/amcl

As an aside ... Just to mock up what the rosparam idea would look like ... definitely a more modern ROS interface (at least in my experience). BUT I do understand that changing the interface comes with it's own pain obviously

    <node name="map_server" pkg="map_server" type="map_server">
      <param name="image" value="$(find amcl)/test/willow-full-0.05.pgm"/>
      <param name="resolution" value="0.05"/>
      <param name="origin" value="[0.0, 0.0, 0.0]"/>
      <param name="occupied_thresh" value="0.65"/>
      <param name="free_thresh" value="0.196"/>
      <param name="negate" value="0"/>
    </node>

OR ...

    <node name="map_server" pkg="map_server" type="map_server">
      <rosparam command="load" file="$(find amcl)/test/willow-full-0.05.yaml" />
    </node>

[EDIT] - We could also do a combination where the node supports rosparams AND the old style command line args

[EDIT 2] - Or the pgm's could be checked in to the tests/ directory