Updated for multi-layer costmaps (hydro navi stack)