costmap_2d: now publishing footprint as a PolygonStamped frequently