Fix costmap publishing frequency problem