From afc6b22fb55255c264c3b76b59a696e3dbea3e9a Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 11 Sep 2026 18:45:00 +0200 Subject: [PATCH 1/3] Add configurable datum LLH transformations with PROJ --- fixposition_driver_lib/CMakeLists.txt | 39 ++- .../fixposition_driver_lib-config.cmake.in | 9 + .../llh_transformer.hpp | 65 +++++ .../include/fixposition_driver_lib/params.hpp | 3 + fixposition_driver_lib/src/helper.cpp | 5 +- .../src/llh_transformer.cpp | 230 ++++++++++++++++++ .../test/llh_transformer_test.cpp | 94 +++++++ .../fixposition_driver_ros1/data_to_ros1.hpp | 7 +- .../fixposition_driver_node.hpp | 7 +- fixposition_driver_ros1/launch/config.yaml | 7 + fixposition_driver_ros1/src/data_to_ros1.cpp | 18 +- .../src/fixposition_driver_node.cpp | 16 +- fixposition_driver_ros1/src/params.cpp | 29 +++ .../fixposition_driver_ros2/data_to_ros2.hpp | 4 +- .../fixposition_driver_node.hpp | 1 + fixposition_driver_ros2/launch/config.yaml | 7 + fixposition_driver_ros2/src/data_to_ros2.cpp | 14 +- .../src/fixposition_driver_node.cpp | 14 +- fixposition_driver_ros2/src/params.cpp | 11 + 19 files changed, 553 insertions(+), 27 deletions(-) create mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/llh_transformer.hpp create mode 100644 fixposition_driver_lib/src/llh_transformer.cpp create mode 100644 fixposition_driver_lib/test/llh_transformer_test.cpp diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt index c52ac2d4..59351a37 100644 --- a/fixposition_driver_lib/CMakeLists.txt +++ b/fixposition_driver_lib/CMakeLists.txt @@ -17,7 +17,6 @@ if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") endif() set(_unused "${FPSDK_BUILD_TESTING}") # suppress warning -set(_unused "${FPSDK_USE_PROJ}") # suppress warning set(_unused "${FPSDK_USE_FFMPEG}") # suppress warning set(_unused "${FPSDK_USE_BZ2}") # suppress warning set(_unused "${CMAKE_C_COMPILER}") # suppress warning @@ -29,6 +28,14 @@ find_package(Eigen3 REQUIRED) find_package(fpsdk_common REQUIRED) find_package(Threads REQUIRED) +# Respect an explicit SDK-wide disable. Otherwise use PROJ when it is available. +if(DEFINED FPSDK_USE_PROJ AND NOT FPSDK_USE_PROJ) + set(FIXPOSITION_DRIVER_USE_PROJ OFF) +else() + find_package(PROJ 9.4 QUIET) + set(FIXPOSITION_DRIVER_USE_PROJ ${PROJ_FOUND}) +endif() + include_directories(include ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} @@ -42,10 +49,38 @@ add_library( ${PROJECT_NAME} SHARED src/fixposition_driver.cpp src/helper.cpp + src/llh_transformer.cpp src/params.cpp ) -target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${fpsdk_common_LIBRARIES} pthread) +target_compile_definitions(${PROJECT_NAME} PUBLIC + FIXPOSITION_DRIVER_USE_PROJ=$ +) + +target_link_libraries(${PROJECT_NAME} + ${Boost_LIBRARIES} + ${fpsdk_common_LIBRARIES} + $<$:PROJ::proj> + pthread +) + + +# TESTS ================================================================================================================= + +if(FPSDK_BUILD_TESTING) + enable_testing() + find_package(GTest REQUIRED) + + add_executable(${PROJECT_NAME}_llh_transformer_test + test/llh_transformer_test.cpp + ) + target_link_libraries(${PROJECT_NAME}_llh_transformer_test + PRIVATE + ${PROJECT_NAME} + GTest::gtest_main + ) + add_test(NAME ${PROJECT_NAME}_llh_transformer_test COMMAND ${PROJECT_NAME}_llh_transformer_test) +endif() # INSTALL ============================================================================================================== diff --git a/fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in b/fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in index cb4bbf37..a856103d 100644 --- a/fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in +++ b/fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in @@ -1,5 +1,14 @@ @PACKAGE_INIT@ +include(CMakeFindDependencyMacro) +set(_fixposition_driver_lib_package_prefix_dir "${PACKAGE_PREFIX_DIR}") +find_dependency(fpsdk_common) +if(@FIXPOSITION_DRIVER_USE_PROJ@) + find_dependency(PROJ 9.4) +endif() +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") diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/llh_transformer.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/llh_transformer.hpp new file mode 100644 index 00000000..0653a4bf --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/llh_transformer.hpp @@ -0,0 +1,65 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Optional PROJ-backed ECEF to geodetic coordinate conversion + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_LLH_TRANSFORMER_HPP__ +#define __FIXPOSITION_DRIVER_LIB_LLH_TRANSFORMER_HPP__ + +/* LIBC/STL */ +#include + +/* EXTERNAL */ +#include + +namespace fixposition { +/* ****************************************************************************************************************** */ + +/** + * @brief Convert ECEF coordinates to latitude, longitude and height + * + * The configured target must be either a three-dimensional geographic CRS or a compound CRS consisting of a + * two-dimensional geographic CRS and a vertical CRS. Output is always latitude [rad], longitude [rad], height [m]. + * PROJ state is created per calling thread because a PROJ context must not be shared across threads. + */ +class LlhTransformer { + public: + /** + * @brief Configure and validate the transformation + * + * @param[in] enabled Use PROJ when true; use the built-in WGS84 conversion when false + * @param[in] ecef_crs Geocentric source CRS, normally "EPSG:4978" + * @param[in] llh_crs Geographic 3D or geographic 2D plus vertical target CRS + * + * @returns true if disabled or if the PROJ transformation is available and valid + */ + bool Init(bool enabled, const std::string& ecef_crs, const std::string& llh_crs); + + /** + * @brief Convert ECEF [m] to latitude [rad], longitude [rad], height [m] + * + * @returns true on success, false if the configured PROJ operation cannot transform the coordinate + */ + bool EcefToLlhRad(const Eigen::Vector3d& ecef, Eigen::Vector3d& llh_rad) const; + + bool enabled() const { return proj_enabled_; } + const std::string& error() const { return error_; } + + private: + bool proj_enabled_ = false; + std::string ecef_crs_; + std::string llh_crs_; + std::string error_; +}; + +/* ****************************************************************************************************************** */ +} // namespace fixposition +#endif // __FIXPOSITION_DRIVER_LIB_LLH_TRANSFORMER_HPP__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp index 331c6865..cf42af67 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp +++ b/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; diff --git a/fixposition_driver_lib/src/helper.cpp b/fixposition_driver_lib/src/helper.cpp index 232c4a8e..580d0368 100644 --- a/fixposition_driver_lib/src/helper.cpp +++ b/fixposition_driver_lib/src/helper.cpp @@ -161,13 +161,12 @@ bool OdometryData::ConvertToEnu(const TfData& tf_ecef_enu0) { const Eigen::Matrix 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 diff --git a/fixposition_driver_lib/src/llh_transformer.cpp b/fixposition_driver_lib/src/llh_transformer.cpp new file mode 100644 index 00000000..f75317dc --- /dev/null +++ b/fixposition_driver_lib/src/llh_transformer.cpp @@ -0,0 +1,230 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Optional PROJ-backed ECEF to geodetic coordinate conversion + */ + +/* LIBC/STL */ +#include +#include +#include + +/* EXTERNAL */ +#include +#include +#include +#if FIXPOSITION_DRIVER_USE_PROJ +#include +#endif + +/* PACKAGE */ +#include "fixposition_driver_lib/llh_transformer.hpp" + +namespace fixposition { +/* ****************************************************************************************************************** */ + +#if FIXPOSITION_DRIVER_USE_PROJ +namespace { + +std::string ProjError(PJ_CONTEXT* context, const std::string& fallback) { + const int error = proj_context_errno(context); + const char* message = proj_context_errno_string(context, error); + return ((message != nullptr) && (message[0] != '\0')) ? std::string(message) : fallback; +} + +bool IsSupportedTarget(PJ_CONTEXT* context, const PJ* target) { + const PJ_TYPE type = proj_get_type(target); + if (type == PJ_TYPE_GEOGRAPHIC_3D_CRS) { + return true; + } + if (type != PJ_TYPE_COMPOUND_CRS) { + return false; + } + + PJ* horizontal = proj_crs_get_sub_crs(context, target, 0); + PJ* vertical = proj_crs_get_sub_crs(context, target, 1); + const bool supported = (horizontal != nullptr) && (vertical != nullptr) && + (proj_get_type(horizontal) == PJ_TYPE_GEOGRAPHIC_2D_CRS) && + (proj_get_type(vertical) == PJ_TYPE_VERTICAL_CRS); + proj_destroy(horizontal); + proj_destroy(vertical); + return supported; +} + +class ThreadProjTransformer { + public: + ThreadProjTransformer() = default; + ~ThreadProjTransformer() { Reset(); } + + bool Configure(const LlhTransformer* owner, const std::string& ecef_crs, const std::string& llh_crs, + std::string& error) { + if ((owner_ == owner) && (ecef_crs_ == ecef_crs) && (llh_crs_ == llh_crs)) { + error = error_; + return transform_ != nullptr; + } + + Reset(); + owner_ = owner; + ecef_crs_ = ecef_crs; + llh_crs_ = llh_crs; + context_ = proj_context_create(); + if (context_ == nullptr) { + error_ = "failed creating PROJ context"; + error = error_; + return false; + } + proj_context_set_enable_network(context_, false); + + PJ* source = proj_create(context_, ecef_crs.c_str()); + if (source == nullptr) { + error_ = "invalid source CRS: " + ProjError(context_, "unknown PROJ error"); + error = error_; + return false; + } + PJ* target = proj_create(context_, llh_crs.c_str()); + if (target == nullptr) { + error_ = "invalid target CRS: " + ProjError(context_, "unknown PROJ error"); + proj_destroy(source); + error = error_; + return false; + } + + if (proj_get_type(source) != PJ_TYPE_GEOCENTRIC_CRS) { + error_ = "source CRS must be geocentric"; + } else if (!IsSupportedTarget(context_, target)) { + error_ = "target CRS must be geographic 3D or geographic 2D plus vertical"; + } else { + const char* options[] = {"ALLOW_BALLPARK=NO", "ONLY_BEST=YES", nullptr}; + PJ* operation = proj_create_crs_to_crs_from_pj(context_, source, target, nullptr, options); + if (operation == nullptr) { + error_ = "failed creating coordinate operation: " + ProjError(context_, "unknown PROJ error"); + } else if (!proj_coordoperation_is_instantiable(context_, operation)) { + error_ = "coordinate operation is unavailable; check that all required PROJ grids are installed"; + proj_destroy(operation); + } else { + // Normalized output has longitude, latitude and height order regardless of the CRS's native axis order. + transform_ = proj_normalize_for_visualization(context_, operation); + proj_destroy(operation); + if (transform_ == nullptr) { + error_ = "failed normalizing coordinate operation: " + ProjError(context_, "unknown PROJ error"); + } + } + } + + proj_destroy(source); + proj_destroy(target); + error = error_; + return transform_ != nullptr; + } + + bool Transform(const Eigen::Vector3d& ecef, Eigen::Vector3d& llh_rad, std::string& error) { + if (transform_ == nullptr) { + error = error_; + return false; + } + + proj_errno_reset(transform_); + const PJ_COORD input = proj_coord(ecef.x(), ecef.y(), ecef.z(), std::numeric_limits::infinity()); + const PJ_COORD output = proj_trans(transform_, PJ_FWD, input); + const int transform_error = proj_errno(transform_); + if ((transform_error != 0) || !std::isfinite(output.xyz.x) || !std::isfinite(output.xyz.y) || + !std::isfinite(output.xyz.z)) { + const char* message = proj_errno_string(transform_error); + error = ((message != nullptr) && (message[0] != '\0')) ? message : "coordinate transformation failed"; + return false; + } + + llh_rad = {fpsdk::common::math::DegToRad(output.xyz.y), fpsdk::common::math::DegToRad(output.xyz.x), + output.xyz.z}; + return true; + } + + bool warning_reported() const { return warning_reported_; } + void set_warning_reported() { warning_reported_ = true; } + + private: + void Reset() { + if (transform_ != nullptr) { + proj_destroy(transform_); + } + if (context_ != nullptr) { + proj_context_destroy(context_); + } + transform_ = nullptr; + context_ = nullptr; + owner_ = nullptr; + ecef_crs_.clear(); + llh_crs_.clear(); + error_.clear(); + warning_reported_ = false; + } + + const LlhTransformer* owner_ = nullptr; + std::string ecef_crs_; + std::string llh_crs_; + std::string error_; + PJ_CONTEXT* context_ = nullptr; + PJ* transform_ = nullptr; + bool warning_reported_ = false; +}; + +} // namespace +#endif // FIXPOSITION_DRIVER_USE_PROJ + +bool LlhTransformer::Init(bool enabled, const std::string& ecef_crs, const std::string& llh_crs) { + proj_enabled_ = false; + ecef_crs_ = ecef_crs; + llh_crs_ = llh_crs; + error_.clear(); + if (!enabled) { + return true; + } + +#if FIXPOSITION_DRIVER_USE_PROJ + // Validate using temporary state so the context is created and destroyed by this thread. Transforming threads + // create their own cached state on first use. + ThreadProjTransformer validator; + if (!validator.Configure(this, ecef_crs_, llh_crs_, error_)) { + return false; + } + proj_enabled_ = true; + return true; +#else + error_ = "driver was built without PROJ support"; + return false; +#endif +} + +bool LlhTransformer::EcefToLlhRad(const Eigen::Vector3d& ecef, Eigen::Vector3d& llh_rad) const { + if (!proj_enabled_) { + llh_rad = fpsdk::common::trafo::TfWgs84LlhEcef(ecef); + return true; + } + +#if FIXPOSITION_DRIVER_USE_PROJ + thread_local ThreadProjTransformer transformer; + std::string error; + if (!transformer.Configure(this, ecef_crs_, llh_crs_, error) || !transformer.Transform(ecef, llh_rad, error)) { + if (!transformer.warning_reported()) { + WARNING("PROJ ECEF to LLH transformation failed: %s", error.c_str()); + transformer.set_warning_reported(); + } + return false; + } + return true; +#else + (void)ecef; + (void)llh_rad; + return false; +#endif +} + +/* ****************************************************************************************************************** */ +} // namespace fixposition diff --git a/fixposition_driver_lib/test/llh_transformer_test.cpp b/fixposition_driver_lib/test/llh_transformer_test.cpp new file mode 100644 index 00000000..405f743b --- /dev/null +++ b/fixposition_driver_lib/test/llh_transformer_test.cpp @@ -0,0 +1,94 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + */ + +/* LIBC/STL */ +#include + +/* EXTERNAL */ +#include + +#include +#include + +/* PACKAGE */ +#include "fixposition_driver_lib/helper.hpp" +#include "fixposition_driver_lib/llh_transformer.hpp" +#include "fixposition_driver_lib/params.hpp" + +namespace fixposition { +/* ****************************************************************************************************************** */ + +TEST(LlhTransformer, ParamsUseWgs84ThreeDimensionalDefaults) { + const DriverParams params; + EXPECT_EQ(params.datum_llh_ecef_crs_, "EPSG:4978"); + EXPECT_EQ(params.datum_llh_llh_crs_, "EPSG:4979"); +} + +TEST(LlhTransformer, DisabledUsesBuiltInWgs84Conversion) { + LlhTransformer transformer; + ASSERT_TRUE(transformer.Init(false, "not a CRS", "also not a CRS")); + EXPECT_FALSE(transformer.enabled()); + + 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(transformer.EcefToLlhRad(ecef, actual_llh)); + EXPECT_TRUE(actual_llh.isApprox(expected_llh, 1e-8)); +} + +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 FIXPOSITION_DRIVER_USE_PROJ +TEST(LlhTransformer, RejectsInvalidAndNonGeographicCrs) { + LlhTransformer transformer; + EXPECT_FALSE(transformer.Init(true, "EPSG:4978", "EPSG:4979+3855")); + EXPECT_FALSE(transformer.Init(true, "EPSG:4978", "EPSG:2056")); + EXPECT_FALSE(transformer.Init(true, "EPSG:4326", "EPSG:4979")); +} + +TEST(LlhTransformer, TransformsOnAThreadDifferentFromInitialization) { + LlhTransformer transformer; + ASSERT_TRUE(transformer.Init(true, "EPSG:4978", "EPSG:4979")) << transformer.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 = transformer.EcefToLlhRad(ecef, actual_llh); }); + worker.join(); + + ASSERT_TRUE(success); + EXPECT_TRUE(actual_llh.isApprox(expected_llh, 1e-8)); +} +#else +TEST(LlhTransformer, EnabledFailsWhenProjWasCompiledOut) { + LlhTransformer transformer; + EXPECT_FALSE(transformer.Init(true, "EPSG:4978", "EPSG:4979")); + EXPECT_FALSE(transformer.enabled()); + EXPECT_FALSE(transformer.error().empty()); +} +#endif + +/* ****************************************************************************************************************** */ +} // namespace fixposition diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index 6ef1e550..53b61398 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp @@ -15,12 +15,12 @@ #define __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1_HPP__ /* LIBC/STL */ -#include #include /* EXTERNAL */ #include #include +#include #include #include @@ -39,7 +39,7 @@ 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_, - ros::Publisher& pub); + const LlhTransformer& llh_transformer, 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); void PublishFpaOdomsh(const fpsdk::common::parser::fpa::FpaOdomshPayload& payload, ros::Publisher& pub); @@ -77,7 +77,8 @@ 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, const LlhTransformer& llh_transformer, + ros::Publisher& pub); void PublishFusionEpochData(const FusionEpochData& data, ros::Publisher& pub); /* ****************************************************************************************************************** */ diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index 3581ade3..99dd349b 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -59,9 +59,10 @@ class FixpositionDriverNode { void StopNode(); private: - ros::NodeHandle nh_; //!< ROS node handle - DriverParams params_; //!< Sensor/driver parameters - FixpositionDriver driver_; //!< Sensor driver + ros::NodeHandle nh_; //!< ROS node handle + DriverParams params_; //!< Sensor/driver parameters + FixpositionDriver driver_; //!< Sensor driver + LlhTransformer llh_transformer_; //!< Optional PROJ-backed ECEF-to-LLH output conversion // ROS publishers // - FP_A messages diff --git a/fixposition_driver_ros1/launch/config.yaml b/fixposition_driver_ros1/launch/config.yaml index fb65e611..957b39f8 100644 --- a/fixposition_driver_ros1/launch/config.yaml +++ b/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} diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 942ba1bd..3b148f43 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -153,7 +153,8 @@ void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, bool nav2 // --------------------------------------------------------------------------------------------------------------------- -void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, ros::Publisher& pub) { +void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, + const LlhTransformer& llh_transformer, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { sensor_msgs::NavSatFix msg; msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); @@ -173,14 +174,17 @@ void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, boo msg.position_covariance_type = msg.COVARIANCE_TYPE_UNKNOWN; cov_map = Eigen::Matrix3d::Zero(); // FIXME: necessary? } else { - const Eigen::Vector3d llh_pos = trafo::TfWgs84LlhEcef(pose.position); + Eigen::Vector3d llh_pos; + if (!llh_transformer.EcefToLlhRad(pose.position, llh_pos)) { + return; + } msg.latitude = math::RadToDeg(llh_pos(0)); msg.longitude = math::RadToDeg(llh_pos(1)); msg.altitude = llh_pos(2); // Populate LLH covariance const Eigen::Matrix3d p_cov_e = pose.cov.topLeftCorner(3, 3); - const Eigen::Matrix3d C_l_e = trafo::RotEnuEcef(pose.position); + const Eigen::Matrix3d C_l_e = trafo::RotEnuEcef(llh_pos.x(), llh_pos.y()); const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose(); cov_map = p_cov_l; msg.position_covariance_type = msg.COVARIANCE_TYPE_KNOWN; @@ -864,7 +868,8 @@ 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, const LlhTransformer& llh_transformer, + ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { sensor_msgs::NavSatFix msg; msg.header.stamp = stamp; @@ -872,7 +877,10 @@ void PublishDatum(const geometry_msgs::Vector3& payload, const ros::Time& stamp, // Populate LLH position const Eigen::Vector3d position = {payload.x, payload.y, payload.z}; - const Eigen::Vector3d llh_pos = trafo::TfWgs84LlhEcef(position); + Eigen::Vector3d llh_pos; + if (!llh_transformer.EcefToLlhRad(position, llh_pos)) { + return; + } msg.latitude = math::RadToDeg(llh_pos(0)); msg.longitude = math::RadToDeg(llh_pos(1)); msg.altitude = llh_pos(2); diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index d5e8de60..bb6904bc 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -45,7 +46,14 @@ FixpositionDriverNode::FixpositionDriverNode(const DriverParams& params, ros::No params_ { params }, driver_ { params_ }, nmea_epoch_data_ { params_.nmea_epoch_ } // clang-format on -{} +{ + if (!llh_transformer_.Init(params_.datum_llh_enabled_, params_.datum_llh_ecef_crs_, params_.datum_llh_llh_crs_)) { + throw std::runtime_error("Failed initializing PROJ transformation: " + llh_transformer_.error()); + } + if (llh_transformer_.enabled()) { + ROS_INFO("PROJ enabled for ECEF to LLH output conversion"); + } +} FixpositionDriverNode::~FixpositionDriverNode() { StopNode(); } @@ -81,7 +89,7 @@ bool FixpositionDriverNode::StartNode() { auto odometry_payload = dynamic_cast(payload); PublishFpaOdometry(odometry_payload, fpa_odometry_pub_); PublishFpaOdometryDataImu(odometry_payload, params_.nav2_mode_, poiimu_pub_); - PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, odometry_llh_pub_); + PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, llh_transformer_, odometry_llh_pub_); OdometryData odometry_data; odometry_data.SetFromFpaOdomPayload(odometry_payload); PublishOdometryData(odometry_data, odometry_ecef_pub_); @@ -701,8 +709,8 @@ void FixpositionDriverNode::PublishNav2Tf() { // Send the transform tf_br_.sendTransform(tf_odom_base); - // Publish WGS84 datum - PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, datum_pub_); + // Publish datum in the configured LLH CRS + PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, llh_transformer_, datum_pub_); } /* ****************************************************************************************************************** */ diff --git a/fixposition_driver_ros1/src/params.cpp b/fixposition_driver_ros1/src/params.cpp index 14a90d79..a3e38046 100644 --- a/fixposition_driver_ros1/src/params.cpp +++ b/fixposition_driver_ros1/src/params.cpp @@ -37,6 +37,18 @@ static bool LoadRosParam(const std::string& name, T& value) { return true; } +template +static bool LoadOptionalRosParam(const std::string& name, T& value) { + try { + if (!ros::param::has(name)) { + return true; + } + return ros::param::get(name, value); + } catch (ros::InvalidNameException& e) { + return false; + } +} + bool LoadParamsFromRos1(const std::string& ns, DriverParams& params) { bool ok = true; ROS_INFO("DriverParams: loading from %s", ns.c_str()); @@ -82,6 +94,20 @@ bool LoadParamsFromRos1(const std::string& ns, DriverParams& params) { ROS_WARN("Failed loading %s/nav2_mode param", ns.c_str()); ok = false; } + // These parameters were added after the initial driver release. Keep the DriverParams defaults when they are + // absent so configurations created by older releases remain valid. + if (!LoadOptionalRosParam(ns + "/datum_llh/enabled", params.datum_llh_enabled_)) { + ROS_WARN("Failed loading %s/datum_llh/enabled param", ns.c_str()); + ok = false; + } + if (!LoadOptionalRosParam(ns + "/datum_llh/ecef_crs", params.datum_llh_ecef_crs_)) { + ROS_WARN("Failed loading %s/datum_llh/ecef_crs param", ns.c_str()); + ok = false; + } + if (!LoadOptionalRosParam(ns + "/datum_llh/llh_crs", params.datum_llh_llh_crs_)) { + ROS_WARN("Failed loading %s/datum_llh/llh_crs param", ns.c_str()); + ok = false; + } if (!LoadRosParam(ns + "/converter/enabled", params.converter_enabled_)) { ROS_WARN("Failed loading %s/converter/enabled param", ns.c_str()); ok = false; @@ -146,6 +172,9 @@ bool LoadParamsFromRos1(const std::string& ns, DriverParams& params) { ROS_INFO("DriverParams: raw_output=%s", params.raw_output_ ? "true" : "false"); ROS_INFO("DriverParams: cov_warning=%s", params.cov_warning_ ? "true" : "false"); ROS_INFO("DriverParams: nav2_mode=%s", params.nav2_mode_ ? "true" : "false"); + ROS_INFO("DriverParams: datum_llh_enabled=%s", params.datum_llh_enabled_ ? "true" : "false"); + ROS_INFO("DriverParams: datum_llh_ecef_crs=%s", params.datum_llh_ecef_crs_.c_str()); + ROS_INFO("DriverParams: datum_llh_llh_crs=%s", params.datum_llh_llh_crs_.c_str()); ROS_INFO("DriverParams: converter_enabled=%s", params.converter_enabled_ ? "true" : "false"); ROS_INFO("DriverParams: converter_topic_type=%s", topic_type_string_.c_str()); ROS_INFO("DriverParams: converter_input_topic=%s", params.converter_input_topic_.c_str()); diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp index b3f9ab35..ab3beee4 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp @@ -15,12 +15,12 @@ #define __FIXPOSITION_DRIVER_ROS2_DATA_TO_ROS2_HPP__ /* LIBC/STL */ -#include #include /* EXTERNAL */ #include #include +#include #include #include @@ -40,6 +40,7 @@ void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& pa void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, rclcpp::Publisher::SharedPtr& pub); void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, + const LlhTransformer& llh_transformer, rclcpp::Publisher::SharedPtr& pub); void PublishFpaOdomenu(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, rclcpp::Publisher::SharedPtr& pub); @@ -105,6 +106,7 @@ void PublishNmeaEpochData(const NmeaEpochData& data, rclcpp::Publisher::SharedPtr& pub); void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher::SharedPtr& pub); void PublishDatum(const geometry_msgs::msg::Vector3& payload, const builtin_interfaces::msg::Time& stamp, + const LlhTransformer& llh_transformer, rclcpp::Publisher::SharedPtr& pub); void PublishFusionEpochData(const FusionEpochData& data, rclcpp::Publisher::SharedPtr& pub); diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 44451d69..03267af9 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -66,6 +66,7 @@ class FixpositionDriverNode { rclcpp::Logger logger_; //!< Logger FixpositionDriver driver_; //!< Sensor driver rclcpp::QoS qos_settings_; //!< QoS settings + LlhTransformer llh_transformer_; //!< Optional PROJ-backed ECEF-to-LLH output conversion // ROS publishers // - FP_A messages diff --git a/fixposition_driver_ros2/launch/config.yaml b/fixposition_driver_ros2/launch/config.yaml index 6b951101..6767c796 100644 --- a/fixposition_driver_ros2/launch/config.yaml +++ b/fixposition_driver_ros2/launch/config.yaml @@ -56,6 +56,13 @@ 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} diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index e9952743..72e9281f 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -154,6 +154,7 @@ void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, bool nav2 // --------------------------------------------------------------------------------------------------------------------- void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, + const LlhTransformer& llh_transformer, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { sensor_msgs::msg::NavSatFix msg; @@ -174,14 +175,17 @@ void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, boo msg.position_covariance_type = msg.COVARIANCE_TYPE_UNKNOWN; cov_map = Eigen::Matrix3d::Zero(); // FIXME: necessary? } else { - const Eigen::Vector3d llh_pos = trafo::TfWgs84LlhEcef(pose.position); + Eigen::Vector3d llh_pos; + if (!llh_transformer.EcefToLlhRad(pose.position, llh_pos)) { + return; + } msg.latitude = math::RadToDeg(llh_pos(0)); msg.longitude = math::RadToDeg(llh_pos(1)); msg.altitude = llh_pos(2); // Populate LLH covariance const Eigen::Matrix3d p_cov_e = pose.cov.topLeftCorner(3, 3); - const Eigen::Matrix3d C_l_e = trafo::RotEnuEcef(pose.position); + const Eigen::Matrix3d C_l_e = trafo::RotEnuEcef(llh_pos.x(), llh_pos.y()); const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose(); cov_map = p_cov_l; msg.position_covariance_type = msg.COVARIANCE_TYPE_KNOWN; @@ -885,6 +889,7 @@ void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { sensor_msgs::msg::NavSatFix msg; @@ -893,7 +898,10 @@ void PublishDatum(const geometry_msgs::msg::Vector3& payload, const builtin_inte // Populate LLH position const Eigen::Vector3d position = {payload.x, payload.y, payload.z}; - const Eigen::Vector3d llh_pos = trafo::TfWgs84LlhEcef(position); + Eigen::Vector3d llh_pos; + if (!llh_transformer.EcefToLlhRad(position, llh_pos)) { + return; + } msg.latitude = math::RadToDeg(llh_pos(0)); msg.longitude = math::RadToDeg(llh_pos(1)); msg.altitude = llh_pos(2); diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 5352e3cf..b95a5c7f 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include /* EXTERNAL */ @@ -66,6 +67,13 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr nh, else if (params_.qos_type_ == "default_long") { qos_settings_ = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_default); } + + if (!llh_transformer_.Init(params_.datum_llh_enabled_, params_.datum_llh_ecef_crs_, params_.datum_llh_llh_crs_)) { + throw std::runtime_error("Failed initializing PROJ transformation: " + llh_transformer_.error()); + } + if (llh_transformer_.enabled()) { + RCLCPP_INFO(logger_, "PROJ enabled for ECEF to LLH output conversion"); + } } FixpositionDriverNode::~FixpositionDriverNode() {} @@ -106,7 +114,7 @@ bool FixpositionDriverNode::StartNode() { auto odometry_payload = dynamic_cast(payload); PublishFpaOdometry(odometry_payload, fpa_odometry_pub_); PublishFpaOdometryDataImu(odometry_payload, params_.nav2_mode_, poiimu_pub_); - PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, odometry_llh_pub_); + PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, llh_transformer_, odometry_llh_pub_); OdometryData odometry_data; odometry_data.SetFromFpaOdomPayload(odometry_payload); PublishOdometryData(odometry_data, odometry_ecef_pub_); @@ -736,8 +744,8 @@ void FixpositionDriverNode::PublishNav2Tf() { tf_odom_base.transform = tf2::toMsg(tf_ENU0POISH); tf_br_->sendTransform(tf_odom_base); - // Publish WGS84 datum - PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, datum_pub_); + // Publish datum in the configured LLH CRS + PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, llh_transformer_, datum_pub_); } /* ****************************************************************************************************************** */ diff --git a/fixposition_driver_ros2/src/params.cpp b/fixposition_driver_ros2/src/params.cpp index 1e1298da..7e78992c 100644 --- a/fixposition_driver_ros2/src/params.cpp +++ b/fixposition_driver_ros2/src/params.cpp @@ -36,6 +36,9 @@ bool LoadParamsFromRos2(std::shared_ptr& nh, DriverParams& params) const std::string RAW_OUTPUT = "raw_output"; const std::string COV_WARNING = "cov_warning"; const std::string NAV2_MODE = "nav2_mode"; + const std::string DATUM_LLH_ENABLED = "datum_llh.enabled"; + const std::string DATUM_LLH_ECEF_CRS = "datum_llh.ecef_crs"; + const std::string DATUM_LLH_LLH_CRS = "datum_llh.llh_crs"; const std::string CONVERTER_ENABLED = "converter.enabled"; const std::string CONVERTER_INPUT_TOPIC = "converter.input_topic"; const std::string CONVERTER_SCALE_FACTOR = "converter.scale_factor"; @@ -58,6 +61,11 @@ bool LoadParamsFromRos2(std::shared_ptr& nh, DriverParams& params) nh->declare_parameter(RAW_OUTPUT, params.raw_output_); nh->declare_parameter(COV_WARNING, params.cov_warning_); nh->declare_parameter(NAV2_MODE, params.nav2_mode_); + // Capture the declared values directly. Parameter overrides are applied by declare_parameter(); when no override + // exists, the DriverParams defaults keep configurations created by older releases backward-compatible. + params.datum_llh_enabled_ = nh->declare_parameter(DATUM_LLH_ENABLED, params.datum_llh_enabled_); + params.datum_llh_ecef_crs_ = nh->declare_parameter(DATUM_LLH_ECEF_CRS, params.datum_llh_ecef_crs_); + params.datum_llh_llh_crs_ = nh->declare_parameter(DATUM_LLH_LLH_CRS, params.datum_llh_llh_crs_); nh->declare_parameter(CONVERTER_ENABLED, params.converter_enabled_); nh->declare_parameter(CONVERTER_INPUT_TOPIC, params.converter_input_topic_); nh->declare_parameter(CONVERTER_SCALE_FACTOR, params.converter_scale_factor_); @@ -178,6 +186,9 @@ bool LoadParamsFromRos2(std::shared_ptr& nh, DriverParams& params) RCLCPP_INFO(logger, "DriverParams: raw_output=%s", params.raw_output_ ? "true" : "false"); RCLCPP_INFO(logger, "DriverParams: cov_warning=%s", params.cov_warning_ ? "true" : "false"); RCLCPP_INFO(logger, "DriverParams: nav2_mode=%s", params.nav2_mode_ ? "true" : "false"); + RCLCPP_INFO(logger, "DriverParams: datum_llh_enabled=%s", params.datum_llh_enabled_ ? "true" : "false"); + RCLCPP_INFO(logger, "DriverParams: datum_llh_ecef_crs=%s", params.datum_llh_ecef_crs_.c_str()); + RCLCPP_INFO(logger, "DriverParams: datum_llh_llh_crs=%s", params.datum_llh_llh_crs_.c_str()); RCLCPP_INFO(logger, "DriverParams: converter_enabled=%s", params.converter_enabled_ ? "true" : "false"); RCLCPP_INFO(logger, "DriverParams: converter_topic_type=%s", topic_type_string_.c_str()); RCLCPP_INFO(logger, "DriverParams: converter_input_topic=%s", params.converter_input_topic_.c_str()); From 0dc7bddd60cbad5064b8012757e5b7738eb9863a Mon Sep 17 00:00:00 2001 From: flipflip Date: Wed, 28 Oct 2026 17:49:00 +0100 Subject: [PATCH 2/3] Fix it Simplify LlhTransformer thread safety Create the PROJ state once in Init() and use a mutex for the transforms, instead of per-thread PROJ state. Only the driver worker thread transforms coordinates, so per-thread state is not necessary. Use SDK Transformer for datum LLH, remove LlhTransformer The SDK Transformer now has the options, CRS checks and thread safety that LlhTransformer had. The driver keeps only the policy: InitLlhTrafo() checks the CRS types, EcefToLlhRad() falls back to the built-in WGS84 conversion if datum LLH is disabled. Without PROJ (FPSDK_USE_PROJ=0) the driver uses the built-in WGS84 conversion and rejects datum LLH enabled. The driver no longer links PROJ directly. Use BUILD_TESTING for fixposition_driver_lib tests Use include(CTest) and BUILD_TESTING (default ON) instead of FPSDK_BUILD_TESTING. Build the tests only if GTest is available. Load ROS1 datum_llh params only if the namespace exists If datum_llh is present, all its params are required. If it is absent, the defaults stay. Remove LoadOptionalRosParam(). --- .gitignore | 1 + fixposition-sdk | 2 +- fixposition_driver_lib/CMakeLists.txt | 47 ++-- .../fixposition_driver_lib-config.cmake.in | 3 - .../include/fixposition_driver_lib/helper.hpp | 31 +++ .../llh_transformer.hpp | 65 ----- fixposition_driver_lib/src/helper.cpp | 59 +++++ .../src/llh_transformer.cpp | 230 ------------------ .../test/llh_trafo_test.cpp | 127 ++++++++++ .../test/llh_transformer_test.cpp | 94 ------- .../fixposition_driver_ros1/data_to_ros1.hpp | 11 +- .../fixposition_driver_node.hpp | 11 +- fixposition_driver_ros1/src/data_to_ros1.cpp | 22 +- .../src/fixposition_driver_node.cpp | 25 +- fixposition_driver_ros1/src/params.cpp | 35 ++- .../fixposition_driver_ros2/data_to_ros2.hpp | 9 +- .../fixposition_driver_node.hpp | 5 +- fixposition_driver_ros2/src/data_to_ros2.cpp | 20 +- .../src/fixposition_driver_node.cpp | 25 +- 19 files changed, 354 insertions(+), 468 deletions(-) delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/llh_transformer.hpp delete mode 100644 fixposition_driver_lib/src/llh_transformer.cpp create mode 100644 fixposition_driver_lib/test/llh_trafo_test.cpp delete mode 100644 fixposition_driver_lib/test/llh_transformer_test.cpp diff --git a/.gitignore b/.gitignore index a2bb453b..6b099c07 100644 --- a/.gitignore +++ b/.gitignore @@ -14,3 +14,4 @@ COLCON_IGNORE .catkin_tools core /tmp +/prompt.md diff --git a/fixposition-sdk b/fixposition-sdk index 8c37c260..f08c7835 160000 --- a/fixposition-sdk +++ b/fixposition-sdk @@ -1 +1 @@ -Subproject commit 8c37c260beeebe0fe26738c469f1806430f9f19d +Subproject commit f08c7835320f0408a1f18b1ca3c0d64e6caec4bf diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt index 59351a37..689fed54 100644 --- a/fixposition_driver_lib/CMakeLists.txt +++ b/fixposition_driver_lib/CMakeLists.txt @@ -17,6 +17,7 @@ if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") endif() set(_unused "${FPSDK_BUILD_TESTING}") # suppress warning +set(_unused "${FPSDK_USE_PROJ}") # suppress warning set(_unused "${FPSDK_USE_FFMPEG}") # suppress warning set(_unused "${FPSDK_USE_BZ2}") # suppress warning set(_unused "${CMAKE_C_COMPILER}") # suppress warning @@ -28,14 +29,6 @@ find_package(Eigen3 REQUIRED) find_package(fpsdk_common REQUIRED) find_package(Threads REQUIRED) -# Respect an explicit SDK-wide disable. Otherwise use PROJ when it is available. -if(DEFINED FPSDK_USE_PROJ AND NOT FPSDK_USE_PROJ) - set(FIXPOSITION_DRIVER_USE_PROJ OFF) -else() - find_package(PROJ 9.4 QUIET) - set(FIXPOSITION_DRIVER_USE_PROJ ${PROJ_FOUND}) -endif() - include_directories(include ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} @@ -49,37 +42,37 @@ add_library( ${PROJECT_NAME} SHARED src/fixposition_driver.cpp src/helper.cpp - src/llh_transformer.cpp src/params.cpp ) -target_compile_definitions(${PROJECT_NAME} PUBLIC - FIXPOSITION_DRIVER_USE_PROJ=$ -) - target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${fpsdk_common_LIBRARIES} - $<$:PROJ::proj> pthread ) # TESTS ================================================================================================================= -if(FPSDK_BUILD_TESTING) - enable_testing() - find_package(GTest REQUIRED) - - add_executable(${PROJECT_NAME}_llh_transformer_test - test/llh_transformer_test.cpp - ) - target_link_libraries(${PROJECT_NAME}_llh_transformer_test - PRIVATE - ${PROJECT_NAME} - GTest::gtest_main - ) - add_test(NAME ${PROJECT_NAME}_llh_transformer_test COMMAND ${PROJECT_NAME}_llh_transformer_test) +# 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() diff --git a/fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in b/fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in index a856103d..36429b8b 100644 --- a/fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in +++ b/fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in @@ -3,9 +3,6 @@ include(CMakeFindDependencyMacro) set(_fixposition_driver_lib_package_prefix_dir "${PACKAGE_PREFIX_DIR}") find_dependency(fpsdk_common) -if(@FIXPOSITION_DRIVER_USE_PROJ@) - find_dependency(PROJ 9.4) -endif() set(PACKAGE_PREFIX_DIR "${_fixposition_driver_lib_package_prefix_dir}") unset(_fixposition_driver_lib_package_prefix_dir) diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp index 466d903a..56e94bf2 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp @@ -16,6 +16,7 @@ /* LIBC/STL */ #include +#include #include #include @@ -27,6 +28,7 @@ #include #include #include +#include /* 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& 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(); /* ****************************************************************************************************************** */ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/llh_transformer.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/llh_transformer.hpp deleted file mode 100644 index 0653a4bf..00000000 --- a/fixposition_driver_lib/include/fixposition_driver_lib/llh_transformer.hpp +++ /dev/null @@ -1,65 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Optional PROJ-backed ECEF to geodetic coordinate conversion - */ - -#ifndef __FIXPOSITION_DRIVER_LIB_LLH_TRANSFORMER_HPP__ -#define __FIXPOSITION_DRIVER_LIB_LLH_TRANSFORMER_HPP__ - -/* LIBC/STL */ -#include - -/* EXTERNAL */ -#include - -namespace fixposition { -/* ****************************************************************************************************************** */ - -/** - * @brief Convert ECEF coordinates to latitude, longitude and height - * - * The configured target must be either a three-dimensional geographic CRS or a compound CRS consisting of a - * two-dimensional geographic CRS and a vertical CRS. Output is always latitude [rad], longitude [rad], height [m]. - * PROJ state is created per calling thread because a PROJ context must not be shared across threads. - */ -class LlhTransformer { - public: - /** - * @brief Configure and validate the transformation - * - * @param[in] enabled Use PROJ when true; use the built-in WGS84 conversion when false - * @param[in] ecef_crs Geocentric source CRS, normally "EPSG:4978" - * @param[in] llh_crs Geographic 3D or geographic 2D plus vertical target CRS - * - * @returns true if disabled or if the PROJ transformation is available and valid - */ - bool Init(bool enabled, const std::string& ecef_crs, const std::string& llh_crs); - - /** - * @brief Convert ECEF [m] to latitude [rad], longitude [rad], height [m] - * - * @returns true on success, false if the configured PROJ operation cannot transform the coordinate - */ - bool EcefToLlhRad(const Eigen::Vector3d& ecef, Eigen::Vector3d& llh_rad) const; - - bool enabled() const { return proj_enabled_; } - const std::string& error() const { return error_; } - - private: - bool proj_enabled_ = false; - std::string ecef_crs_; - std::string llh_crs_; - std::string error_; -}; - -/* ****************************************************************************************************************** */ -} // namespace fixposition -#endif // __FIXPOSITION_DRIVER_LIB_LLH_TRANSFORMER_HPP__ diff --git a/fixposition_driver_lib/src/helper.cpp b/fixposition_driver_lib/src/helper.cpp index 580d0368..c863db69 100644 --- a/fixposition_driver_lib/src/helper.cpp +++ b/fixposition_driver_lib/src/helper.cpp @@ -12,10 +12,12 @@ */ /* LIBC/STL */ +#include #include /* EXTERNAL */ #include +#include #include #include #include @@ -414,6 +416,63 @@ FusionEpochData FusionEpochData::CompleteAndReset(const fpsdk::common::parser::f // --------------------------------------------------------------------------------------------------------------------- +#if FPSDK_USE_PROJ +bool InitLlhTrafo(const DriverParams& params, std::unique_ptr& 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("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 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 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()); diff --git a/fixposition_driver_lib/src/llh_transformer.cpp b/fixposition_driver_lib/src/llh_transformer.cpp deleted file mode 100644 index f75317dc..00000000 --- a/fixposition_driver_lib/src/llh_transformer.cpp +++ /dev/null @@ -1,230 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Optional PROJ-backed ECEF to geodetic coordinate conversion - */ - -/* LIBC/STL */ -#include -#include -#include - -/* EXTERNAL */ -#include -#include -#include -#if FIXPOSITION_DRIVER_USE_PROJ -#include -#endif - -/* PACKAGE */ -#include "fixposition_driver_lib/llh_transformer.hpp" - -namespace fixposition { -/* ****************************************************************************************************************** */ - -#if FIXPOSITION_DRIVER_USE_PROJ -namespace { - -std::string ProjError(PJ_CONTEXT* context, const std::string& fallback) { - const int error = proj_context_errno(context); - const char* message = proj_context_errno_string(context, error); - return ((message != nullptr) && (message[0] != '\0')) ? std::string(message) : fallback; -} - -bool IsSupportedTarget(PJ_CONTEXT* context, const PJ* target) { - const PJ_TYPE type = proj_get_type(target); - if (type == PJ_TYPE_GEOGRAPHIC_3D_CRS) { - return true; - } - if (type != PJ_TYPE_COMPOUND_CRS) { - return false; - } - - PJ* horizontal = proj_crs_get_sub_crs(context, target, 0); - PJ* vertical = proj_crs_get_sub_crs(context, target, 1); - const bool supported = (horizontal != nullptr) && (vertical != nullptr) && - (proj_get_type(horizontal) == PJ_TYPE_GEOGRAPHIC_2D_CRS) && - (proj_get_type(vertical) == PJ_TYPE_VERTICAL_CRS); - proj_destroy(horizontal); - proj_destroy(vertical); - return supported; -} - -class ThreadProjTransformer { - public: - ThreadProjTransformer() = default; - ~ThreadProjTransformer() { Reset(); } - - bool Configure(const LlhTransformer* owner, const std::string& ecef_crs, const std::string& llh_crs, - std::string& error) { - if ((owner_ == owner) && (ecef_crs_ == ecef_crs) && (llh_crs_ == llh_crs)) { - error = error_; - return transform_ != nullptr; - } - - Reset(); - owner_ = owner; - ecef_crs_ = ecef_crs; - llh_crs_ = llh_crs; - context_ = proj_context_create(); - if (context_ == nullptr) { - error_ = "failed creating PROJ context"; - error = error_; - return false; - } - proj_context_set_enable_network(context_, false); - - PJ* source = proj_create(context_, ecef_crs.c_str()); - if (source == nullptr) { - error_ = "invalid source CRS: " + ProjError(context_, "unknown PROJ error"); - error = error_; - return false; - } - PJ* target = proj_create(context_, llh_crs.c_str()); - if (target == nullptr) { - error_ = "invalid target CRS: " + ProjError(context_, "unknown PROJ error"); - proj_destroy(source); - error = error_; - return false; - } - - if (proj_get_type(source) != PJ_TYPE_GEOCENTRIC_CRS) { - error_ = "source CRS must be geocentric"; - } else if (!IsSupportedTarget(context_, target)) { - error_ = "target CRS must be geographic 3D or geographic 2D plus vertical"; - } else { - const char* options[] = {"ALLOW_BALLPARK=NO", "ONLY_BEST=YES", nullptr}; - PJ* operation = proj_create_crs_to_crs_from_pj(context_, source, target, nullptr, options); - if (operation == nullptr) { - error_ = "failed creating coordinate operation: " + ProjError(context_, "unknown PROJ error"); - } else if (!proj_coordoperation_is_instantiable(context_, operation)) { - error_ = "coordinate operation is unavailable; check that all required PROJ grids are installed"; - proj_destroy(operation); - } else { - // Normalized output has longitude, latitude and height order regardless of the CRS's native axis order. - transform_ = proj_normalize_for_visualization(context_, operation); - proj_destroy(operation); - if (transform_ == nullptr) { - error_ = "failed normalizing coordinate operation: " + ProjError(context_, "unknown PROJ error"); - } - } - } - - proj_destroy(source); - proj_destroy(target); - error = error_; - return transform_ != nullptr; - } - - bool Transform(const Eigen::Vector3d& ecef, Eigen::Vector3d& llh_rad, std::string& error) { - if (transform_ == nullptr) { - error = error_; - return false; - } - - proj_errno_reset(transform_); - const PJ_COORD input = proj_coord(ecef.x(), ecef.y(), ecef.z(), std::numeric_limits::infinity()); - const PJ_COORD output = proj_trans(transform_, PJ_FWD, input); - const int transform_error = proj_errno(transform_); - if ((transform_error != 0) || !std::isfinite(output.xyz.x) || !std::isfinite(output.xyz.y) || - !std::isfinite(output.xyz.z)) { - const char* message = proj_errno_string(transform_error); - error = ((message != nullptr) && (message[0] != '\0')) ? message : "coordinate transformation failed"; - return false; - } - - llh_rad = {fpsdk::common::math::DegToRad(output.xyz.y), fpsdk::common::math::DegToRad(output.xyz.x), - output.xyz.z}; - return true; - } - - bool warning_reported() const { return warning_reported_; } - void set_warning_reported() { warning_reported_ = true; } - - private: - void Reset() { - if (transform_ != nullptr) { - proj_destroy(transform_); - } - if (context_ != nullptr) { - proj_context_destroy(context_); - } - transform_ = nullptr; - context_ = nullptr; - owner_ = nullptr; - ecef_crs_.clear(); - llh_crs_.clear(); - error_.clear(); - warning_reported_ = false; - } - - const LlhTransformer* owner_ = nullptr; - std::string ecef_crs_; - std::string llh_crs_; - std::string error_; - PJ_CONTEXT* context_ = nullptr; - PJ* transform_ = nullptr; - bool warning_reported_ = false; -}; - -} // namespace -#endif // FIXPOSITION_DRIVER_USE_PROJ - -bool LlhTransformer::Init(bool enabled, const std::string& ecef_crs, const std::string& llh_crs) { - proj_enabled_ = false; - ecef_crs_ = ecef_crs; - llh_crs_ = llh_crs; - error_.clear(); - if (!enabled) { - return true; - } - -#if FIXPOSITION_DRIVER_USE_PROJ - // Validate using temporary state so the context is created and destroyed by this thread. Transforming threads - // create their own cached state on first use. - ThreadProjTransformer validator; - if (!validator.Configure(this, ecef_crs_, llh_crs_, error_)) { - return false; - } - proj_enabled_ = true; - return true; -#else - error_ = "driver was built without PROJ support"; - return false; -#endif -} - -bool LlhTransformer::EcefToLlhRad(const Eigen::Vector3d& ecef, Eigen::Vector3d& llh_rad) const { - if (!proj_enabled_) { - llh_rad = fpsdk::common::trafo::TfWgs84LlhEcef(ecef); - return true; - } - -#if FIXPOSITION_DRIVER_USE_PROJ - thread_local ThreadProjTransformer transformer; - std::string error; - if (!transformer.Configure(this, ecef_crs_, llh_crs_, error) || !transformer.Transform(ecef, llh_rad, error)) { - if (!transformer.warning_reported()) { - WARNING("PROJ ECEF to LLH transformation failed: %s", error.c_str()); - transformer.set_warning_reported(); - } - return false; - } - return true; -#else - (void)ecef; - (void)llh_rad; - return false; -#endif -} - -/* ****************************************************************************************************************** */ -} // namespace fixposition diff --git a/fixposition_driver_lib/test/llh_trafo_test.cpp b/fixposition_driver_lib/test/llh_trafo_test.cpp new file mode 100644 index 00000000..368618ff --- /dev/null +++ b/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 +#include +#include + +/* EXTERNAL */ +#include + +#include +#include + +/* 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 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 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 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 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 diff --git a/fixposition_driver_lib/test/llh_transformer_test.cpp b/fixposition_driver_lib/test/llh_transformer_test.cpp deleted file mode 100644 index 405f743b..00000000 --- a/fixposition_driver_lib/test/llh_transformer_test.cpp +++ /dev/null @@ -1,94 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - */ - -/* LIBC/STL */ -#include - -/* EXTERNAL */ -#include - -#include -#include - -/* PACKAGE */ -#include "fixposition_driver_lib/helper.hpp" -#include "fixposition_driver_lib/llh_transformer.hpp" -#include "fixposition_driver_lib/params.hpp" - -namespace fixposition { -/* ****************************************************************************************************************** */ - -TEST(LlhTransformer, ParamsUseWgs84ThreeDimensionalDefaults) { - const DriverParams params; - EXPECT_EQ(params.datum_llh_ecef_crs_, "EPSG:4978"); - EXPECT_EQ(params.datum_llh_llh_crs_, "EPSG:4979"); -} - -TEST(LlhTransformer, DisabledUsesBuiltInWgs84Conversion) { - LlhTransformer transformer; - ASSERT_TRUE(transformer.Init(false, "not a CRS", "also not a CRS")); - EXPECT_FALSE(transformer.enabled()); - - 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(transformer.EcefToLlhRad(ecef, actual_llh)); - EXPECT_TRUE(actual_llh.isApprox(expected_llh, 1e-8)); -} - -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 FIXPOSITION_DRIVER_USE_PROJ -TEST(LlhTransformer, RejectsInvalidAndNonGeographicCrs) { - LlhTransformer transformer; - EXPECT_FALSE(transformer.Init(true, "EPSG:4978", "EPSG:4979+3855")); - EXPECT_FALSE(transformer.Init(true, "EPSG:4978", "EPSG:2056")); - EXPECT_FALSE(transformer.Init(true, "EPSG:4326", "EPSG:4979")); -} - -TEST(LlhTransformer, TransformsOnAThreadDifferentFromInitialization) { - LlhTransformer transformer; - ASSERT_TRUE(transformer.Init(true, "EPSG:4978", "EPSG:4979")) << transformer.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 = transformer.EcefToLlhRad(ecef, actual_llh); }); - worker.join(); - - ASSERT_TRUE(success); - EXPECT_TRUE(actual_llh.isApprox(expected_llh, 1e-8)); -} -#else -TEST(LlhTransformer, EnabledFailsWhenProjWasCompiledOut) { - LlhTransformer transformer; - EXPECT_FALSE(transformer.Init(true, "EPSG:4978", "EPSG:4979")); - EXPECT_FALSE(transformer.enabled()); - EXPECT_FALSE(transformer.error().empty()); -} -#endif - -/* ****************************************************************************************************************** */ -} // namespace fixposition diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index 53b61398..78a49e3c 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp @@ -20,7 +20,6 @@ /* EXTERNAL */ #include #include -#include #include #include @@ -39,7 +38,10 @@ 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_, - const LlhTransformer& llh_transformer, ros::Publisher& pub); +#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); void PublishFpaOdomsh(const fpsdk::common::parser::fpa::FpaOdomshPayload& payload, ros::Publisher& pub); @@ -77,7 +79,10 @@ 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, const LlhTransformer& llh_transformer, +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); diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index 99dd349b..02bde18a 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -59,10 +59,13 @@ class FixpositionDriverNode { void StopNode(); private: - ros::NodeHandle nh_; //!< ROS node handle - DriverParams params_; //!< Sensor/driver parameters - FixpositionDriver driver_; //!< Sensor driver - LlhTransformer llh_transformer_; //!< Optional PROJ-backed ECEF-to-LLH output conversion + ros::NodeHandle nh_; //!< ROS node handle + DriverParams params_; //!< Sensor/driver parameters + FixpositionDriver driver_; //!< Sensor driver + +#if FPSDK_USE_PROJ + std::unique_ptr llh_trafo_; //!< ECEF to LLH, nullptr = built-in WGS84 +#endif // ROS publishers // - FP_A messages diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 3b148f43..e2f8414f 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -154,7 +154,10 @@ void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, bool nav2 // --------------------------------------------------------------------------------------------------------------------- void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, - const LlhTransformer& llh_transformer, ros::Publisher& pub) { +#if FPSDK_USE_PROJ + const trafo::Transformer* llh_trafo, +#endif + ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { sensor_msgs::NavSatFix msg; msg.header.stamp = ros1::ConvTime(FpaGpsTimeToTime(payload.gps_time)); @@ -175,9 +178,13 @@ void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, boo cov_map = Eigen::Matrix3d::Zero(); // FIXME: necessary? } else { Eigen::Vector3d llh_pos; - if (!llh_transformer.EcefToLlhRad(pose.position, llh_pos)) { +#if FPSDK_USE_PROJ + if (!EcefToLlhRad(llh_trafo, pose.position, llh_pos)) { return; } +#else + llh_pos = trafo::TfWgs84LlhEcef(pose.position); +#endif msg.latitude = math::RadToDeg(llh_pos(0)); msg.longitude = math::RadToDeg(llh_pos(1)); msg.altitude = llh_pos(2); @@ -868,7 +875,10 @@ void PublishJumpWarning(const JumpDetector& jump_detector, ros::Publisher& pub) // --------------------------------------------------------------------------------------------------------------------- -void PublishDatum(const geometry_msgs::Vector3& payload, const ros::Time& stamp, const LlhTransformer& llh_transformer, +void PublishDatum(const geometry_msgs::Vector3& payload, const ros::Time& stamp, +#if FPSDK_USE_PROJ + const trafo::Transformer* llh_trafo, +#endif ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { sensor_msgs::NavSatFix msg; @@ -878,9 +888,13 @@ void PublishDatum(const geometry_msgs::Vector3& payload, const ros::Time& stamp, // Populate LLH position const Eigen::Vector3d position = {payload.x, payload.y, payload.z}; Eigen::Vector3d llh_pos; - if (!llh_transformer.EcefToLlhRad(position, llh_pos)) { +#if FPSDK_USE_PROJ + if (!EcefToLlhRad(llh_trafo, position, llh_pos)) { return; } +#else + llh_pos = trafo::TfWgs84LlhEcef(position); +#endif msg.latitude = math::RadToDeg(llh_pos(0)); msg.longitude = math::RadToDeg(llh_pos(1)); msg.altitude = llh_pos(2); diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index bb6904bc..c1306717 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -47,12 +47,19 @@ FixpositionDriverNode::FixpositionDriverNode(const DriverParams& params, ros::No driver_ { params_ }, nmea_epoch_data_ { params_.nmea_epoch_ } // clang-format on { - if (!llh_transformer_.Init(params_.datum_llh_enabled_, params_.datum_llh_ecef_crs_, params_.datum_llh_llh_crs_)) { - throw std::runtime_error("Failed initializing PROJ transformation: " + llh_transformer_.error()); +#if FPSDK_USE_PROJ + std::string error; + if (!InitLlhTrafo(params_, llh_trafo_, error)) { + throw std::runtime_error("Failed initializing PROJ transformation: " + error); } - if (llh_transformer_.enabled()) { + if (llh_trafo_) { ROS_INFO("PROJ enabled for ECEF to LLH output conversion"); } +#else + if (params_.datum_llh_enabled_) { + throw std::runtime_error("Failed initializing PROJ transformation: driver was built without PROJ support"); + } +#endif } FixpositionDriverNode::~FixpositionDriverNode() { StopNode(); } @@ -89,7 +96,11 @@ bool FixpositionDriverNode::StartNode() { auto odometry_payload = dynamic_cast(payload); PublishFpaOdometry(odometry_payload, fpa_odometry_pub_); PublishFpaOdometryDataImu(odometry_payload, params_.nav2_mode_, poiimu_pub_); - PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, llh_transformer_, odometry_llh_pub_); +#if FPSDK_USE_PROJ + PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, llh_trafo_.get(), odometry_llh_pub_); +#else + PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, odometry_llh_pub_); +#endif OdometryData odometry_data; odometry_data.SetFromFpaOdomPayload(odometry_payload); PublishOdometryData(odometry_data, odometry_ecef_pub_); @@ -710,7 +721,11 @@ void FixpositionDriverNode::PublishNav2Tf() { tf_br_.sendTransform(tf_odom_base); // Publish datum in the configured LLH CRS - PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, llh_transformer_, datum_pub_); +#if FPSDK_USE_PROJ + PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, llh_trafo_.get(), datum_pub_); +#else + PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, datum_pub_); +#endif } /* ****************************************************************************************************************** */ diff --git a/fixposition_driver_ros1/src/params.cpp b/fixposition_driver_ros1/src/params.cpp index a3e38046..43e7f4db 100644 --- a/fixposition_driver_ros1/src/params.cpp +++ b/fixposition_driver_ros1/src/params.cpp @@ -37,13 +37,9 @@ static bool LoadRosParam(const std::string& name, T& value) { return true; } -template -static bool LoadOptionalRosParam(const std::string& name, T& value) { +static bool HasRosParam(const std::string& name) { try { - if (!ros::param::has(name)) { - return true; - } - return ros::param::get(name, value); + return ros::param::has(name); } catch (ros::InvalidNameException& e) { return false; } @@ -94,19 +90,20 @@ bool LoadParamsFromRos1(const std::string& ns, DriverParams& params) { ROS_WARN("Failed loading %s/nav2_mode param", ns.c_str()); ok = false; } - // These parameters were added after the initial driver release. Keep the DriverParams defaults when they are - // absent so configurations created by older releases remain valid. - if (!LoadOptionalRosParam(ns + "/datum_llh/enabled", params.datum_llh_enabled_)) { - ROS_WARN("Failed loading %s/datum_llh/enabled param", ns.c_str()); - ok = false; - } - if (!LoadOptionalRosParam(ns + "/datum_llh/ecef_crs", params.datum_llh_ecef_crs_)) { - ROS_WARN("Failed loading %s/datum_llh/ecef_crs param", ns.c_str()); - ok = false; - } - if (!LoadOptionalRosParam(ns + "/datum_llh/llh_crs", params.datum_llh_llh_crs_)) { - ROS_WARN("Failed loading %s/datum_llh/llh_crs param", ns.c_str()); - ok = false; + // Optional, keep defaults if datum_llh absent + if (HasRosParam(ns + "/datum_llh")) { + if (!LoadRosParam(ns + "/datum_llh/enabled", params.datum_llh_enabled_)) { + ROS_WARN("Failed loading %s/datum_llh/enabled param", ns.c_str()); + ok = false; + } + if (!LoadRosParam(ns + "/datum_llh/ecef_crs", params.datum_llh_ecef_crs_)) { + ROS_WARN("Failed loading %s/datum_llh/ecef_crs param", ns.c_str()); + ok = false; + } + if (!LoadRosParam(ns + "/datum_llh/llh_crs", params.datum_llh_llh_crs_)) { + ROS_WARN("Failed loading %s/datum_llh/llh_crs param", ns.c_str()); + ok = false; + } } if (!LoadRosParam(ns + "/converter/enabled", params.converter_enabled_)) { ROS_WARN("Failed loading %s/converter/enabled param", ns.c_str()); diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp index ab3beee4..a71e18c8 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp @@ -20,7 +20,6 @@ /* EXTERNAL */ #include #include -#include #include #include @@ -40,7 +39,9 @@ void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& pa void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, rclcpp::Publisher::SharedPtr& pub); void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, - const LlhTransformer& llh_transformer, +#if FPSDK_USE_PROJ + const fpsdk::common::trafo::Transformer* llh_trafo, +#endif rclcpp::Publisher::SharedPtr& pub); void PublishFpaOdomenu(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, rclcpp::Publisher::SharedPtr& pub); @@ -106,7 +107,9 @@ void PublishNmeaEpochData(const NmeaEpochData& data, rclcpp::Publisher::SharedPtr& pub); void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher::SharedPtr& pub); void PublishDatum(const geometry_msgs::msg::Vector3& payload, const builtin_interfaces::msg::Time& stamp, - const LlhTransformer& llh_transformer, +#if FPSDK_USE_PROJ + const fpsdk::common::trafo::Transformer* llh_trafo, +#endif rclcpp::Publisher::SharedPtr& pub); void PublishFusionEpochData(const FusionEpochData& data, rclcpp::Publisher::SharedPtr& pub); diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 03267af9..e784fc76 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -66,7 +66,10 @@ class FixpositionDriverNode { rclcpp::Logger logger_; //!< Logger FixpositionDriver driver_; //!< Sensor driver rclcpp::QoS qos_settings_; //!< QoS settings - LlhTransformer llh_transformer_; //!< Optional PROJ-backed ECEF-to-LLH output conversion + +#if FPSDK_USE_PROJ + std::unique_ptr llh_trafo_; //!< ECEF to LLH, nullptr = built-in WGS84 +#endif // ROS publishers // - FP_A messages diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index 72e9281f..be0a42c3 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -154,7 +154,9 @@ void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, bool nav2 // --------------------------------------------------------------------------------------------------------------------- void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, - const LlhTransformer& llh_transformer, +#if FPSDK_USE_PROJ + const trafo::Transformer* llh_trafo, +#endif rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { sensor_msgs::msg::NavSatFix msg; @@ -176,9 +178,13 @@ void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, boo cov_map = Eigen::Matrix3d::Zero(); // FIXME: necessary? } else { Eigen::Vector3d llh_pos; - if (!llh_transformer.EcefToLlhRad(pose.position, llh_pos)) { +#if FPSDK_USE_PROJ + if (!EcefToLlhRad(llh_trafo, pose.position, llh_pos)) { return; } +#else + llh_pos = trafo::TfWgs84LlhEcef(pose.position); +#endif msg.latitude = math::RadToDeg(llh_pos(0)); msg.longitude = math::RadToDeg(llh_pos(1)); msg.altitude = llh_pos(2); @@ -889,7 +895,9 @@ void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { sensor_msgs::msg::NavSatFix msg; @@ -899,9 +907,13 @@ void PublishDatum(const geometry_msgs::msg::Vector3& payload, const builtin_inte // Populate LLH position const Eigen::Vector3d position = {payload.x, payload.y, payload.z}; Eigen::Vector3d llh_pos; - if (!llh_transformer.EcefToLlhRad(position, llh_pos)) { +#if FPSDK_USE_PROJ + if (!EcefToLlhRad(llh_trafo, position, llh_pos)) { return; } +#else + llh_pos = trafo::TfWgs84LlhEcef(position); +#endif msg.latitude = math::RadToDeg(llh_pos(0)); msg.longitude = math::RadToDeg(llh_pos(1)); msg.altitude = llh_pos(2); diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index b95a5c7f..e94426fe 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -68,12 +68,19 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr nh, qos_settings_ = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_default); } - if (!llh_transformer_.Init(params_.datum_llh_enabled_, params_.datum_llh_ecef_crs_, params_.datum_llh_llh_crs_)) { - throw std::runtime_error("Failed initializing PROJ transformation: " + llh_transformer_.error()); +#if FPSDK_USE_PROJ + std::string error; + if (!InitLlhTrafo(params_, llh_trafo_, error)) { + throw std::runtime_error("Failed initializing PROJ transformation: " + error); } - if (llh_transformer_.enabled()) { + if (llh_trafo_) { RCLCPP_INFO(logger_, "PROJ enabled for ECEF to LLH output conversion"); } +#else + if (params_.datum_llh_enabled_) { + throw std::runtime_error("Failed initializing PROJ transformation: driver was built without PROJ support"); + } +#endif } FixpositionDriverNode::~FixpositionDriverNode() {} @@ -114,7 +121,11 @@ bool FixpositionDriverNode::StartNode() { auto odometry_payload = dynamic_cast(payload); PublishFpaOdometry(odometry_payload, fpa_odometry_pub_); PublishFpaOdometryDataImu(odometry_payload, params_.nav2_mode_, poiimu_pub_); - PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, llh_transformer_, odometry_llh_pub_); +#if FPSDK_USE_PROJ + PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, llh_trafo_.get(), odometry_llh_pub_); +#else + PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, odometry_llh_pub_); +#endif OdometryData odometry_data; odometry_data.SetFromFpaOdomPayload(odometry_payload); PublishOdometryData(odometry_data, odometry_ecef_pub_); @@ -745,7 +756,11 @@ void FixpositionDriverNode::PublishNav2Tf() { tf_br_->sendTransform(tf_odom_base); // Publish datum in the configured LLH CRS - PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, llh_transformer_, datum_pub_); +#if FPSDK_USE_PROJ + PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, llh_trafo_.get(), datum_pub_); +#else + PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, datum_pub_); +#endif } /* ****************************************************************************************************************** */ From 08f54bdcbd1cae6e088b01b8ecac2c0d0542c7a4 Mon Sep 17 00:00:00 2001 From: flipflip Date: Thu, 29 Oct 2026 11:13:16 +0100 Subject: [PATCH 3/3] bump sdk --- fixposition-sdk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fixposition-sdk b/fixposition-sdk index f08c7835..3a664467 160000 --- a/fixposition-sdk +++ b/fixposition-sdk @@ -1 +1 @@ -Subproject commit f08c7835320f0408a1f18b1ca3c0d64e6caec4bf +Subproject commit 3a664467b64a69a47ba292ed00f346232f75f0fe