From d6e8ff6a068f1d4f2691de0d2ad56632e6a8ac81 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 11 Sep 2026 18:45:00 +0200 Subject: [PATCH] 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 | 6 + fixposition_driver_ros1/src/data_to_ros1.cpp | 18 +- .../src/fixposition_driver_node.cpp | 16 +- fixposition_driver_ros1/src/params.cpp | 15 ++ .../fixposition_driver_ros2/data_to_ros2.hpp | 4 +- .../fixposition_driver_node.hpp | 1 + fixposition_driver_ros2/launch/config.yaml | 6 + fixposition_driver_ros2/src/data_to_ros2.cpp | 14 +- .../src/fixposition_driver_node.cpp | 14 +- fixposition_driver_ros2/src/params.cpp | 21 ++ 19 files changed, 547 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 759deea7..4edd2234 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..282dad29 100644 --- a/fixposition_driver_ros1/launch/config.yaml +++ b/fixposition_driver_ros1/launch/config.yaml @@ -54,6 +54,12 @@ raw_output: false # Enable raw messages output cov_warning: false # Enable covariance warnings nav2_mode: false # Enable nav2 mode +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 a82cbdc9..6b925a07 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; @@ -863,7 +867,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; @@ -871,7 +876,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..a6aa4030 100644 --- a/fixposition_driver_ros1/src/params.cpp +++ b/fixposition_driver_ros1/src/params.cpp @@ -82,6 +82,18 @@ bool LoadParamsFromRos1(const std::string& ns, DriverParams& params) { ROS_WARN("Failed loading %s/nav2_mode param", ns.c_str()); ok = false; } + 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()); ok = false; @@ -146,6 +158,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..f82044a2 100644 --- a/fixposition_driver_ros2/launch/config.yaml +++ b/fixposition_driver_ros2/launch/config.yaml @@ -56,6 +56,12 @@ cov_warning: false # Enable covariance warnings nav2_mode: false # Enable nav2 mode + 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 4ecaa879..90f6df8f 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; @@ -884,6 +888,7 @@ void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { sensor_msgs::msg::NavSatFix msg; @@ -892,7 +897,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..b942a779 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,9 @@ 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_); + nh->declare_parameter(DATUM_LLH_ENABLED, params.datum_llh_enabled_); + nh->declare_parameter(DATUM_LLH_ECEF_CRS, params.datum_llh_ecef_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_); @@ -111,6 +117,18 @@ bool LoadParamsFromRos2(std::shared_ptr& nh, DriverParams& params) RCLCPP_WARN(logger, "Failed loading %s param", NAV2_MODE.c_str()); ok = false; } + if (!nh->get_parameter(DATUM_LLH_ENABLED, params.datum_llh_enabled_)) { + RCLCPP_WARN(logger, "Failed loading %s param", DATUM_LLH_ENABLED.c_str()); + ok = false; + } + if (!nh->get_parameter(DATUM_LLH_ECEF_CRS, params.datum_llh_ecef_crs_)) { + RCLCPP_WARN(logger, "Failed loading %s param", DATUM_LLH_ECEF_CRS.c_str()); + ok = false; + } + if (!nh->get_parameter(DATUM_LLH_LLH_CRS, params.datum_llh_llh_crs_)) { + RCLCPP_WARN(logger, "Failed loading %s param", DATUM_LLH_LLH_CRS.c_str()); + ok = false; + } if (!nh->get_parameter(CONVERTER_ENABLED, params.converter_enabled_)) { RCLCPP_WARN(logger, "Failed loading %s param", CONVERTER_ENABLED.c_str()); ok = false; @@ -178,6 +196,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());