Bringing back coloured octrees

[wxmerkt]

This PR adds the option to compile OctomapServer to use available RGB information and save it in a ColorOcTree :rainbow:. Hereto, in a first step the OctomapServer was generalised using typedefs so that a compile-time switch (define COLOR_OCTOMAP_SERVER) changes the required types throughout.

As the define statement is not set, it will not change default behaviour but will allow people who wish to have coloured maps to compile octomap_server with them enabled. Having octomap_server compiled with colours enabled, the parameter colored_map needs to be set to true in the launch file (and the height_map switch removed or set to false).

I will continue to work in this area and add stand-alone nodes at a later stage.

[ahornung]

Thanks for your contribution! How do you propose to proceed? Do you want your PRs merged bit by bit or as a whole at the end? Wouldn't it be a good idea to create a new branch for the color octrees and merge later then?

Am I understanding you right that in the long run there's no more compile-time switch between the different variants (color and regular occupancy), but different classes (using the node-traversal hooks, see OctomapServerMultilayer)? I think this should be the preferred (and most clean) way.

If we're talking about a larger change, I was also thinking about having the OctomapServer class templated about the underlying octree structure. The lightweight node classes could then instantiate e.g. OctomapServer<OcTree> or OctomapServer<ColoredOcTree>, with function pointers to the correct node integrations for each. I never got the time to implement that larger change, however.

[wxmerkt]

If it's fine with you, I'd propose to have this PR merged in right now with users that compile themselves able to set the flag before compilation. My other changes might take some time to be put together, tested thoroughly and cleaned up (~few weeks down the road). Very happy to create a new branch for color octrees as well.

Yes, in the long-run I am aiming at a templated function. I am busy with testing other parts and integration right now and plan to revisit the generalisation/clean up bit at a later stage (likely at the same time as switching to insertPointCloud to speed things up and having an asynchronous/threaded publishAll ).

[ahornung]

Alright, sounds like a good plan!

[ahornung]

New branch for to future color updates (until finalized to be merge back): indigo-devel-color