Create a cost map

[cdondrup]

Create a cost map for the human-aware navigation.
Maybe a potential field approach might also be applicable.