From a9960190ab82dea823748ed62febae9ae150b349 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Thu, 4 Apr 2024 13:39:42 +0100 Subject: [PATCH] gps_global_origin: remove LLA to ECEF conversion gps_global_origin is being published as geographic_msgs::GeoPointStamped message, which wants LLA format https://docs.ros.org/en/api/geographic_msgs/html/msg/GeoPointStamped.html FIX https://github.com/mavlink/mavros/issues/1381 Signed-off-by: Beniamino Pozzan --- mavros/src/plugins/global_position.cpp | 17 +---------------- 1 file changed, 1 insertion(+), 16 deletions(-) diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp index c8e35f395..d8ef442d3 100644 --- a/mavros/src/plugins/global_position.cpp +++ b/mavros/src/plugins/global_position.cpp @@ -233,22 +233,7 @@ class GlobalPositionPlugin : public plugin::PluginBase { 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); - } - catch (const std::exception& e) { - ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl); - } + gp_global_origin_pub.publish(g_origin); } /** @todo Handler for GLOBAL_POSITION_INT_COV */