Fix no update bug
Extra accessor method
Working layered costmap