Adding velocity costmaps to DWA planner

[cdondrup]

Velocity costmaps are standard nav_msgs/OccupancyGrid messages which are usually used for metric maps and costmaps. In this case, velocity costmaps work in polar space and assign costs to linear/angular velocity combinations based on the value of the corresponding pixel of that map. See https://www.youtube.com/watch?v=evWTA8FcYJo for explanation.

This listens to /velocity_costmap_server/map for the costmap. This is currently hard-coded but will be made a parameter. If no map is published, 0 costs are assigned; Does therefore not change the defualt behaviour. The weight of the velocity costmaps costs can be adjusted dynamically with velmaps_scale.

This also publishes a new occupancy map on /move_base/velocity_costmap visualising the velocity costmap including the sampled trajectories as coloured points.

Tested in simulation. Will test on Linda and might include minor improvements. No merge yet please.

[cdondrup]

If no one objects I will merge this because it does not change the default behaviour. Only if there is someone publishing on the right topic, this becomes true and the costs for the trajectory are changed according to the map. If there is no map published for 2 seconds, the map info is reset and therefore the last published map is ignored. Hence, I believe this is harmless.

[marc-hanheide]

retest this please

[marc-hanheide]

@cdondrup will you merge?