Add option to map colors on OccupancyGrid

[aamirhatim]

The current implementation of the OccupancyGrid class allows the user to specify a single color for the grid. That color is then used to create 3 shades for the map depending on if it is free/occupied/unknown. It would be great if we could specify our own color mapping (something like the colormap attribute for PointCloud2) instead of being constrained to this 3 shade option so we can more clearly see costmaps or other grids that have more information.

[J-Rojas]

We are currently adding two functions to OccupancyGrid. See the #343. This should allow you to create a subclass for colormapping. Feel free to contribute a ColorMapped subclass that supports these options if you do chose to implement it.