Create new Costmap Layer for the FormationPlanning

[Heenne]

Create new costmap layer that combines the costmap of the robots that are participating in the formation
The layer should prevent that the obstacle detection of the global costmap detects the robots and inserts these as obstacles.

Idea:
Take all costmaps and allways choose the lowest cost for the formation costmap. This way only the intersection of the three robot obstacles remains. Then clear the footprint of the formation.
That should be the costmap that can be used and published again

[Heenne]

The Costmap Layer also should publish the footprint of the formation