ROS2 frame_id field fix

[Z4nna]

We misunderstood the frame_id field is std_msgs/header ROS2 message: it should be an id of the coordinate system reference that is being used. Implement this in the whole code. The possible reference frames are: "ECI" (Earth Centered Inertial frame), "ECEF" (Earth-Centered Earth-Fixed frame), "NED" (North-East-Down coordinate system), "RBF" (Rov Body Frame), as described in "Thor I. Fossen. - Handbook of Marine Craft Hydrodynamics and Motion Control".