Add footprint subscriber back into costmap2dros
Use PolygonStamped, fix node handles