Feature/costmap integration

Many changes related to my work on mobile manipulation.

The collision checking is now done initially during onConnectComponent. A costmap will be created and global planning is now based on this. Global planning now only takes ~1ms. However, the creation of the costmap might take several seconds (~10-20s).

Instead of A*, users can now also select the ShortestPathFasterAlgorithm which is applicable if the graph has non-positive edge weights. This might be the case if the distance to the obstacles is considered. The resulting trajectory: