added ability to update costmap with new paths