Added support for proj to use custom geodetic datums and orthometric height

fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp

@@ -104,6 +104,7 @@ struct OdometryData {
     TwistWithCovData twist;
     bool SetFromFpaOdomPayload(const fpsdk::common::parser::fpa::FpaOdomPayload& payload);
     bool ConvertToEnu(const TfData& tf_ecef_enu0);
+    bool ConvertToEnu(const TfData& tf_ecef_enu0, const Eigen::Vector3d& wgs84llh_ref);
 };
 
 /**

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 proj_enabled_ = false;
+    std::string proj_ecef_crs_ = "EPSG:4936";
+    std::string proj_llh_crs_ = "EPSG:4326";
 
     enum class VelTopicType { UNSPECIFIED, TWIST, TWISTWITHCOV, ODOMETRY };
     bool converter_enabled_ = false;

fixposition_driver_lib/src/helper.cpp

@@ -150,6 +150,13 @@ bool OdometryData::SetFromFpaOdomPayload(const fpa::FpaOdomPayload& payload) {
 // ---------------------------------------------------------------------------------------------------------------------
 
 bool OdometryData::ConvertToEnu(const TfData& tf_ecef_enu0) {
+    const Eigen::Vector3d wgs84llh_ref = fpsdk::common::trafo::TfWgs84LlhEcef(tf_ecef_enu0.translation);
+    return ConvertToEnu(tf_ecef_enu0, wgs84llh_ref);
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+bool OdometryData::ConvertToEnu(const TfData& tf_ecef_enu0, const Eigen::Vector3d& wgs84llh_ref) {
     // Check that TF data is valid
     if (!tf_ecef_enu0.valid) {
         return false;
@@ -161,13 +168,11 @@ bool OdometryData::ConvertToEnu(const TfData& tf_ecef_enu0) {
     const Eigen::Matrix<double, 6, 6> cov_ecef = pose.cov;
 
     // Extract data from the TF message (using the arrow operator)
-    const Eigen::Vector3d t_ecef_enu0 = tf_ecef_enu0.translation;
     const Eigen::Quaterniond q_ecef_enu0 = tf_ecef_enu0.rotation;
     const Eigen::Matrix3d rot_ecef_enu0 = q_ecef_enu0.toRotationMatrix();
 
     // Convert position in ECEF into position in ENU
-    const Eigen::Vector3d t_enu_body =
-        fpsdk::common::trafo::TfEnuEcef(t_ecef_body, fpsdk::common::trafo::TfWgs84LlhEcef(t_ecef_enu0));
+    const Eigen::Vector3d 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

fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp

@@ -16,13 +16,15 @@
 
 /* LIBC/STL */
 #include <memory>
+#include <string>
 #include <unordered_map>
 
 /* EXTERNAL */
 #include <fixposition_driver_lib/fixposition_driver.hpp>
 #include <fixposition_driver_lib/helper.hpp>
 #include <fpsdk_common/parser/fpa.hpp>
 #include <fpsdk_common/parser/novb.hpp>
+#include <fpsdk_common/trafo.hpp>
 #include <fpsdk_ros1/ext/ros.hpp>
 
 /* PACKAGE */
@@ -31,14 +33,28 @@
 namespace fixposition {
 /* ****************************************************************************************************************** */
 
+class LlhTransformer {
+   public:
+    bool Init(bool enabled, const std::string& ecef_crs, const std::string& llh_crs);
+    bool EcefToLlhRad(const Eigen::Vector3d& ecef, Eigen::Vector3d& llh_rad) const;
+    bool LlhRadToEcef(const Eigen::Vector3d& llh_rad, Eigen::Vector3d& ecef) const;
+    bool enabled() const { return proj_enabled_; }
+
+   private:
+    bool proj_enabled_ = false;
+#if FPSDK_USE_PROJ
+    std::unique_ptr<fpsdk::common::trafo::Transformer> transformer_;
+#endif
+};
+
 void TfDataToTransformStamped(const TfData& data, geometry_msgs::TransformStamped& msg);
 void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::TransformStamped& msg);
 
 void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, ros::Publisher& pub);
 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);
@@ -76,7 +92,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);
 
 /* ****************************************************************************************************************** */

fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp

@@ -58,9 +58,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 LLH/ECEF conversions
 
     // ROS publishers
     // - FP_A messages

fixposition_driver_ros1/launch/config.yaml

@@ -54,6 +54,11 @@ raw_output:   false    # Enable raw messages output
 cov_warning:  false    # Enable covariance warnings
 nav2_mode:    false    # Enable nav2 mode
 
+proj:
+    enabled: false              # Use PROJ for ECEF/LLH conversions
+    ecef_crs: "EPSG:4978"       # Source CRS for ECEF coordinates
+    llh_crs: "EPSG:4979"        # Target CRS for LLH coordinates. Note: +3855 adds height above ellipsoid
+
 converter:
     enabled: false
     topic_type: "Twist"        # Supported types: nav_msgs/{Twist, TwistWithCov, Odometry}

fixposition_driver_ros1/src/data_to_ros1.cpp

@@ -34,6 +34,66 @@ using namespace fpsdk::common::parser;
 
 // ---------------------------------------------------------------------------------------------------------------------
 
+bool LlhTransformer::Init(bool enabled, const std::string& ecef_crs, const std::string& llh_crs) {
+    proj_enabled_ = false;
+#if FPSDK_USE_PROJ
+    transformer_.reset();
+#endif
+    if (!enabled) {
+        return true;
+    }
+#if FPSDK_USE_PROJ
+    auto transformer = std::make_unique<trafo::Transformer>("fixposition_driver_llh");
+    if (!transformer->Init(ecef_crs, llh_crs)) {
+        return false;
+    }
+    transformer_ = std::move(transformer);
+    proj_enabled_ = true;
+    return true;
+#else
+    (void)ecef_crs;
+    (void)llh_crs;
+    return false;
+#endif
+}
+
+bool LlhTransformer::EcefToLlhRad(const Eigen::Vector3d& ecef, Eigen::Vector3d& llh_rad) const {
+    if (proj_enabled_) {
+#if FPSDK_USE_PROJ
+        if (!transformer_) {
+            return false;
+        }
+        Eigen::Vector3d llh_deg;
+        if (!transformer_->Transform(ecef, llh_deg)) {
+            return false;
+        }
+        llh_rad = trafo::LlhDegToRad(llh_deg);
+        return true;
+#endif
+    }
+    llh_rad = trafo::TfWgs84LlhEcef(ecef);
+    return true;
+}
+
+bool LlhTransformer::LlhRadToEcef(const Eigen::Vector3d& llh_rad, Eigen::Vector3d& ecef) const {
+    if (proj_enabled_) {
+#if FPSDK_USE_PROJ
+        if (!transformer_) {
+            return false;
+        }
+        const Eigen::Vector3d llh_deg = trafo::LlhRadToDeg(llh_rad);
+        if (!transformer_->Transform(llh_deg, ecef, true)) {
+            return false;
+        }
+        return true;
+#endif
+    }
+    ecef = trafo::TfEcefWgs84Llh(llh_rad);
+    return true;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
 static void PoseWithCovDataToMsg(const PoseWithCovData& data, geometry_msgs::PoseWithCovariance& msg) {
     tf::pointEigenToMsg(data.position, msg.pose.position);
     tf::quaternionEigenToMsg(data.orientation, msg.pose.orientation);
@@ -152,7 +212,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::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
@@ -172,14 +233,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 || !llh_transformer->EcefToLlhRad(pose.position, llh_pos)) {
+                llh_pos = trafo::TfWgs84LlhEcef(pose.position);
+            }
             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;
@@ -860,15 +924,19 @@ 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;
         msg.header.frame_id = "vrtk_link";
 
         // 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 || !llh_transformer->EcefToLlhRad(position, llh_pos)) {
+            llh_pos = trafo::TfWgs84LlhEcef(position);
+        }
         msg.latitude = math::RadToDeg(llh_pos(0));
         msg.longitude = math::RadToDeg(llh_pos(1));
         msg.altitude = llh_pos(2);

fixposition_driver_ros1/src/fixposition_driver_node.cpp

@@ -44,7 +44,21 @@ FixpositionDriverNode::FixpositionDriverNode(const DriverParams& params, ros::No
     params_            { params },
     driver_            { params_ },
     nmea_epoch_data_   { params_.nmea_epoch_ }  // clang-format on
-{}
+{
+    const bool proj_ok = llh_transformer_.Init(params_.proj_enabled_, params_.proj_ecef_crs_, params_.proj_llh_crs_);
+    if (params_.proj_enabled_) {
+#if FPSDK_USE_PROJ
+        if (!proj_ok) {
+            ROS_WARN("PROJ enabled but failed to initialize, falling back to WGS84 conversions");
+        } else {
+            ROS_INFO("PROJ enabled for ECEF/LLH conversions");
+        }
+#else
+        (void)proj_ok;
+        ROS_WARN("PROJ requested but FPSDK_USE_PROJ is disabled, falling back to WGS84 conversions");
+#endif
+    }
+}
 
 FixpositionDriverNode::~FixpositionDriverNode() { StopNode(); }
 
@@ -80,7 +94,7 @@ bool FixpositionDriverNode::StartNode() {
             auto odometry_payload = dynamic_cast<const fpa::FpaOdometryPayload&>(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_);
@@ -112,7 +126,11 @@ bool FixpositionDriverNode::StartNode() {
 
             // Convert message to ENU
             if (ecef_enu0_tf_) {
-                bool enu_valid = odometry_data.ConvertToEnu(*ecef_enu0_tf_);
+                Eigen::Vector3d llh_ref;
+                if (!llh_transformer_.EcefToLlhRad(ecef_enu0_tf_->translation, llh_ref)) {
+                    llh_ref = trafo::TfWgs84LlhEcef(ecef_enu0_tf_->translation);
+                }
+                bool enu_valid = odometry_data.ConvertToEnu(*ecef_enu0_tf_, llh_ref);
                 if (enu_valid) {
                     PublishOdometryData(odometry_data, odometry_enu_smooth_pub_);
                 }
@@ -667,7 +685,11 @@ void FixpositionDriverNode::PublishNav2Tf() {
     Eigen::Quaterniond q_ecef_poish(rot_ecef_poish.w, rot_ecef_poish.x, rot_ecef_poish.y, rot_ecef_poish.z);
 
     // Compute the ENU transformation
-    const Eigen::Vector3d t_enu0_poish = trafo::TfEnuEcef(t_ecef_poish, trafo::TfWgs84LlhEcef(t_ecef_enu0_));
+    Eigen::Vector3d llh_enu0;
+    if (!llh_transformer_.EcefToLlhRad(t_ecef_enu0_, llh_enu0)) {
+        llh_enu0 = trafo::TfWgs84LlhEcef(t_ecef_enu0_);
+    }
+    const Eigen::Vector3d t_enu0_poish = trafo::TfEnuEcef(t_ecef_poish, llh_enu0);
     const Eigen::Quaterniond q_enu0_poish = q_ecef_enu0_.inverse() * q_ecef_poish;
 
     // Create tf2::Transform tf_ENU0POISH
@@ -701,7 +723,7 @@ void FixpositionDriverNode::PublishNav2Tf() {
     tf_br_.sendTransform(tf_odom_base);
 
     // Publish WGS84 datum
-    PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, datum_pub_);
+    PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, &llh_transformer_, datum_pub_);
 }
 
 /* ****************************************************************************************************************** */

fixposition_driver_ros1/src/params.cpp

@@ -72,6 +72,18 @@ bool LoadParamsFromRos1(const std::string& ns, DriverParams& params) {
         ROS_WARN("Failed loading %s/nav2_mode param", ns.c_str());
         ok = false;
     }
+    if (!utils::LoadRosParam(ns + "/proj/enabled", params.proj_enabled_)) {
+        ROS_WARN("Failed loading %s/proj/enabled param", ns.c_str());
+        ok = false;
+    }
+    if (!utils::LoadRosParam(ns + "/proj/ecef_crs", params.proj_ecef_crs_)) {
+        ROS_WARN("Failed loading %s/proj/ecef_crs param", ns.c_str());
+        ok = false;
+    }
+    if (!utils::LoadRosParam(ns + "/proj/llh_crs", params.proj_llh_crs_)) {
+        ROS_WARN("Failed loading %s/proj/llh_crs param", ns.c_str());
+        ok = false;
+    }
     if (!utils::LoadRosParam(ns + "/converter/enabled", params.converter_enabled_)) {
         ROS_WARN("Failed loading %s/converter/enabled param", ns.c_str());
         ok = false;
@@ -136,6 +148,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: proj_enabled=%s", params.proj_enabled_ ? "true" : "false");
+    ROS_INFO("DriverParams: proj_ecef_crs=%s", params.proj_ecef_crs_.c_str());
+    ROS_INFO("DriverParams: proj_llh_crs=%s", params.proj_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());

fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp

@@ -16,13 +16,15 @@
 
 /* LIBC/STL */
 #include <memory>
+#include <string>
 #include <unordered_map>
 
 /* EXTERNAL */
 #include <fixposition_driver_lib/fixposition_driver.hpp>
 #include <fixposition_driver_lib/helper.hpp>
 #include <fpsdk_common/parser/fpa.hpp>
 #include <fpsdk_common/parser/novb.hpp>
+#include <fpsdk_common/trafo.hpp>
 #include <fpsdk_ros2/ext/rclcpp.hpp>
 
 /* PACKAGE */
@@ -31,6 +33,20 @@
 namespace fixposition {
 /* ****************************************************************************************************************** */
 
+class LlhTransformer {
+   public:
+    bool Init(bool enabled, const std::string& ecef_crs, const std::string& llh_crs);
+    bool EcefToLlhRad(const Eigen::Vector3d& ecef, Eigen::Vector3d& llh_rad) const;
+    bool LlhRadToEcef(const Eigen::Vector3d& llh_rad, Eigen::Vector3d& ecef) const;
+    bool enabled() const { return proj_enabled_; }
+
+   private:
+    bool proj_enabled_ = false;
+#if FPSDK_USE_PROJ
+    std::unique_ptr<fpsdk::common::trafo::Transformer> transformer_;
+#endif
+};
+
 void TfDataToTransformStamped(const TfData& data, geometry_msgs::msg::TransformStamped& msg);
 void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::msg::TransformStamped& msg);
 
@@ -39,6 +55,7 @@ void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& pa
 void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_,
                                rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr& pub);
 void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_,
+                                     const LlhTransformer* llh_transformer,
                                      rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr& pub);
 void PublishFpaOdomenu(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload,
                        rclcpp::Publisher<fpmsgs::FpaOdomenu>::SharedPtr& pub);
@@ -104,6 +121,7 @@ void PublishNmeaEpochData(const NmeaEpochData& data, rclcpp::Publisher<fpmsgs::N
 void PublishOdometryData(const OdometryData& data, rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr& pub);
 void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher<fpmsgs::CovWarn>::SharedPtr& pub);
 void PublishDatum(const geometry_msgs::msg::Vector3& payload, const builtin_interfaces::msg::Time& stamp,
+                  const LlhTransformer* llh_transformer,
                   rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr& pub);
 void PublishFusionEpochData(const FusionEpochData& data, rclcpp::Publisher<fpmsgs::FusionEpoch>::SharedPtr& pub);