Add configurable datum LLH transformation using PROJ

.gitignore

@@ -14,3 +14,4 @@ COLCON_IGNORE
 .catkin_tools
 core
 /tmp
+/prompt.md

fixposition_driver_lib/CMakeLists.txt

@@ -45,7 +45,35 @@ add_library(
   src/params.cpp
 )
 
-target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${fpsdk_common_LIBRARIES} pthread)
+target_link_libraries(${PROJECT_NAME}
+  ${Boost_LIBRARIES}
+  ${fpsdk_common_LIBRARIES}
+  pthread
+)
+
+
+# TESTS =================================================================================================================
+
+# Defines BUILD_TESTING (default ON), calls enable_testing()
+include(CTest)
+
+# Build tests only if GTest is available
+if(BUILD_TESTING)
+  find_package(GTest 1.11.0 QUIET)
+  if(GTest_FOUND)
+    add_executable(${PROJECT_NAME}_llh_trafo_test
+      test/llh_trafo_test.cpp
+    )
+    target_link_libraries(${PROJECT_NAME}_llh_trafo_test
+      PRIVATE
+        ${PROJECT_NAME}
+        GTest::gtest_main
+    )
+    add_test(NAME ${PROJECT_NAME}_llh_trafo_test COMMAND ${PROJECT_NAME}_llh_trafo_test)
+  else()
+    message(STATUS "${PROJECT_NAME}: GTest not found, not building tests")
+  endif()
+endif()
 
 
 # INSTALL ==============================================================================================================

fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in

@@ -1,5 +1,11 @@
 @PACKAGE_INIT@
 
+include(CMakeFindDependencyMacro)
+set(_fixposition_driver_lib_package_prefix_dir "${PACKAGE_PREFIX_DIR}")
+find_dependency(fpsdk_common)
+set(PACKAGE_PREFIX_DIR "${_fixposition_driver_lib_package_prefix_dir}")
+unset(_fixposition_driver_lib_package_prefix_dir)
+
 set(@PROJECT_NAME@_FOUND ON)
 set_and_check(@PROJECT_NAME@_INCLUDE_DIRS "${PACKAGE_PREFIX_DIR}/include")
 set_and_check(@PROJECT_NAME@_LIBRARY_DIRS "${PACKAGE_PREFIX_DIR}/lib")

fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp

@@ -16,6 +16,7 @@
 
 /* LIBC/STL */
 #include <cstdint>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -27,6 +28,7 @@
 #include <fpsdk_common/parser/nmea.hpp>
 #include <fpsdk_common/parser/novb.hpp>
 #include <fpsdk_common/time.hpp>
+#include <fpsdk_common/trafo.hpp>
 
 /* PACKAGE */
 #include "fixposition_driver_lib/params.hpp"
@@ -307,6 +309,35 @@ struct FusionEpochData {
     FusionEpochData CompleteAndReset(const fpsdk::common::parser::fpa::FpaEoePayload& eoe);
 };
 
+#if FPSDK_USE_PROJ
+/**
+ * @brief Initialise ECEF to LLH transformer
+ *
+ * Creates a PROJ transformer (see fpsdk::common::trafo::Transformer) if params.datum_llh_enabled_ is true. The source
+ * CRS must be geocentric. The target CRS must be either geographic 3D, or compound geographic 2D plus vertical.
+ *
+ * @param[in]  params     Driver parameters (datum_llh_...)
+ * @param[out] llh_trafo  The transformer, nullptr if disabled
+ * @param[out] error      Error message, on failure
+ *
+ * @returns true if disabled or if the transformer is valid, false otherwise
+ */
+bool InitLlhTrafo(const DriverParams& params, std::unique_ptr<fpsdk::common::trafo::Transformer>& llh_trafo,
+                  std::string& error);
+
+/**
+ * @brief Convert ECEF to latitude, longitude and height
+ *
+ * @param[in]  llh_trafo  Transformer from InitLlhTrafo(), nullptr to use the built-in WGS84 conversion
+ * @param[in]  ecef       ECEF position [m]
+ * @param[out] llh_rad    Latitude [rad], longitude [rad], height [m]
+ *
+ * @returns true on success, false if the transformation failed
+ */
+bool EcefToLlhRad(const fpsdk::common::trafo::Transformer* llh_trafo, const Eigen::Vector3d& ecef,
+                  Eigen::Vector3d& llh_rad);
+#endif  // FPSDK_USE_PROJ
+
 void HelloWorld();
 
 /* ****************************************************************************************************************** */

fixposition_driver_lib/include/fixposition_driver_lib/params.hpp

@@ -41,6 +41,9 @@ struct DriverParams {
     bool raw_output_ = false;
     bool cov_warning_ = false;
     bool nav2_mode_ = false;
+    bool datum_llh_enabled_ = false;
+    std::string datum_llh_ecef_crs_ = "EPSG:4978";
+    std::string datum_llh_llh_crs_ = "EPSG:4979";
 
     enum class VelTopicType { UNSPECIFIED, TWIST, TWISTWITHCOV, ODOMETRY };
     bool converter_enabled_ = false;

fixposition_driver_lib/src/helper.cpp

@@ -12,10 +12,12 @@
  */
 
 /* LIBC/STL */
+#include <atomic>
 #include <utility>
 
 /* EXTERNAL */
 #include <fpsdk_common/logging.hpp>
+#include <fpsdk_common/math.hpp>
 #include <fpsdk_common/parser/crc.hpp>
 #include <fpsdk_common/string.hpp>
 #include <fpsdk_common/trafo.hpp>
@@ -161,13 +163,12 @@ bool OdometryData::ConvertToEnu(const TfData& tf_ecef_enu0) {
     const Eigen::Matrix<double, 6, 6> cov_ecef = pose.cov;
 
     // Extract data from the TF message (using the arrow operator)
-    const Eigen::Vector3d t_ecef_enu0 = tf_ecef_enu0.translation;
     const Eigen::Quaterniond q_ecef_enu0 = tf_ecef_enu0.rotation;
     const Eigen::Matrix3d rot_ecef_enu0 = q_ecef_enu0.toRotationMatrix();
 
     // Convert position in ECEF into position in ENU
-    const Eigen::Vector3d t_enu_body =
-        fpsdk::common::trafo::TfEnuEcef(t_ecef_body, fpsdk::common::trafo::TfWgs84LlhEcef(t_ecef_enu0));
+    const Eigen::Vector3d wgs84llh_ref = fpsdk::common::trafo::TfWgs84LlhEcef(tf_ecef_enu0.translation);
+    const Eigen::Vector3d t_enu_body = fpsdk::common::trafo::TfEnuEcef(t_ecef_body, wgs84llh_ref);
     const Eigen::Quaterniond q_enu_body = q_ecef_enu0.inverse() * q_ecef_body;
 
     // Convert covariance matrix to ENU
@@ -415,6 +416,63 @@ FusionEpochData FusionEpochData::CompleteAndReset(const fpsdk::common::parser::f
 
 // ---------------------------------------------------------------------------------------------------------------------
 
+#if FPSDK_USE_PROJ
+bool InitLlhTrafo(const DriverParams& params, std::unique_ptr<trafo::Transformer>& llh_trafo, std::string& error) {
+    llh_trafo.reset();
+    error.clear();
+    if (!params.datum_llh_enabled_) {
+        return true;
+    }
+
+    // Normalized output has longitude, latitude and height order regardless of the CRS's native axis order.
+    trafo::TransformerOpts opts;
+    opts.precise_ = true;
+    opts.normalize_axes_ = true;
+    auto new_trafo = std::make_unique<trafo::Transformer>("datum_llh");
+    if (!new_trafo->Init(params.datum_llh_ecef_crs_, params.datum_llh_llh_crs_, opts)) {
+        error = "invalid CRS or transformation unavailable";
+        return false;
+    }
+
+    const auto source = new_trafo->SourceCrs();
+    const auto target = new_trafo->TargetCrs();
+    if (source.type_ != trafo::CrsType::GEOCENTRIC) {
+        error = "source CRS must be geocentric";
+        return false;
+    }
+    const std::vector<trafo::CrsType> geo2d_vertical = {trafo::CrsType::GEOGRAPHIC_2D, trafo::CrsType::VERTICAL};
+    if (!((target.type_ == trafo::CrsType::GEOGRAPHIC_3D) ||
+          ((target.type_ == trafo::CrsType::COMPOUND) && (target.sub_types_ == geo2d_vertical)))) {
+        error = "target CRS must be geographic 3D or geographic 2D plus vertical";
+        return false;
+    }
+
+    llh_trafo = std::move(new_trafo);
+    return true;
+}
+
+bool EcefToLlhRad(const trafo::Transformer* llh_trafo, const Eigen::Vector3d& ecef, Eigen::Vector3d& llh_rad) {
+    if (llh_trafo == nullptr) {
+        llh_rad = trafo::TfWgs84LlhEcef(ecef);
+        return true;
+    }
+
+    Eigen::Vector3d lonlath;
+    if (!llh_trafo->Transform(ecef, lonlath)) {
+        // Report only once
+        static std::atomic<bool> warned = false;
+        if (!warned.exchange(true)) {
+            WARNING("PROJ ECEF to LLH transformation failed");
+        }
+        return false;
+    }
+    llh_rad = {math::DegToRad(lonlath.y()), math::DegToRad(lonlath.x()), lonlath.z()};
+    return true;
+}
+#endif  // FPSDK_USE_PROJ
+
+// ---------------------------------------------------------------------------------------------------------------------
+
 void HelloWorld() {
     NOTICE("Fixposition driver (https://github.com/fixposition/fixposition_driver)");
     NOTICE("Fixposition SDK %s", utils::GetVersionString());

fixposition_driver_lib/test/llh_trafo_test.cpp

@@ -0,0 +1,127 @@
+/**
+ * \verbatim
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
+ * \endverbatim
+ */
+
+/* LIBC/STL */
+#include <memory>
+#include <string>
+#include <thread>
+
+/* EXTERNAL */
+#include <gtest/gtest.h>
+
+#include <fpsdk_common/math.hpp>
+#include <fpsdk_common/trafo.hpp>
+
+/* PACKAGE */
+#include "fixposition_driver_lib/helper.hpp"
+#include "fixposition_driver_lib/params.hpp"
+
+namespace fixposition {
+/* ****************************************************************************************************************** */
+
+TEST(LlhTrafo, ParamsUseWgs84ThreeDimensionalDefaults) {
+    const DriverParams params;
+    EXPECT_FALSE(params.datum_llh_enabled_);
+    EXPECT_EQ(params.datum_llh_ecef_crs_, "EPSG:4978");
+    EXPECT_EQ(params.datum_llh_llh_crs_, "EPSG:4979");
+}
+
+TEST(OdometryData, EnuOriginAlwaysUsesEllipsoidalWgs84Height) {
+    const Eigen::Vector3d llh_ref(fpsdk::common::math::DegToRad(47.0), fpsdk::common::math::DegToRad(8.0), 500.0);
+    const Eigen::Vector3d ecef_ref = fpsdk::common::trafo::TfEcefWgs84Llh(llh_ref);
+
+    TfData tf;
+    tf.valid = true;
+    tf.translation = ecef_ref;
+    tf.rotation.setIdentity();
+
+    OdometryData odometry;
+    odometry.pose.position = ecef_ref;
+    odometry.pose.orientation.setIdentity();
+    ASSERT_TRUE(odometry.ConvertToEnu(tf));
+    EXPECT_LT(odometry.pose.position.norm(), 1e-3) << odometry.pose.position.transpose();
+}
+
+#if FPSDK_USE_PROJ
+using fpsdk::common::trafo::Transformer;
+
+// Driver params with datum LLH enabled
+static DriverParams LlhParams(const std::string& ecef_crs, const std::string& llh_crs) {
+    DriverParams params;
+    params.datum_llh_enabled_ = true;
+    params.datum_llh_ecef_crs_ = ecef_crs;
+    params.datum_llh_llh_crs_ = llh_crs;
+    return params;
+}
+
+TEST(LlhTrafo, DisabledUsesBuiltInWgs84Conversion) {
+    DriverParams params;
+    params.datum_llh_ecef_crs_ = "not a CRS";
+    params.datum_llh_llh_crs_ = "also not a CRS";
+    std::unique_ptr<Transformer> llh_trafo;
+    std::string error;
+    ASSERT_TRUE(InitLlhTrafo(params, llh_trafo, error));
+    EXPECT_FALSE(llh_trafo);
+    EXPECT_TRUE(error.empty());
+
+    const Eigen::Vector3d expected_llh(fpsdk::common::math::DegToRad(47.0), fpsdk::common::math::DegToRad(8.0), 500.0);
+    const Eigen::Vector3d ecef = fpsdk::common::trafo::TfEcefWgs84Llh(expected_llh);
+    Eigen::Vector3d actual_llh;
+    ASSERT_TRUE(EcefToLlhRad(llh_trafo.get(), ecef, actual_llh));
+    EXPECT_TRUE(actual_llh.isApprox(expected_llh, 1e-8));
+}
+
+TEST(LlhTrafo, RejectsInvalidAndNonGeographicCrs) {
+    std::unique_ptr<Transformer> llh_trafo;
+    std::string error;
+    EXPECT_FALSE(InitLlhTrafo(LlhParams("EPSG:4978", "EPSG:4979+3855"), llh_trafo, error));
+    EXPECT_FALSE(llh_trafo);
+    EXPECT_FALSE(error.empty());
+    EXPECT_FALSE(InitLlhTrafo(LlhParams("EPSG:4978", "EPSG:2056"), llh_trafo, error));
+    EXPECT_FALSE(llh_trafo);
+    EXPECT_EQ(error, "target CRS must be geographic 3D or geographic 2D plus vertical");
+    EXPECT_FALSE(InitLlhTrafo(LlhParams("EPSG:4326", "EPSG:4979"), llh_trafo, error));
+    EXPECT_FALSE(llh_trafo);
+    EXPECT_EQ(error, "source CRS must be geocentric");
+}
+
+TEST(LlhTrafo, MatchesBuiltInWgs84Conversion) {
+    std::unique_ptr<Transformer> llh_trafo;
+    std::string error;
+    ASSERT_TRUE(InitLlhTrafo(LlhParams("EPSG:4978", "EPSG:4979"), llh_trafo, error)) << error;
+    ASSERT_TRUE(llh_trafo);
+
+    // Checks lon/lat swap and deg to rad
+    const Eigen::Vector3d expected_llh(fpsdk::common::math::DegToRad(47.0), fpsdk::common::math::DegToRad(8.0), 500.0);
+    const Eigen::Vector3d ecef = fpsdk::common::trafo::TfEcefWgs84Llh(expected_llh);
+    Eigen::Vector3d actual_llh;
+    ASSERT_TRUE(EcefToLlhRad(llh_trafo.get(), ecef, actual_llh));
+    EXPECT_TRUE(actual_llh.isApprox(expected_llh, 1e-8)) << actual_llh.transpose();
+}
+
+TEST(LlhTrafo, TransformsOnAThreadDifferentFromInitialization) {
+    std::unique_ptr<Transformer> llh_trafo;
+    std::string error;
+    ASSERT_TRUE(InitLlhTrafo(LlhParams("EPSG:4978", "EPSG:4979"), llh_trafo, error)) << error;
+
+    const Eigen::Vector3d expected_llh(fpsdk::common::math::DegToRad(47.0), fpsdk::common::math::DegToRad(8.0), 500.0);
+    const Eigen::Vector3d ecef = fpsdk::common::trafo::TfEcefWgs84Llh(expected_llh);
+    Eigen::Vector3d actual_llh;
+    bool success = false;
+    std::thread worker([&]() { success = EcefToLlhRad(llh_trafo.get(), ecef, actual_llh); });
+    worker.join();
+
+    ASSERT_TRUE(success);
+    EXPECT_TRUE(actual_llh.isApprox(expected_llh, 1e-8));
+}
+#endif
+
+/* ****************************************************************************************************************** */
+}  // namespace fixposition

fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp

@@ -15,7 +15,6 @@
 #define __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1_HPP__
 
 /* LIBC/STL */
-#include <memory>
 #include <unordered_map>
 
 /* EXTERNAL */
@@ -39,6 +38,9 @@ void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& pa
 void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_,
                                ros::Publisher& pub);
 void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_,
+#if FPSDK_USE_PROJ
+                                     const fpsdk::common::trafo::Transformer* llh_trafo,
+#endif
                                      ros::Publisher& pub);
 void PublishFpaOdomenu(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, ros::Publisher& pub);
 void PublishFpaOdomenuVector3Stamped(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, ros::Publisher& pub);
@@ -77,7 +79,11 @@ void PublishParserMsg(const fpsdk::common::parser::ParserMsg& msg, ros::Publishe
 void PublishNmeaEpochData(const NmeaEpochData& data, ros::Publisher& pub);
 void PublishOdometryData(const OdometryData& data, ros::Publisher& pub);
 void PublishJumpWarning(const JumpDetector& jump_detector, ros::Publisher& pub);
-void PublishDatum(const geometry_msgs::Vector3& payload, const ros::Time& stamp, ros::Publisher& pub);
+void PublishDatum(const geometry_msgs::Vector3& payload, const ros::Time& stamp,
+#if FPSDK_USE_PROJ
+                  const fpsdk::common::trafo::Transformer* llh_trafo,
+#endif
+                  ros::Publisher& pub);
 void PublishFusionEpochData(const FusionEpochData& data, ros::Publisher& pub);
 
 /* ****************************************************************************************************************** */

fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp

@@ -63,6 +63,10 @@ class FixpositionDriverNode {
     DriverParams params_;       //!< Sensor/driver parameters
     FixpositionDriver driver_;  //!< Sensor driver
 
+#if FPSDK_USE_PROJ
+    std::unique_ptr<fpsdk::common::trafo::Transformer> llh_trafo_;  //!< ECEF to LLH, nullptr = built-in WGS84
+#endif
+
     // ROS publishers
     // - FP_A messages
     ros::Publisher fpa_eoe_pub_;         //!< FP_A-EOE message

fixposition_driver_ros1/launch/config.yaml

@@ -54,6 +54,13 @@ raw_output:   false    # Enable raw messages output
 cov_warning:  false    # Enable covariance warnings
 nav2_mode:    false    # Enable nav2 mode
 
+# Optional: omitting this block preserves the legacy WGS-84 LLH output.
+datum_llh:
+    enabled: false               # Use PROJ to convert the altitude published on odometry_llh and datum
+    ecef_crs: "EPSG:4978"        # Geocentric source CRS of the sensor's ECEF coordinates; normally do not change
+    llh_crs: "EPSG:4979"         # Geographic output CRS. For EGM2008 orthometric height use "EPSG:4326+3855"
+                                 # and install the us_nga_egm08_25.tif PROJ grid. Missing grids are treated as errors.
+
 converter:
     enabled: false
     topic_type: "Twist"        # Supported types: nav_msgs/{Twist, TwistWithCov, Odometry}