Add a generic function to fill the geometry_msgs