planning cost map now takes into account obstacles