[global_position] 'gp_origin' and 'set_gp_origin' mismatching

[standmit]

Issue details

I found some mismatching in global_position plugin. When I send coordinates (LLA) to topic mavros/global_position/set_gp_origin, the plugin accepts this coordinates as LLA and sends it to FCU as is.

gpo.latitude = req->position.latitude * 1E7;
gpo.longitude = req->position.longitude * 1E7;
gpo.altitude = req->position.altitude * 1E3 + m_uas->ellipsoid_to_geoid_height(&req->position);

UAS_FCU(m_uas)->send_message_ignore_drop(gpo);

Then FCU confirms this with GPS_GLOBAL_ORIGIN message containing the same coordinates as LLA. But plugin transforms it to ECEF and sends to topic mavros/global_position/gp_origin.

g_origin->position.latitude = glob_orig.latitude / 1E7;
g_origin->position.longitude = glob_orig.longitude / 1E7;
g_origin->position.altitude = glob_orig.altitude / 1E3 + m_uas->geoid_to_ellipsoid_height(&g_origin->position); // convert height amsl to height above the ellipsoid

try {
    /**
     * @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed)
     * Note: "earth" frame, in ECEF, of the global origin
     */
    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
    earth.Forward(g_origin->position.latitude, g_origin->position.longitude, g_origin->position.altitude,
            g_origin->position.latitude, g_origin->position.longitude, g_origin->position.altitude);

    gp_global_origin_pub.publish(g_origin);
}

Topic mavros/global_position/gp_origin has type geographic_msgs/GeoPointStamped and must contain coordinates in LLA. When I send a request containing LLA, I expect to receive a response containing LLA too in order to compare it and ensure that global_origin has been successfully set. Why global_position plugin transforms global_origin to ECEF?

MAVROS version and platform

Mavros: 1.0.0 ROS: Melodic Ubuntu: 18.04

Autopilot type and version

[ X ] ArduPilot [ ] PX4

Version: ArduCopter V4.0.0-dev (8c413d3c)

[TSC21]

@standmit you have the answer for that in https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/global_position.cpp#L237 and in https://www.ros.org/reps/rep-0105.html#id17. If you want a topic that just serves as feedback, you can always add it and propose a PR.

[Alefunk]

I'm sorry to dig up this 2 year old issue, but i agree with @standmit regarding the confusing message type and i believe @TSC21 might have misunderstood the intention of the issue. It is highly counter intuitive to send ECEF coordinates in a ROS message, that is only intended to be used with LLA coordinates: http://docs.ros.org/en/noetic/api/geographic_msgs/html/msg/GeoPoint.html

Either a different message type should be choosen or the ECEF transformation should be removed.

Edit: By having a deeper look into the code and Rep 105 I personally think that this comment https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/global_position.cpp#L70 is not valid. The frame_id just specifies the reference point of the coordinates that are transmitted in the message, which is identical in case of ECEF and WGS84 LLA coordinates, as also depicted in the attached image in Rep 105. Setting the frame_id to "earth" gives no indication of the coordinate type (if cartesian or spherical). The coordinate type itself is determined by the variables that are transmitted in the message (e.g. lat, lon, alt). That's why I would suggest to remove the ECEF transformation completely, instead of changing the message type.

[lopsided98]

I just ran into this as well and agree that it is very confusing and probably wrong to use ECEF in a GeoPoint. I think that the transformation should be removed to match both set_gp_origin and MAVLink itself. An alternative could be to keep ECEF and switch the type to PointStamped. Both of these would break compatibility, so maybe a new topic should be created.

This topic has recently become important for ArduPilot, which now defines defines its local coordinate system relative to the EKF origin (published in the GPS_GLOBAL_ORIGIN message), rather than the home position (see https://github.com/ArduPilot/ardupilot/pull/15169).

[haavarpb]

I agree, the current implementation is rather misleading. Expecting LLA, when it's really ECEF surely will make something break at some point.