From 021b0f205b1f88b934b2a7edd1d4d21aa26c8513 Mon Sep 17 00:00:00 2001
From: Facundo Garcia <facundo.garcia@fixposition.com>
Date: Thu, 11 Jan 2024 10:32:11 +0100
Subject: [PATCH 1/9] Added gpgga to ROS1

---
 fixposition_driver_lib/CMakeLists.txt         |  1 +
 .../converter/gpgga.hpp                       | 65 ++++++++++++++++
 .../src/fixposition_driver.cpp                | 15 +++-
 fixposition_driver_lib/src/gpgga.cpp          | 75 +++++++++++++++++++
 .../fixposition_driver_node.hpp               |  1 +
 fixposition_driver_ros1/launch/tcp.yaml       |  2 +-
 .../src/fixposition_driver_node.cpp           | 11 +++
 fixposition_driver_ros2/CATKIN_IGNORE         |  0
 .../fixposition_driver_node.hpp               |  1 +
 .../src/fixposition_driver_node.cpp           | 11 +++
 10 files changed, 179 insertions(+), 3 deletions(-)
 create mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp
 create mode 100644 fixposition_driver_lib/src/gpgga.cpp
 create mode 100644 fixposition_driver_ros2/CATKIN_IGNORE

diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt
index 6df5040..859d3c3 100644
--- a/fixposition_driver_lib/CMakeLists.txt
+++ b/fixposition_driver_lib/CMakeLists.txt
@@ -29,6 +29,7 @@ add_library(
   src/tf.cpp
   src/helper.cpp
   src/parser.cpp
+  src/gpgga.cpp
 )
 
 target_link_libraries(${PROJECT_NAME} ${fixposition_gnss_tf_LIBRARIES} ${Boost_LIBRARIES} pthread)
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp
new file mode 100644
index 0000000..83fbb3a
--- /dev/null
+++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp
@@ -0,0 +1,65 @@
+/**
+ *  @file
+ *  @brief Declaration of GpggaConverter
+ *
+ * \verbatim
+ *  ___    ___
+ *  \  \  /  /
+ *   \  \/  /   Fixposition AG
+ *   /  /\  \   All right reserved.
+ *  /__/  \__\
+ * \endverbatim
+ *
+ */
+
+#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__
+#define __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__
+
+/* SYSTEM / STL */
+
+/* EXTERNAL */
+
+/* PACKAGE */
+#include <fixposition_driver_lib/converter/base_converter.hpp>
+#include <fixposition_driver_lib/msg_data.hpp>
+#include <fixposition_driver_lib/time_conversions.hpp>
+
+namespace fixposition {
+
+class GpggaConverter : public BaseAsciiConverter {
+   public:
+    using GpggaObserver = std::function<void(const NavSatFixData&)>;
+    /**
+     * @brief Construct a new GpggaConverter
+     *
+     */
+    GpggaConverter() {}
+
+    ~GpggaConverter() = default;
+
+    /**
+     * @brief Take comma-delimited tokens of GPGGA message, convert to Data structs and if available,
+     * call observers
+     * Example:
+     * $GPGGA,151229.40,4723.54108,N,00826.88485,E,4,12,00.98,473.5,M,,,,*3A\r\n
+     *
+     * @param[in] tokens message split in tokens
+     */
+    void ConvertTokens(const std::vector<std::string>& tokens) final;
+
+    /**
+     * @brief Add Observer to call at the end of ConvertTokens()
+     *
+     * @param[in] ob
+     */
+    void AddObserver(GpggaObserver ob) { obs_.push_back(ob); }
+
+   private:
+    NavSatFixData msg_;
+    std::vector<GpggaObserver> obs_;
+    const std::string header_ = "LLH";
+    static constexpr const int kVersion_ = 1;
+    static constexpr const int kSize_ = 14;
+};
+}  // namespace fixposition
+#endif  // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__
diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp
index 3901539..a53711c 100644
--- a/fixposition_driver_lib/src/fixposition_driver.cpp
+++ b/fixposition_driver_lib/src/fixposition_driver.cpp
@@ -22,6 +22,7 @@
 #include <fixposition_driver_lib/converter/llh.hpp>
 #include <fixposition_driver_lib/converter/odometry.hpp>
 #include <fixposition_driver_lib/converter/tf.hpp>
+#include <fixposition_driver_lib/converter/gpgga.hpp>
 #include <fixposition_driver_lib/fixposition_driver.hpp>
 #include <fixposition_driver_lib/helper.hpp>
 #include <fixposition_driver_lib/parser.hpp>
@@ -137,6 +138,8 @@ bool FixpositionDriver::InitializeConverters() {
             a_converters_["RAWIMU"] = std::unique_ptr<ImuConverter>(new ImuConverter(false));
         } else if (format == "CORRIMU") {
             a_converters_["CORRIMU"] = std::unique_ptr<ImuConverter>(new ImuConverter(true));
+        } else if (format == "GPGGA") {
+            a_converters_["GPGGA"] = std::unique_ptr<GpggaConverter>(new GpggaConverter());
         } else if (format == "TF") {
             if (a_converters_.find("TF") == a_converters_.end()) {
                 a_converters_["TF"] = std::unique_ptr<TfConverter>(new TfConverter());
@@ -230,12 +233,20 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) {
     SplitMessage(tokens, msg.substr(1, star_pos - 1), ",");
 
     // if it doesn't start with FP then do nothing
-    if (tokens.at(0) != "FP") {
+    if ((tokens.at(0) != "FP") || (tokens.at(0) != "GPGGA")) {
         return;
     }
 
     // Get the header of the sentence
-    const std::string header = tokens.at(1);
+    std::string _header;
+    if (tokens.at(0) == "GPGGA") {
+        _header = "GPGGA";
+    } else {
+        _header = tokens.at(1);
+    }
+    const std::string header = _header;
+    std::cout << "Header: " << _header << std::endl;
+    std::cout << "Header: " << header << std::endl;
 
     // If we have a converter available, convert to ros. Currently supported are "FP", "LLH", "TF", "RAWIMU", "CORRIMU"
     if (a_converters_[header] != nullptr) {
diff --git a/fixposition_driver_lib/src/gpgga.cpp b/fixposition_driver_lib/src/gpgga.cpp
new file mode 100644
index 0000000..bfeb29a
--- /dev/null
+++ b/fixposition_driver_lib/src/gpgga.cpp
@@ -0,0 +1,75 @@
+/**
+ *  @file
+ *  @brief Implementation of GpggaConverter converter
+ *
+ * \verbatim
+ *  ___    ___
+ *  \  \  /  /
+ *   \  \/  /   Fixposition AG
+ *   /  /\  \   All right reserved.
+ *  /__/  \__\
+ * \endverbatim
+ *
+ */
+
+/* SYSTEM / STL */
+#include <iostream>
+
+/* PACKAGE */
+#include <fixposition_driver_lib/converter/gpgga.hpp>
+
+namespace fixposition {
+
+/// msg field indices
+static constexpr const int time_idx = 1;
+static constexpr const int lat_idx = 2;
+static constexpr const int lat_ns_idx = 3;
+static constexpr const int lon_idx = 4;
+static constexpr const int lon_ew_idx = 5;
+static constexpr const int quality_idx = 6;
+static constexpr const int num_sv_idx = 7;
+static constexpr const int hdop_idx = 8;
+static constexpr const int alt_idx = 9;
+static constexpr const int alt_unit_idx = 10;
+static constexpr const int sep_idx = 11;
+static constexpr const int sep_unit_idx = 12;
+static constexpr const int diff_age_idx = 13;
+static constexpr const int diff_sta_idx = 14;
+
+void GpggaConverter::ConvertTokens(const std::vector<std::string>& tokens) {
+    // Check if message size is wrong
+    bool ok = tokens.size() == kSize_;
+    if (!ok) {
+        std::cout << "Error in parsing GPGGA string with " << tokens.size() << " fields! GPGGA message will be empty.\n";
+        msg_ = NavSatFixData();
+        return;
+    }
+
+    // Header stamps
+    msg_.stamp = ConvertGpsTime(tokens.at(time_idx), tokens.at(time_idx));
+    msg_.frame_id = "LLH";
+
+    // LLH coordinates
+    msg_.latitude = StringToDouble(tokens.at(lat_idx));
+    msg_.longitude = StringToDouble(tokens.at(lon_idx));
+    msg_.altitude = StringToDouble(tokens.at(alt_idx));
+
+    // Covariance diagonals
+    const double hdop = StringToDouble(tokens.at(hdop_idx));
+    msg_.cov(0, 0) = hdop * hdop;
+    msg_.cov(1, 1) = hdop * hdop;
+    msg_.cov(2, 2) = 4 * hdop * hdop;
+
+    // Rest of covariance fields
+    msg_.cov(0, 1) = msg_.cov(1, 0) = 0.0;
+    msg_.cov(0, 2) = msg_.cov(2, 0) = 0.0;
+    msg_.cov(1, 2) = msg_.cov(2, 1) = 0.0;
+    msg_.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED
+
+    // Process all observers
+    for (auto& ob : obs_) {
+        ob(msg_);
+    }
+}
+
+}  // namespace fixposition
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 064ea13..c58625e 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
@@ -69,6 +69,7 @@ class FixpositionDriverNode : public FixpositionDriver {
     ros::Publisher navsatfix_pub_;
     ros::Publisher navsatfix_gnss1_pub_;
     ros::Publisher navsatfix_gnss2_pub_;
+    ros::Publisher navsatfix_gpgga_pub_;
     ros::Publisher odometry_pub_;       //!< ECEF Odometry
     ros::Publisher poiimu_pub_;         //!< Bias corrected IMU
     ros::Publisher vrtk_pub_;           //!< VRTK message
diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml
index f9165b6..9d535e2 100644
--- a/fixposition_driver_ros1/launch/tcp.yaml
+++ b/fixposition_driver_ros1/launch/tcp.yaml
@@ -1,5 +1,5 @@
 fp_output:
-    formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "TF"]
+    formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "TF"]
     type: tcp
     port: "21000"
     ip: "10.0.2.1" # change to VRTK2's IP address in the network
diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
index 49a9225..adb646b 100644
--- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp
+++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
@@ -22,6 +22,7 @@
 #include <fixposition_driver_lib/converter/llh.hpp>
 #include <fixposition_driver_lib/converter/odometry.hpp>
 #include <fixposition_driver_lib/converter/tf.hpp>
+#include <fixposition_driver_lib/converter/gpgga.hpp>
 #include <fixposition_driver_lib/fixposition_driver.hpp>
 #include <fixposition_driver_lib/helper.hpp>
 #include <fixposition_gnss_tf/gnss_tf.hpp>
@@ -41,6 +42,7 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para
       navsatfix_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/navsatfix", 100)),
       navsatfix_gnss1_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/gnss1", 100)),
       navsatfix_gnss2_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/gnss2", 100)),
+      navsatfix_gpgga_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/gpgga", 100)),
       //   ODOMETRY
       odometry_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry", 100)),
       poiimu_pub_(nh_.advertise<sensor_msgs::Imu>("/fixposition/poiimu", 100)),
@@ -154,6 +156,15 @@ void FixpositionDriverNode::RegisterObservers() {
                     static_br_.sendTransform(tf);
                 }
             });
+        } else if (format == "GPGGA") {
+            dynamic_cast<GpggaConverter*>(a_converters_["GPGGA"].get())->AddObserver([this](const NavSatFixData& data) {
+                // GPGGA Observer Lambda
+                if (navsatfix_gpgga_pub_.getNumSubscribers() > 0) {
+                    sensor_msgs::NavSatFix msg;
+                    NavSatFixDataToMsg(data, msg);
+                    navsatfix_gpgga_pub_.publish(msg);
+                }
+            });
         }
     }
 }
diff --git a/fixposition_driver_ros2/CATKIN_IGNORE b/fixposition_driver_ros2/CATKIN_IGNORE
new file mode 100644
index 0000000..e69de29
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 609ca39..a01336d 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
@@ -72,6 +72,7 @@ class FixpositionDriverNode : public FixpositionDriver {
     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_pub_;
     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_gnss1_pub_;
     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_gnss2_pub_;
+    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_gpgga_pub_;
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_pub_;
     rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr poiimu_pub_;             //!< Bias corrected IMU from ODOMETRY
     rclcpp::Publisher<fixposition_driver_ros2::msg::VRTK>::SharedPtr vrtk_pub_;  //!< VRTK message
diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
index 0046534..7a2edff 100644
--- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp
+++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
@@ -24,6 +24,7 @@
 #include <fixposition_driver_lib/converter/llh.hpp>
 #include <fixposition_driver_lib/converter/odometry.hpp>
 #include <fixposition_driver_lib/converter/tf.hpp>
+#include <fixposition_driver_lib/converter/gpgga.hpp>
 #include <fixposition_driver_lib/fixposition_driver.hpp>
 #include <fixposition_driver_lib/helper.hpp>
 #include <fixposition_gnss_tf/gnss_tf.hpp>
@@ -43,6 +44,7 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr<rclcpp::Node> node,
       navsatfix_pub_(node_->create_publisher<sensor_msgs::msg::NavSatFix>("/fixposition/navsatfix", 100)),
       navsatfix_gnss1_pub_(node_->create_publisher<sensor_msgs::msg::NavSatFix>("/fixposition/gnss1", 100)),
       navsatfix_gnss2_pub_(node_->create_publisher<sensor_msgs::msg::NavSatFix>("/fixposition/gnss2", 100)),
+      navsatfix_gpgga_pub_(node_->create_publisher<sensor_msgs::msg::NavSatFix>("/fixposition/gpgga", 100)),
       odometry_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("/fixposition/odometry", 100)),
       poiimu_pub_(node_->create_publisher<sensor_msgs::msg::Imu>("/fixposition/poiimu", 100)),
       vrtk_pub_(node_->create_publisher<fixposition_driver_ros2::msg::VRTK>("/fixposition/vrtk", 100)),
@@ -183,6 +185,15 @@ void FixpositionDriverNode::RegisterObservers() {
                     static_br_->sendTransform(tf);
                 }
             });
+        } else if (format == "GPGGA") {
+            dynamic_cast<GpggaConverter*>(a_converters_["GPGGA"].get())->AddObserver([this](const NavSatFixData& data) {
+                // GPGGA Observer Lambda
+                if (navsatfix_gpgga_pub_.getNumSubscribers() > 0) {
+                    sensor_msgs::NavSatFix msg;
+                    NavSatFixDataToMsg(data, msg);
+                    navsatfix_gpgga_pub_.publish(msg);
+                }
+            });
         }
     }
 }

From 57891eb50ee783ffaae709f3535d8198aeb10f4e Mon Sep 17 00:00:00 2001
From: Facundo Garcia <facundo.garcia@fixposition.com>
Date: Thu, 11 Jan 2024 16:06:20 +0100
Subject: [PATCH 2/9] Patched implementation

---
 .../fixposition_driver_lib/converter/gpgga.hpp     |  3 +--
 fixposition_driver_lib/src/fixposition_driver.cpp  |  4 +---
 fixposition_driver_lib/src/gpgga.cpp               | 14 +++++++++++---
 3 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp
index 83fbb3a..5f13c33 100644
--- a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp
+++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp
@@ -58,8 +58,7 @@ class GpggaConverter : public BaseAsciiConverter {
     NavSatFixData msg_;
     std::vector<GpggaObserver> obs_;
     const std::string header_ = "LLH";
-    static constexpr const int kVersion_ = 1;
-    static constexpr const int kSize_ = 14;
+    static constexpr const int kSize_ = 15;
 };
 }  // namespace fixposition
 #endif  // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__
diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp
index a53711c..8e20580 100644
--- a/fixposition_driver_lib/src/fixposition_driver.cpp
+++ b/fixposition_driver_lib/src/fixposition_driver.cpp
@@ -233,7 +233,7 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) {
     SplitMessage(tokens, msg.substr(1, star_pos - 1), ",");
 
     // if it doesn't start with FP then do nothing
-    if ((tokens.at(0) != "FP") || (tokens.at(0) != "GPGGA")) {
+    if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA")) {
         return;
     }
 
@@ -245,8 +245,6 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) {
         _header = tokens.at(1);
     }
     const std::string header = _header;
-    std::cout << "Header: " << _header << std::endl;
-    std::cout << "Header: " << header << std::endl;
 
     // If we have a converter available, convert to ros. Currently supported are "FP", "LLH", "TF", "RAWIMU", "CORRIMU"
     if (a_converters_[header] != nullptr) {
diff --git a/fixposition_driver_lib/src/gpgga.cpp b/fixposition_driver_lib/src/gpgga.cpp
index bfeb29a..eebfe99 100644
--- a/fixposition_driver_lib/src/gpgga.cpp
+++ b/fixposition_driver_lib/src/gpgga.cpp
@@ -46,12 +46,20 @@ void GpggaConverter::ConvertTokens(const std::vector<std::string>& tokens) {
     }
 
     // Header stamps
-    msg_.stamp = ConvertGpsTime(tokens.at(time_idx), tokens.at(time_idx));
+    msg_.stamp = times::GpsTime(0, 0); // ConvertGpsTime(tokens.at(time_idx), tokens.at(time_idx));
     msg_.frame_id = "LLH";
 
     // LLH coordinates
-    msg_.latitude = StringToDouble(tokens.at(lat_idx));
-    msg_.longitude = StringToDouble(tokens.at(lon_idx));
+    const std::string _latstr = tokens.at(lat_idx);
+    double _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60;
+    if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1;
+    msg_.latitude = _lat;
+
+    const std::string _lonstr = tokens.at(lon_idx);
+    double _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60;
+    if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1;
+    msg_.longitude = _lon;
+
     msg_.altitude = StringToDouble(tokens.at(alt_idx));
 
     // Covariance diagonals

From 5ff34dbdda3728e0dcc39dd6cdffc1a00a6309d2 Mon Sep 17 00:00:00 2001
From: Facundo Garcia <facundo.garcia@fixposition.com>
Date: Fri, 12 Jan 2024 16:55:49 +0100
Subject: [PATCH 3/9] Added NMEA epoch and GPZDA handler

---
 fixposition_driver_lib/CMakeLists.txt         |   1 +
 .../converter/gpgga.hpp                       |   4 +-
 .../converter/gpzda.hpp                       |  65 +++++++++++
 .../fixposition_driver_lib/msg_data.hpp       |  19 +++
 .../src/fixposition_driver.cpp                |   7 +-
 fixposition_driver_lib/src/gpgga.cpp          |   5 +-
 fixposition_driver_lib/src/gpzda.cpp          | 108 ++++++++++++++++++
 .../fixposition_driver_node.hpp               |  20 +++-
 fixposition_driver_ros1/launch/tcp.yaml       |   2 +-
 .../src/fixposition_driver_node.cpp           |  43 ++++++-
 10 files changed, 260 insertions(+), 14 deletions(-)
 create mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/converter/gpzda.hpp
 create mode 100644 fixposition_driver_lib/src/gpzda.cpp

diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt
index 859d3c3..f1bf604 100644
--- a/fixposition_driver_lib/CMakeLists.txt
+++ b/fixposition_driver_lib/CMakeLists.txt
@@ -30,6 +30,7 @@ add_library(
   src/helper.cpp
   src/parser.cpp
   src/gpgga.cpp
+  src/gpzda.cpp
 )
 
 target_link_libraries(${PROJECT_NAME} ${fixposition_gnss_tf_LIBRARIES} ${Boost_LIBRARIES} pthread)
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp
index 5f13c33..98d339b 100644
--- a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp
+++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp
@@ -28,7 +28,7 @@ namespace fixposition {
 
 class GpggaConverter : public BaseAsciiConverter {
    public:
-    using GpggaObserver = std::function<void(const NavSatFixData&)>;
+    using GpggaObserver = std::function<void(const GpggaData&)>;
     /**
      * @brief Construct a new GpggaConverter
      *
@@ -55,7 +55,7 @@ class GpggaConverter : public BaseAsciiConverter {
     void AddObserver(GpggaObserver ob) { obs_.push_back(ob); }
 
    private:
-    NavSatFixData msg_;
+    GpggaData msg_;
     std::vector<GpggaObserver> obs_;
     const std::string header_ = "LLH";
     static constexpr const int kSize_ = 15;
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpzda.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpzda.hpp
new file mode 100644
index 0000000..a2447b2
--- /dev/null
+++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gpzda.hpp
@@ -0,0 +1,65 @@
+/**
+ *  @file
+ *  @brief Declaration of GpzdaConverter
+ *
+ * \verbatim
+ *  ___    ___
+ *  \  \  /  /
+ *   \  \/  /   Fixposition AG
+ *   /  /\  \   All right reserved.
+ *  /__/  \__\
+ * \endverbatim
+ *
+ */
+
+#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_GPZDA__
+#define __FIXPOSITION_DRIVER_LIB_CONVERTER_GPZDA__
+
+/* SYSTEM / STL */
+
+/* EXTERNAL */
+
+/* PACKAGE */
+#include <fixposition_driver_lib/converter/base_converter.hpp>
+#include <fixposition_driver_lib/msg_data.hpp>
+#include <fixposition_driver_lib/time_conversions.hpp>
+#include <chrono>
+
+namespace fixposition {
+
+class GpzdaConverter : public BaseAsciiConverter {
+   public:
+    using GpzdaObserver = std::function<void(const GpzdaData&)>;
+    /**
+     * @brief Construct a new GpzdaConverter
+     *
+     */
+    GpzdaConverter() {}
+
+    ~GpzdaConverter() = default;
+
+    /**
+     * @brief Take comma-delimited tokens of GPZDA message, convert to Data structs and if available,
+     * call observers
+     * Example:
+     * $GPZDA,090411.0001,10,10,2023,00,00*69\r\n
+     *
+     * @param[in] tokens message split in tokens
+     */
+    void ConvertTokens(const std::vector<std::string>& tokens) final;
+
+    /**
+     * @brief Add Observer to call at the end of ConvertTokens()
+     *
+     * @param[in] ob
+     */
+    void AddObserver(GpzdaObserver ob) { obs_.push_back(ob); }
+
+   private:
+    GpzdaData msg_;
+    std::vector<GpzdaObserver> obs_;
+    const std::string header_ = "LLH";
+    static constexpr const int kSize_ = 7;
+};
+}  // namespace fixposition
+#endif  // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPZDA__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp
index ecf9d57..8ce23f2 100644
--- a/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp
+++ b/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp
@@ -132,5 +132,24 @@ struct NavSatFixData {
     NavSatFixData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0) { cov.setZero(); }
 };
 
+struct GpggaData {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    std::string time;
+    double latitude;
+    double longitude;
+    double altitude;
+    Eigen::Matrix<double, 3, 3> cov;
+    int position_covariance_type;
+    GpggaData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0) { cov.setZero(); }
+};
+
+struct GpzdaData {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    std::string time;
+    std::string date;
+    times::GpsTime stamp;
+    GpzdaData() : time(""), date("") {}
+};
+
 }  // namespace fixposition
 #endif  //__FIXPOSITION_DRIVER_LIB_MSG_DATA__
diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp
index 8e20580..4c0cc03 100644
--- a/fixposition_driver_lib/src/fixposition_driver.cpp
+++ b/fixposition_driver_lib/src/fixposition_driver.cpp
@@ -23,6 +23,7 @@
 #include <fixposition_driver_lib/converter/odometry.hpp>
 #include <fixposition_driver_lib/converter/tf.hpp>
 #include <fixposition_driver_lib/converter/gpgga.hpp>
+#include <fixposition_driver_lib/converter/gpzda.hpp>
 #include <fixposition_driver_lib/fixposition_driver.hpp>
 #include <fixposition_driver_lib/helper.hpp>
 #include <fixposition_driver_lib/parser.hpp>
@@ -140,6 +141,8 @@ bool FixpositionDriver::InitializeConverters() {
             a_converters_["CORRIMU"] = std::unique_ptr<ImuConverter>(new ImuConverter(true));
         } else if (format == "GPGGA") {
             a_converters_["GPGGA"] = std::unique_ptr<GpggaConverter>(new GpggaConverter());
+        } else if (format == "GPZDA") {
+            a_converters_["GPZDA"] = std::unique_ptr<GpzdaConverter>(new GpzdaConverter());
         } else if (format == "TF") {
             if (a_converters_.find("TF") == a_converters_.end()) {
                 a_converters_["TF"] = std::unique_ptr<TfConverter>(new TfConverter());
@@ -233,7 +236,7 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) {
     SplitMessage(tokens, msg.substr(1, star_pos - 1), ",");
 
     // if it doesn't start with FP then do nothing
-    if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA")) {
+    if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA") && (tokens.at(0) != "GPZDA")) {
         return;
     }
 
@@ -241,6 +244,8 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) {
     std::string _header;
     if (tokens.at(0) == "GPGGA") {
         _header = "GPGGA";
+    } else if (tokens.at(0) == "GPZDA") {
+        _header = "GPZDA";
     } else {
         _header = tokens.at(1);
     }
diff --git a/fixposition_driver_lib/src/gpgga.cpp b/fixposition_driver_lib/src/gpgga.cpp
index eebfe99..52e129a 100644
--- a/fixposition_driver_lib/src/gpgga.cpp
+++ b/fixposition_driver_lib/src/gpgga.cpp
@@ -41,13 +41,12 @@ void GpggaConverter::ConvertTokens(const std::vector<std::string>& tokens) {
     bool ok = tokens.size() == kSize_;
     if (!ok) {
         std::cout << "Error in parsing GPGGA string with " << tokens.size() << " fields! GPGGA message will be empty.\n";
-        msg_ = NavSatFixData();
+        msg_ = GpggaData();
         return;
     }
 
     // Header stamps
-    msg_.stamp = times::GpsTime(0, 0); // ConvertGpsTime(tokens.at(time_idx), tokens.at(time_idx));
-    msg_.frame_id = "LLH";
+    msg_.time = tokens.at(time_idx);
 
     // LLH coordinates
     const std::string _latstr = tokens.at(lat_idx);
diff --git a/fixposition_driver_lib/src/gpzda.cpp b/fixposition_driver_lib/src/gpzda.cpp
new file mode 100644
index 0000000..e774de8
--- /dev/null
+++ b/fixposition_driver_lib/src/gpzda.cpp
@@ -0,0 +1,108 @@
+/**
+ *  @file
+ *  @brief Implementation of GpzdaConverter converter
+ *
+ * \verbatim
+ *  ___    ___
+ *  \  \  /  /
+ *   \  \/  /   Fixposition AG
+ *   /  /\  \   All right reserved.
+ *  /__/  \__\
+ * \endverbatim
+ *
+ */
+
+/* SYSTEM / STL */
+#include <iostream>
+
+/* PACKAGE */
+#include <fixposition_driver_lib/converter/gpzda.hpp>
+
+namespace fixposition {
+
+/// msg field indices
+static constexpr const int time_idx = 1;
+static constexpr const int day_idx = 2;
+static constexpr const int month_idx = 3;
+static constexpr const int year_idx = 4;
+static constexpr const int local_hr_idx = 5;
+static constexpr const int local_min_idx = 6;
+
+#include <iostream>
+#include <ctime>
+#include <iomanip>
+
+// Function to convert UTC time with milliseconds to GPS time
+void convertToGPSTime(const std::string& utcTimeString, std::string& gpsWeek, std::string& gpsTimeOfWeek) {
+    // Define constants
+    const double secondsInWeek = 604800.0; // 7 days in seconds
+
+    // Parse the input string
+    std::tm tmTime = {};
+    std::istringstream iss(utcTimeString);
+    iss >> std::get_time(&tmTime, "%d/%m/%Y %H:%M:%S");
+
+    // Read milliseconds from input
+    char dot;
+    std::string milliseconds;
+    iss >> dot >> milliseconds;
+    double ms = std::stod("0." + milliseconds);
+    
+    if (iss.fail()) {
+        std::cerr << "Error parsing input string.\n";
+        return;
+    }
+    
+    // Convert UTC time to time since epoch
+    std::time_t utcTime = std::mktime(&tmTime);
+
+    // GPS epoch time (January 6, 1980)
+    std::tm gpsEpoch = {};
+    gpsEpoch.tm_year = 80; // years since 1900
+    gpsEpoch.tm_mon = 0;   // months since January
+    gpsEpoch.tm_mday = 6;  // day of the month
+    std::time_t gpsEpochTime = std::mktime(&gpsEpoch);
+
+    // Calculate GPS time of week and GPS week number
+    double timeDifference = std::difftime(utcTime, gpsEpochTime);
+    int gpsWeekNumber = static_cast<int>(std::floor(timeDifference / secondsInWeek));
+    double gpsTime = std::fmod(timeDifference, secondsInWeek);
+
+    // Add milliseconds to GPS time
+    gpsTime += (18 + ms);
+
+    // Convert results to strings
+    std::ostringstream ossWeek, ossTime;
+    ossWeek << gpsWeekNumber;
+    ossTime << std::fixed << std::setprecision(6) << gpsTime;
+
+    gpsWeek = ossWeek.str();
+    gpsTimeOfWeek = ossTime.str();
+}
+
+void GpzdaConverter::ConvertTokens(const std::vector<std::string>& tokens) {
+    // Check if message size is wrong
+    bool ok = tokens.size() == kSize_;
+    if (!ok) {
+        std::cout << "Error in parsing GPZDA string with " << tokens.size() << " fields! GPZDA message will be empty.\n";
+        msg_ = GpzdaData();
+        return;
+    }
+
+    // Populate time fields
+    msg_.time = tokens.at(time_idx);
+    msg_.date = tokens.at(day_idx) + '/' + tokens.at(month_idx) + '/' + tokens.at(year_idx);
+
+    // Generate GPS timestamp
+    std::string utcTimeString = msg_.date + " " + msg_.time.substr(0,2) + ":" + msg_.time.substr(2,2) + ":" + msg_.time.substr(4);
+    std::string gps_tow, gps_week;
+    convertToGPSTime(utcTimeString, gps_week, gps_tow);
+    msg_.stamp = ConvertGpsTime(gps_week, gps_tow);
+
+    // Process all observers
+    for (auto& ob : obs_) {
+        ob(msg_);
+    }
+}
+
+}  // namespace fixposition
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 c58625e..461aa52 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
@@ -52,6 +52,20 @@ class FixpositionDriverNode : public FixpositionDriver {
 
     void WsCallback(const fixposition_driver_ros1::SpeedConstPtr& msg);
 
+    struct NmeaMessage {
+        GpggaData gpgga;
+        GpzdaData gpzda;
+        NavSatFixData msg;
+        
+        bool checkEpoch() {
+            if (gpgga.time.compare(gpzda.time) == 0) {
+                return true;
+            } else {
+                return false;
+            }
+        }  
+    };
+
    private:
     /**
      * @brief Observer Functions to publish NavSatFix from BestGnssPos
@@ -61,6 +75,8 @@ class FixpositionDriverNode : public FixpositionDriver {
      */
     void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload);
 
+    void PublishNmea(NmeaMessage data);
+
     ros::NodeHandle nh_;
     ros::Subscriber ws_sub_;  //!< wheelspeed message subscriber
 
@@ -69,7 +85,7 @@ class FixpositionDriverNode : public FixpositionDriver {
     ros::Publisher navsatfix_pub_;
     ros::Publisher navsatfix_gnss1_pub_;
     ros::Publisher navsatfix_gnss2_pub_;
-    ros::Publisher navsatfix_gpgga_pub_;
+    ros::Publisher nmea_pub_;
     ros::Publisher odometry_pub_;       //!< ECEF Odometry
     ros::Publisher poiimu_pub_;         //!< Bias corrected IMU
     ros::Publisher vrtk_pub_;           //!< VRTK message
@@ -77,6 +93,8 @@ class FixpositionDriverNode : public FixpositionDriver {
     ros::Publisher eul_pub_;            //!< Euler angles Yaw-Pitch-Roll in local ENU
     ros::Publisher eul_imu_pub_;        //!< Euler angles Pitch-Roll as estimated from the IMU in local horizontal
 
+    NmeaMessage nmea_message_;
+
     tf2_ros::TransformBroadcaster br_;
     tf2_ros::StaticTransformBroadcaster static_br_;
 };
diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml
index 9d535e2..c10e573 100644
--- a/fixposition_driver_ros1/launch/tcp.yaml
+++ b/fixposition_driver_ros1/launch/tcp.yaml
@@ -1,5 +1,5 @@
 fp_output:
-    formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "TF"]
+    formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "TF"]
     type: tcp
     port: "21000"
     ip: "10.0.2.1" # change to VRTK2's IP address in the network
diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
index adb646b..4878355 100644
--- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp
+++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
@@ -23,6 +23,7 @@
 #include <fixposition_driver_lib/converter/odometry.hpp>
 #include <fixposition_driver_lib/converter/tf.hpp>
 #include <fixposition_driver_lib/converter/gpgga.hpp>
+#include <fixposition_driver_lib/converter/gpzda.hpp>
 #include <fixposition_driver_lib/fixposition_driver.hpp>
 #include <fixposition_driver_lib/helper.hpp>
 #include <fixposition_gnss_tf/gnss_tf.hpp>
@@ -42,7 +43,7 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para
       navsatfix_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/navsatfix", 100)),
       navsatfix_gnss1_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/gnss1", 100)),
       navsatfix_gnss2_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/gnss2", 100)),
-      navsatfix_gpgga_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/gpgga", 100)),
+      nmea_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/nmea", 100)),
       //   ODOMETRY
       odometry_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry", 100)),
       poiimu_pub_(nh_.advertise<sensor_msgs::Imu>("/fixposition/poiimu", 100)),
@@ -50,9 +51,13 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para
       odometry_enu0_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry_enu", 100)),
       eul_pub_(nh_.advertise<geometry_msgs::Vector3Stamped>("/fixposition/ypr", 100)),
       eul_imu_pub_(nh_.advertise<geometry_msgs::Vector3Stamped>("/fixposition/imu_ypr", 100)) {
+    
     ws_sub_ = nh_.subscribe<fixposition_driver_ros1::Speed>(params_.customer_input.speed_topic, 100,
                                                             &FixpositionDriverNode::WsCallback, this,
                                                             ros::TransportHints().tcpNoDelay());
+
+    
+
     Connect();
     RegisterObservers();
 }
@@ -157,18 +162,44 @@ void FixpositionDriverNode::RegisterObservers() {
                 }
             });
         } else if (format == "GPGGA") {
-            dynamic_cast<GpggaConverter*>(a_converters_["GPGGA"].get())->AddObserver([this](const NavSatFixData& data) {
+            dynamic_cast<GpggaConverter*>(a_converters_["GPGGA"].get())->AddObserver([this](const GpggaData& data) {
                 // GPGGA Observer Lambda
-                if (navsatfix_gpgga_pub_.getNumSubscribers() > 0) {
-                    sensor_msgs::NavSatFix msg;
-                    NavSatFixDataToMsg(data, msg);
-                    navsatfix_gpgga_pub_.publish(msg);
+                if (nmea_pub_.getNumSubscribers() > 0) {
+                    nmea_message_.gpgga = data;
+                    PublishNmea(nmea_message_);
+                }
+            });
+        } else if (format == "GPZDA") {
+            dynamic_cast<GpzdaConverter*>(a_converters_["GPZDA"].get())->AddObserver([this](const GpzdaData& data) {
+                // GPZDA Observer Lambda
+                if (nmea_pub_.getNumSubscribers() > 0) {
+                    nmea_message_.gpzda = data;
+                    PublishNmea(nmea_message_);
                 }
             });
         }
     }
 }
 
+void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
+    // If epoch message is complete, generate NMEA output
+    if (data.checkEpoch()) {
+        sensor_msgs::NavSatFix msg;
+        msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp));
+        msg.header.frame_id = "LLH";
+        msg.latitude = data.gpgga.latitude;
+        msg.longitude = data.gpgga.longitude;
+        msg.altitude = data.gpgga.altitude;
+
+        Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
+            Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.position_covariance.data());
+        cov_map = data.gpgga.cov;
+
+        msg.position_covariance_type = data.gpgga.position_covariance_type;
+        nmea_pub_.publish(msg);
+    }
+}
+
 void FixpositionDriverNode::Run() {
     ros::Rate rate(params_.fp_output.rate);
     while (ros::ok()) {

From a9bf143c7d2c1c631e10df8cc0c06dad8cc7c71f Mon Sep 17 00:00:00 2001
From: Facundo Garcia <facundo.garcia@fixposition.com>
Date: Fri, 12 Jan 2024 18:00:09 +0100
Subject: [PATCH 4/9] Added new NMEA.msg and integrated GPRMC to nmea topic

---
 fixposition_driver_lib/CMakeLists.txt         |  1 +
 .../converter/gprmc.hpp                       | 64 ++++++++++++++++
 .../fixposition_driver_lib/msg_data.hpp       | 11 +++
 .../src/fixposition_driver.cpp                |  7 +-
 fixposition_driver_lib/src/gprmc.cpp          | 73 +++++++++++++++++++
 fixposition_driver_ros1/CMakeLists.txt        |  1 +
 .../fixposition_driver_ros1/data_to_ros1.hpp  |  1 +
 .../fixposition_driver_node.hpp               |  4 +-
 fixposition_driver_ros1/launch/tcp.yaml       |  2 +-
 fixposition_driver_ros1/msg/NMEA.msg          | 66 +++++++++++++++++
 .../src/fixposition_driver_node.cpp           | 36 ++++++++-
 fixposition_driver_ros2/CMakeLists.txt        |  1 +
 12 files changed, 260 insertions(+), 7 deletions(-)
 create mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/converter/gprmc.hpp
 create mode 100644 fixposition_driver_lib/src/gprmc.cpp
 create mode 100644 fixposition_driver_ros1/msg/NMEA.msg

diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt
index f1bf604..b1b35af 100644
--- a/fixposition_driver_lib/CMakeLists.txt
+++ b/fixposition_driver_lib/CMakeLists.txt
@@ -31,6 +31,7 @@ add_library(
   src/parser.cpp
   src/gpgga.cpp
   src/gpzda.cpp
+  src/gprmc.cpp
 )
 
 target_link_libraries(${PROJECT_NAME} ${fixposition_gnss_tf_LIBRARIES} ${Boost_LIBRARIES} pthread)
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/converter/gprmc.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gprmc.hpp
new file mode 100644
index 0000000..832de05
--- /dev/null
+++ b/fixposition_driver_lib/include/fixposition_driver_lib/converter/gprmc.hpp
@@ -0,0 +1,64 @@
+/**
+ *  @file
+ *  @brief Declaration of GprmcConverter
+ *
+ * \verbatim
+ *  ___    ___
+ *  \  \  /  /
+ *   \  \/  /   Fixposition AG
+ *   /  /\  \   All right reserved.
+ *  /__/  \__\
+ * \endverbatim
+ *
+ */
+
+#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_GPRMC__
+#define __FIXPOSITION_DRIVER_LIB_CONVERTER_GPRMC__
+
+/* SYSTEM / STL */
+
+/* EXTERNAL */
+
+/* PACKAGE */
+#include <fixposition_driver_lib/converter/base_converter.hpp>
+#include <fixposition_driver_lib/msg_data.hpp>
+#include <fixposition_driver_lib/time_conversions.hpp>
+
+namespace fixposition {
+
+class GprmcConverter : public BaseAsciiConverter {
+   public:
+    using GprmcObserver = std::function<void(const GprmcData&)>;
+    /**
+     * @brief Construct a new GprmcConverter
+     *
+     */
+    GprmcConverter() {}
+
+    ~GprmcConverter() = default;
+
+    /**
+     * @brief Take comma-delimited tokens of GPRMC message, convert to Data structs and if available,
+     * call observers
+     * Example:
+     * $GPRMC,151227.40,A,4723.54036,N,00826.88672,E,0.0,81.6,111022,,,R*7C\r\n
+     *
+     * @param[in] tokens message split in tokens
+     */
+    void ConvertTokens(const std::vector<std::string>& tokens) final;
+
+    /**
+     * @brief Add Observer to call at the end of ConvertTokens()
+     *
+     * @param[in] ob
+     */
+    void AddObserver(GprmcObserver ob) { obs_.push_back(ob); }
+
+   private:
+    GprmcData msg_;
+    std::vector<GprmcObserver> obs_;
+    const std::string header_ = "LLH";
+    static constexpr const int kSize_ = 13;
+};
+}  // namespace fixposition
+#endif  // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPRMC__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp
index 8ce23f2..ff38989 100644
--- a/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp
+++ b/fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp
@@ -151,5 +151,16 @@ struct GpzdaData {
     GpzdaData() : time(""), date("") {}
 };
 
+struct GprmcData {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    std::string time;
+    std::string mode;
+    double latitude;
+    double longitude;
+    double speed;
+    double course;
+    GprmcData() : latitude(0.0), longitude(0.0), speed(0.0), course(0.0) {}
+};
+
 }  // namespace fixposition
 #endif  //__FIXPOSITION_DRIVER_LIB_MSG_DATA__
diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp
index 4c0cc03..1f2fb78 100644
--- a/fixposition_driver_lib/src/fixposition_driver.cpp
+++ b/fixposition_driver_lib/src/fixposition_driver.cpp
@@ -24,6 +24,7 @@
 #include <fixposition_driver_lib/converter/tf.hpp>
 #include <fixposition_driver_lib/converter/gpgga.hpp>
 #include <fixposition_driver_lib/converter/gpzda.hpp>
+#include <fixposition_driver_lib/converter/gprmc.hpp>
 #include <fixposition_driver_lib/fixposition_driver.hpp>
 #include <fixposition_driver_lib/helper.hpp>
 #include <fixposition_driver_lib/parser.hpp>
@@ -143,6 +144,8 @@ bool FixpositionDriver::InitializeConverters() {
             a_converters_["GPGGA"] = std::unique_ptr<GpggaConverter>(new GpggaConverter());
         } else if (format == "GPZDA") {
             a_converters_["GPZDA"] = std::unique_ptr<GpzdaConverter>(new GpzdaConverter());
+        } else if (format == "GPRMC") {
+            a_converters_["GPRMC"] = std::unique_ptr<GprmcConverter>(new GprmcConverter());
         } else if (format == "TF") {
             if (a_converters_.find("TF") == a_converters_.end()) {
                 a_converters_["TF"] = std::unique_ptr<TfConverter>(new TfConverter());
@@ -236,7 +239,7 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) {
     SplitMessage(tokens, msg.substr(1, star_pos - 1), ",");
 
     // if it doesn't start with FP then do nothing
-    if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA") && (tokens.at(0) != "GPZDA")) {
+    if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA") && (tokens.at(0) != "GPZDA") && (tokens.at(0) != "GPRMC")) {
         return;
     }
 
@@ -246,6 +249,8 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) {
         _header = "GPGGA";
     } else if (tokens.at(0) == "GPZDA") {
         _header = "GPZDA";
+    } else if (tokens.at(0) == "GPRMC") {
+        _header = "GPRMC";
     } else {
         _header = tokens.at(1);
     }
diff --git a/fixposition_driver_lib/src/gprmc.cpp b/fixposition_driver_lib/src/gprmc.cpp
new file mode 100644
index 0000000..2a0d5a0
--- /dev/null
+++ b/fixposition_driver_lib/src/gprmc.cpp
@@ -0,0 +1,73 @@
+/**
+ *  @file
+ *  @brief Implementation of GprmcConverter converter
+ *
+ * \verbatim
+ *  ___    ___
+ *  \  \  /  /
+ *   \  \/  /   Fixposition AG
+ *   /  /\  \   All right reserved.
+ *  /__/  \__\
+ * \endverbatim
+ *
+ */
+
+/* SYSTEM / STL */
+#include <iostream>
+
+/* PACKAGE */
+#include <fixposition_driver_lib/converter/gprmc.hpp>
+
+namespace fixposition {
+
+/// msg field indices
+static constexpr const int time_idx = 1;
+static constexpr const int status_idx = 2;
+static constexpr const int lat_idx = 3;
+static constexpr const int lat_ns_idx = 4;
+static constexpr const int lon_idx = 5;
+static constexpr const int lon_ew_idx = 6;
+static constexpr const int speed_idx = 7;
+static constexpr const int course_idx = 8;
+static constexpr const int date_idx = 9;
+static constexpr const int magvar_idx = 10;
+static constexpr const int magvar_ew = 11;
+static constexpr const int mode_idx = 12;
+
+void GprmcConverter::ConvertTokens(const std::vector<std::string>& tokens) {
+    // Check if message size is wrong
+    bool ok = tokens.size() == kSize_;
+    if (!ok) {
+        std::cout << "Error in parsing GPRMC string with " << tokens.size() << " fields! GPRMC message will be empty.\n";
+        msg_ = GprmcData();
+        return;
+    }
+
+    // Header stamps
+    msg_.time = tokens.at(time_idx);
+
+    // LLH coordinates
+    const std::string _latstr = tokens.at(lat_idx);
+    double _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60;
+    if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1;
+    msg_.latitude = _lat;
+
+    const std::string _lonstr = tokens.at(lon_idx);
+    double _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60;
+    if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1;
+    msg_.longitude = _lon;
+
+    // Speed and course over groung
+    msg_.speed = StringToDouble(tokens.at(speed_idx)) * 1852.0 / 3600.0;
+    msg_.course = StringToDouble(tokens.at(course_idx));
+
+    // Get GPS status
+    msg_.mode = tokens.at(mode_idx);
+
+    // Process all observers
+    for (auto& ob : obs_) {
+        ob(msg_);
+    }
+}
+
+}  // namespace fixposition
diff --git a/fixposition_driver_ros1/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt
index 07e1898..74ce039 100644
--- a/fixposition_driver_ros1/CMakeLists.txt
+++ b/fixposition_driver_ros1/CMakeLists.txt
@@ -26,6 +26,7 @@ add_message_files(
   FILES
   VRTK.msg
   Speed.msg
+  NMEA.msg
 )
 
 generate_messages(
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 0bb259f..2f5c093 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
@@ -27,6 +27,7 @@
 
 /* PACKAGE */
 #include <fixposition_driver_ros1/VRTK.h>
+#include <fixposition_driver_ros1/NMEA.h>
 
 namespace fixposition {
 /**
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 461aa52..bd87663 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
@@ -55,10 +55,10 @@ class FixpositionDriverNode : public FixpositionDriver {
     struct NmeaMessage {
         GpggaData gpgga;
         GpzdaData gpzda;
-        NavSatFixData msg;
+        GprmcData gprmc;
         
         bool checkEpoch() {
-            if (gpgga.time.compare(gpzda.time) == 0) {
+            if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) {
                 return true;
             } else {
                 return false;
diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml
index c10e573..7bf49c4 100644
--- a/fixposition_driver_ros1/launch/tcp.yaml
+++ b/fixposition_driver_ros1/launch/tcp.yaml
@@ -1,5 +1,5 @@
 fp_output:
-    formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "TF"]
+    formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"]
     type: tcp
     port: "21000"
     ip: "10.0.2.1" # change to VRTK2's IP address in the network
diff --git a/fixposition_driver_ros1/msg/NMEA.msg b/fixposition_driver_ros1/msg/NMEA.msg
new file mode 100644
index 0000000..98d2649
--- /dev/null
+++ b/fixposition_driver_ros1/msg/NMEA.msg
@@ -0,0 +1,66 @@
+####################################################################################################
+#
+#    Copyright (c) 2023  ___     ___
+#                       \\  \\  /  /
+#                        \\  \\/  /
+#                         /  /\\  \\
+#                        /__/  \\__\\  Fixposition AG
+#
+####################################################################################################
+#
+# Fixposition NMEA Message
+#
+#
+####################################################################################################
+
+# Navigation Satellite fix for any Global Navigation Satellite System
+#
+# Specified using the WGS 84 reference ellipsoid
+
+# header.stamp specifies the ROS time for this measurement (the
+#        corresponding satellite time may be reported using the
+#        sensor_msgs/TimeReference message).
+#
+# header.frame_id is the frame of reference reported by the satellite
+#        receiver, usually the location of the antenna.  This is a
+#        Euclidean frame relative to the vehicle, not a reference
+#        ellipsoid.
+Header header
+
+# Latitude [degrees]. Positive is north of equator; negative is south.
+float64 latitude
+
+# Longitude [degrees]. Positive is east of prime meridian; negative is west.
+float64 longitude
+
+# Altitude [m]. Positive is above the WGS 84 ellipsoid.
+float64 altitude
+
+# Speed over ground [m/s]
+float64 speed
+
+# Course over ground [deg]
+float64 course
+
+# Position covariance [m^2] defined relative to a tangential plane
+# through the reported position. The components are East, North, and
+# Up (ENU), in row-major order.
+#
+# Beware: this coordinate system exhibits singularities at the poles.
+
+float64[9] position_covariance
+
+# If the covariance of the fix is known, fill it in completely. If the
+# GPS receiver provides the variance of each measurement, put them
+# along the diagonal. If only Dilution of Precision is available,
+# estimate an approximate covariance from that.
+
+uint8 COVARIANCE_TYPE_UNKNOWN = 0
+uint8 COVARIANCE_TYPE_APPROXIMATED = 1
+uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
+uint8 COVARIANCE_TYPE_KNOWN = 3
+
+uint8 position_covariance_type
+
+# Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N
+string mode
\ No newline at end of file
diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
index 4878355..161426c 100644
--- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp
+++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
@@ -24,6 +24,7 @@
 #include <fixposition_driver_lib/converter/tf.hpp>
 #include <fixposition_driver_lib/converter/gpgga.hpp>
 #include <fixposition_driver_lib/converter/gpzda.hpp>
+#include <fixposition_driver_lib/converter/gprmc.hpp>
 #include <fixposition_driver_lib/fixposition_driver.hpp>
 #include <fixposition_driver_lib/helper.hpp>
 #include <fixposition_gnss_tf/gnss_tf.hpp>
@@ -43,7 +44,7 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para
       navsatfix_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/navsatfix", 100)),
       navsatfix_gnss1_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/gnss1", 100)),
       navsatfix_gnss2_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/gnss2", 100)),
-      nmea_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/nmea", 100)),
+      nmea_pub_(nh_.advertise<fixposition_driver_ros1::NMEA>("/fixposition/nmea", 100)),
       //   ODOMETRY
       odometry_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry", 100)),
       poiimu_pub_(nh_.advertise<sensor_msgs::Imu>("/fixposition/poiimu", 100)),
@@ -177,6 +178,14 @@ void FixpositionDriverNode::RegisterObservers() {
                     PublishNmea(nmea_message_);
                 }
             });
+        } else if (format == "GPRMC") {
+            dynamic_cast<GprmcConverter*>(a_converters_["GPRMC"].get())->AddObserver([this](const GprmcData& data) {
+                // GPRMC Observer Lambda
+                if (nmea_pub_.getNumSubscribers() > 0) {
+                    nmea_message_.gprmc = data;
+                    PublishNmea(nmea_message_);
+                }
+            });
         }
     }
 }
@@ -184,18 +193,39 @@ void FixpositionDriverNode::RegisterObservers() {
 void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
     // If epoch message is complete, generate NMEA output
     if (data.checkEpoch()) {
-        sensor_msgs::NavSatFix msg;
+        // Generate new message
+        fixposition_driver_ros1::NMEA msg;
+        
+        // ROS Header
         msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp));
         msg.header.frame_id = "LLH";
+        
+        // Latitude [degrees]. Positive is north of equator; negative is south
         msg.latitude = data.gpgga.latitude;
+        
+        // Longitude [degrees]. Positive is east of prime meridian; negative is west
         msg.longitude = data.gpgga.longitude;
+        
+        // Altitude [m]. Positive is above the WGS 84 ellipsoid
         msg.altitude = data.gpgga.altitude;
 
+        // Speed over ground [m/s]
+        msg.speed = data.gprmc.speed;
+        
+        // Course over ground [deg]
+        msg.course = data.gprmc.course;
+
+        // TODO: Get better position covariance from NMEA-GP-GST
+        // Position covariance [m^2]
         Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
             Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.position_covariance.data());
         cov_map = data.gpgga.cov;
-
         msg.position_covariance_type = data.gpgga.position_covariance_type;
+
+        // Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N
+        msg.mode = data.gprmc.mode;
+        
+        // Publish message
         nmea_pub_.publish(msg);
     }
 }
diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt
index ca2b66f..7353ac4 100644
--- a/fixposition_driver_ros2/CMakeLists.txt
+++ b/fixposition_driver_ros2/CMakeLists.txt
@@ -26,6 +26,7 @@ find_package(fixposition_driver_lib REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   msg/VRTK.msg
   msg/Speed.msg
+  msg/NMEA.msg
   DEPENDENCIES
   std_msgs
   nav_msgs

From b243cdeb5b819e15a45a30438a05d901fa12c3bc Mon Sep 17 00:00:00 2001
From: Facundo Garcia <facundo.garcia@fixposition.com>
Date: Fri, 12 Jan 2024 18:11:25 +0100
Subject: [PATCH 5/9] ROS2 implementation - Please test

---
 .../fixposition_driver_ros2/data_to_ros2.hpp  |  1 +
 .../fixposition_driver_node.hpp               | 18 ++++-
 fixposition_driver_ros2/msg/NMEA.msg          | 66 ++++++++++++++++++
 .../src/fixposition_driver_node.cpp           | 69 +++++++++++++++++--
 4 files changed, 147 insertions(+), 7 deletions(-)
 create mode 100644 fixposition_driver_ros2/msg/NMEA.msg

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 0bcdc16..8afe911 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
@@ -37,6 +37,7 @@
 
 /* PACKAGE */
 #include <fixposition_driver_ros2/msg/vrtk.hpp>
+#include <fixposition_driver_ros2/msg/nmea.hpp>
 
 namespace fixposition {
 
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 a01336d..180697b 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
@@ -55,6 +55,20 @@ class FixpositionDriverNode : public FixpositionDriver {
 
     void WsCallback(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg);
 
+    struct NmeaMessage {
+        GpggaData gpgga;
+        GpzdaData gpzda;
+        GprmcData gprmc;
+        
+        bool checkEpoch() {
+            if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) {
+                return true;
+            } else {
+                return false;
+            }
+        }  
+    };
+
    private:
     /**
      * @brief Observer Functions to publish NavSatFix from BestGnssPos
@@ -72,7 +86,7 @@ class FixpositionDriverNode : public FixpositionDriver {
     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_pub_;
     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_gnss1_pub_;
     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_gnss2_pub_;
-    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_gpgga_pub_;
+    rclcpp::Publisher<fixposition_driver_ros2::msg::NMEA>::SharedPtr nmea_pub_;
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_pub_;
     rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr poiimu_pub_;             //!< Bias corrected IMU from ODOMETRY
     rclcpp::Publisher<fixposition_driver_ros2::msg::VRTK>::SharedPtr vrtk_pub_;  //!< VRTK message
@@ -84,6 +98,8 @@ class FixpositionDriverNode : public FixpositionDriver {
 
     std::shared_ptr<tf2_ros::TransformBroadcaster> br_;
     std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_br_;
+
+    NmeaMessage nmea_message_;
 };
 
 }  // namespace fixposition
diff --git a/fixposition_driver_ros2/msg/NMEA.msg b/fixposition_driver_ros2/msg/NMEA.msg
new file mode 100644
index 0000000..98d2649
--- /dev/null
+++ b/fixposition_driver_ros2/msg/NMEA.msg
@@ -0,0 +1,66 @@
+####################################################################################################
+#
+#    Copyright (c) 2023  ___     ___
+#                       \\  \\  /  /
+#                        \\  \\/  /
+#                         /  /\\  \\
+#                        /__/  \\__\\  Fixposition AG
+#
+####################################################################################################
+#
+# Fixposition NMEA Message
+#
+#
+####################################################################################################
+
+# Navigation Satellite fix for any Global Navigation Satellite System
+#
+# Specified using the WGS 84 reference ellipsoid
+
+# header.stamp specifies the ROS time for this measurement (the
+#        corresponding satellite time may be reported using the
+#        sensor_msgs/TimeReference message).
+#
+# header.frame_id is the frame of reference reported by the satellite
+#        receiver, usually the location of the antenna.  This is a
+#        Euclidean frame relative to the vehicle, not a reference
+#        ellipsoid.
+Header header
+
+# Latitude [degrees]. Positive is north of equator; negative is south.
+float64 latitude
+
+# Longitude [degrees]. Positive is east of prime meridian; negative is west.
+float64 longitude
+
+# Altitude [m]. Positive is above the WGS 84 ellipsoid.
+float64 altitude
+
+# Speed over ground [m/s]
+float64 speed
+
+# Course over ground [deg]
+float64 course
+
+# Position covariance [m^2] defined relative to a tangential plane
+# through the reported position. The components are East, North, and
+# Up (ENU), in row-major order.
+#
+# Beware: this coordinate system exhibits singularities at the poles.
+
+float64[9] position_covariance
+
+# If the covariance of the fix is known, fill it in completely. If the
+# GPS receiver provides the variance of each measurement, put them
+# along the diagonal. If only Dilution of Precision is available,
+# estimate an approximate covariance from that.
+
+uint8 COVARIANCE_TYPE_UNKNOWN = 0
+uint8 COVARIANCE_TYPE_APPROXIMATED = 1
+uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
+uint8 COVARIANCE_TYPE_KNOWN = 3
+
+uint8 position_covariance_type
+
+# Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N
+string mode
\ No newline at end of file
diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
index 7a2edff..f9b1466 100644
--- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp
+++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
@@ -25,6 +25,8 @@
 #include <fixposition_driver_lib/converter/odometry.hpp>
 #include <fixposition_driver_lib/converter/tf.hpp>
 #include <fixposition_driver_lib/converter/gpgga.hpp>
+#include <fixposition_driver_lib/converter/gpzda.hpp>
+#include <fixposition_driver_lib/converter/gprmc.hpp>
 #include <fixposition_driver_lib/fixposition_driver.hpp>
 #include <fixposition_driver_lib/helper.hpp>
 #include <fixposition_gnss_tf/gnss_tf.hpp>
@@ -44,7 +46,7 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr<rclcpp::Node> node,
       navsatfix_pub_(node_->create_publisher<sensor_msgs::msg::NavSatFix>("/fixposition/navsatfix", 100)),
       navsatfix_gnss1_pub_(node_->create_publisher<sensor_msgs::msg::NavSatFix>("/fixposition/gnss1", 100)),
       navsatfix_gnss2_pub_(node_->create_publisher<sensor_msgs::msg::NavSatFix>("/fixposition/gnss2", 100)),
-      navsatfix_gpgga_pub_(node_->create_publisher<sensor_msgs::msg::NavSatFix>("/fixposition/gpgga", 100)),
+      nmea_pub_(node_->create_publisher<fixposition_driver_ros2::msg::NMEA>("/fixposition/nmea", 100)),
       odometry_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("/fixposition/odometry", 100)),
       poiimu_pub_(node_->create_publisher<sensor_msgs::msg::Imu>("/fixposition/poiimu", 100)),
       vrtk_pub_(node_->create_publisher<fixposition_driver_ros2::msg::VRTK>("/fixposition/vrtk", 100)),
@@ -186,18 +188,73 @@ void FixpositionDriverNode::RegisterObservers() {
                 }
             });
         } else if (format == "GPGGA") {
-            dynamic_cast<GpggaConverter*>(a_converters_["GPGGA"].get())->AddObserver([this](const NavSatFixData& data) {
+            dynamic_cast<GpggaConverter*>(a_converters_["GPGGA"].get())->AddObserver([this](const GpggaData& data) {
                 // GPGGA Observer Lambda
-                if (navsatfix_gpgga_pub_.getNumSubscribers() > 0) {
-                    sensor_msgs::NavSatFix msg;
-                    NavSatFixDataToMsg(data, msg);
-                    navsatfix_gpgga_pub_.publish(msg);
+                if (nmea_pub_.getNumSubscribers() > 0) {
+                    nmea_message_.gpgga = data;
+                    PublishNmea(nmea_message_);
+                }
+            });
+        } else if (format == "GPZDA") {
+            dynamic_cast<GpzdaConverter*>(a_converters_["GPZDA"].get())->AddObserver([this](const GpzdaData& data) {
+                // GPZDA Observer Lambda
+                if (nmea_pub_.getNumSubscribers() > 0) {
+                    nmea_message_.gpzda = data;
+                    PublishNmea(nmea_message_);
+                }
+            });
+        } else if (format == "GPRMC") {
+            dynamic_cast<GprmcConverter*>(a_converters_["GPRMC"].get())->AddObserver([this](const GprmcData& data) {
+                // GPRMC Observer Lambda
+                if (nmea_pub_.getNumSubscribers() > 0) {
+                    nmea_message_.gprmc = data;
+                    PublishNmea(nmea_message_);
                 }
             });
         }
     }
 }
 
+void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
+    // If epoch message is complete, generate NMEA output
+    if (data.checkEpoch()) {
+        // Generate new message
+        fixposition_driver_ros1::NMEA msg;
+        
+        // ROS Header
+        msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp));
+        msg.header.frame_id = "LLH";
+        
+        // Latitude [degrees]. Positive is north of equator; negative is south
+        msg.latitude = data.gpgga.latitude;
+        
+        // Longitude [degrees]. Positive is east of prime meridian; negative is west
+        msg.longitude = data.gpgga.longitude;
+        
+        // Altitude [m]. Positive is above the WGS 84 ellipsoid
+        msg.altitude = data.gpgga.altitude;
+
+        // Speed over ground [m/s]
+        msg.speed = data.gprmc.speed;
+        
+        // Course over ground [deg]
+        msg.course = data.gprmc.course;
+
+        // TODO: Get better position covariance from NMEA-GP-GST
+        // Position covariance [m^2]
+        Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
+            Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.position_covariance.data());
+        cov_map = data.gpgga.cov;
+        msg.position_covariance_type = data.gpgga.position_covariance_type;
+
+        // Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N
+        msg.mode = data.gprmc.mode;
+        
+        // Publish message
+        nmea_pub_.publish(msg);
+    }
+}
+
 void FixpositionDriverNode::WsCallback(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg) {
     FixpositionDriver::WsCallback(msg->speeds);
 }

From 4ece5416f095b8180cecc495cb2a607ce7e62976 Mon Sep 17 00:00:00 2001
From: Facundo Garcia <facundo.garcia@fixposition.com>
Date: Mon, 15 Jan 2024 16:08:25 +0100
Subject: [PATCH 6/9] Added unit conversion as constant

---
 fixposition_driver_lib/src/gprmc.cpp    | 7 +++++--
 fixposition_driver_ros1/launch/tcp.yaml | 2 +-
 fixposition_driver_ros2/CATKIN_IGNORE   | 0
 3 files changed, 6 insertions(+), 3 deletions(-)
 delete mode 100644 fixposition_driver_ros2/CATKIN_IGNORE

diff --git a/fixposition_driver_lib/src/gprmc.cpp b/fixposition_driver_lib/src/gprmc.cpp
index 2a0d5a0..e09fc23 100644
--- a/fixposition_driver_lib/src/gprmc.cpp
+++ b/fixposition_driver_lib/src/gprmc.cpp
@@ -20,6 +20,9 @@
 
 namespace fixposition {
 
+// unit transformation constant
+static constexpr double knots_to_ms = 1852.0 / 3600.0; //!< convert knots to m/s
+
 /// msg field indices
 static constexpr const int time_idx = 1;
 static constexpr const int status_idx = 2;
@@ -57,8 +60,8 @@ void GprmcConverter::ConvertTokens(const std::vector<std::string>& tokens) {
     if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1;
     msg_.longitude = _lon;
 
-    // Speed and course over groung
-    msg_.speed = StringToDouble(tokens.at(speed_idx)) * 1852.0 / 3600.0;
+    // Speed [m/s] and course [deg] over ground
+    msg_.speed = StringToDouble(tokens.at(speed_idx)) * knots_to_ms;
     msg_.course = StringToDouble(tokens.at(course_idx));
 
     // Get GPS status
diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml
index 7bf49c4..44ecfc4 100644
--- a/fixposition_driver_ros1/launch/tcp.yaml
+++ b/fixposition_driver_ros1/launch/tcp.yaml
@@ -2,7 +2,7 @@ fp_output:
     formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"]
     type: tcp
     port: "21000"
-    ip: "10.0.2.1" # change to VRTK2's IP address in the network
+    ip: "127.0.0.1" # change to VRTK2's IP address in the network
     rate: 200
     reconnect_delay: 5.0 # wait time in [s] until retry connection
 
diff --git a/fixposition_driver_ros2/CATKIN_IGNORE b/fixposition_driver_ros2/CATKIN_IGNORE
deleted file mode 100644
index e69de29..0000000

From 6dd89eb6fdfc8939ed8628caa4d5961309797951 Mon Sep 17 00:00:00 2001
From: Facundo Garcia <facundo.garcia@fixposition.com>
Date: Mon, 15 Jan 2024 16:49:40 +0100
Subject: [PATCH 7/9] ROS2 patch fix

---
 fixposition_driver_lib/src/gprmc.cpp                 |  2 +-
 fixposition_driver_ros1/launch/tcp.yaml              |  2 +-
 .../fixposition_driver_node.hpp                      |  2 ++
 fixposition_driver_ros2/launch/tcp.yaml              |  2 +-
 fixposition_driver_ros2/msg/NMEA.msg                 |  9 +++------
 .../src/fixposition_driver_node.cpp                  | 12 ++++++------
 6 files changed, 14 insertions(+), 15 deletions(-)

diff --git a/fixposition_driver_lib/src/gprmc.cpp b/fixposition_driver_lib/src/gprmc.cpp
index e09fc23..4d6a6f4 100644
--- a/fixposition_driver_lib/src/gprmc.cpp
+++ b/fixposition_driver_lib/src/gprmc.cpp
@@ -20,7 +20,7 @@
 
 namespace fixposition {
 
-// unit transformation constant
+// unit conversion constant
 static constexpr double knots_to_ms = 1852.0 / 3600.0; //!< convert knots to m/s
 
 /// msg field indices
diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml
index 44ecfc4..7bf49c4 100644
--- a/fixposition_driver_ros1/launch/tcp.yaml
+++ b/fixposition_driver_ros1/launch/tcp.yaml
@@ -2,7 +2,7 @@ fp_output:
     formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"]
     type: tcp
     port: "21000"
-    ip: "127.0.0.1" # change to VRTK2's IP address in the network
+    ip: "10.0.2.1" # change to VRTK2's IP address in the network
     rate: 200
     reconnect_delay: 5.0 # wait time in [s] until retry connection
 
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 180697b..0a53850 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
@@ -77,6 +77,8 @@ class FixpositionDriverNode : public FixpositionDriver {
      * @param[in] payload
      */
     void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload);
+
+    void PublishNmea(NmeaMessage data);
     
     std::shared_ptr<rclcpp::Node> node_;
     rclcpp::Subscription<fixposition_driver_ros2::msg::Speed>::SharedPtr ws_sub_;  //!< wheelspeed message subscriber
diff --git a/fixposition_driver_ros2/launch/tcp.yaml b/fixposition_driver_ros2/launch/tcp.yaml
index 9348466..09d7079 100644
--- a/fixposition_driver_ros2/launch/tcp.yaml
+++ b/fixposition_driver_ros2/launch/tcp.yaml
@@ -1,7 +1,7 @@
 fixposition_driver_ros2:
   ros__parameters:
     fp_output:
-      formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "TF"]
+      formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"]
       type: "tcp"
       port: "21000"
       ip: "10.0.2.1" # change to VRTK2's IP address in the network
diff --git a/fixposition_driver_ros2/msg/NMEA.msg b/fixposition_driver_ros2/msg/NMEA.msg
index 98d2649..0c6064e 100644
--- a/fixposition_driver_ros2/msg/NMEA.msg
+++ b/fixposition_driver_ros2/msg/NMEA.msg
@@ -1,10 +1,7 @@
 ####################################################################################################
 #
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
+#    Copyright (c) 2023
+#    Fixposition AG
 #
 ####################################################################################################
 #
@@ -25,7 +22,7 @@
 #        receiver, usually the location of the antenna.  This is a
 #        Euclidean frame relative to the vehicle, not a reference
 #        ellipsoid.
-Header header
+std_msgs/Header header
 
 # Latitude [degrees]. Positive is north of equator; negative is south.
 float64 latitude
diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
index f9b1466..7c1d2b9 100644
--- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp
+++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
@@ -190,7 +190,7 @@ void FixpositionDriverNode::RegisterObservers() {
         } else if (format == "GPGGA") {
             dynamic_cast<GpggaConverter*>(a_converters_["GPGGA"].get())->AddObserver([this](const GpggaData& data) {
                 // GPGGA Observer Lambda
-                if (nmea_pub_.getNumSubscribers() > 0) {
+                if (nmea_pub_->get_subscription_count() > 0) {
                     nmea_message_.gpgga = data;
                     PublishNmea(nmea_message_);
                 }
@@ -198,7 +198,7 @@ void FixpositionDriverNode::RegisterObservers() {
         } else if (format == "GPZDA") {
             dynamic_cast<GpzdaConverter*>(a_converters_["GPZDA"].get())->AddObserver([this](const GpzdaData& data) {
                 // GPZDA Observer Lambda
-                if (nmea_pub_.getNumSubscribers() > 0) {
+                if (nmea_pub_->get_subscription_count() > 0) {
                     nmea_message_.gpzda = data;
                     PublishNmea(nmea_message_);
                 }
@@ -206,7 +206,7 @@ void FixpositionDriverNode::RegisterObservers() {
         } else if (format == "GPRMC") {
             dynamic_cast<GprmcConverter*>(a_converters_["GPRMC"].get())->AddObserver([this](const GprmcData& data) {
                 // GPRMC Observer Lambda
-                if (nmea_pub_.getNumSubscribers() > 0) {
+                if (nmea_pub_->get_subscription_count() > 0) {
                     nmea_message_.gprmc = data;
                     PublishNmea(nmea_message_);
                 }
@@ -219,10 +219,10 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
     // If epoch message is complete, generate NMEA output
     if (data.checkEpoch()) {
         // Generate new message
-        fixposition_driver_ros1::NMEA msg;
+        fixposition_driver_ros2::msg::NMEA msg;
         
         // ROS Header
-        msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp));
+        msg.header.stamp = GpsTimeToMsgTime(data.gpzda.stamp);
         msg.header.frame_id = "LLH";
         
         // Latitude [degrees]. Positive is north of equator; negative is south
@@ -251,7 +251,7 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
         msg.mode = data.gprmc.mode;
         
         // Publish message
-        nmea_pub_.publish(msg);
+        nmea_pub_->publish(msg);
     }
 }
 

From df585046bc5527849e043a95da2c1fff30f657f3 Mon Sep 17 00:00:00 2001
From: Facundo Garcia <facundo.garcia@fixposition.com>
Date: Mon, 15 Jan 2024 17:02:55 +0100
Subject: [PATCH 8/9] Updated README with NMEA output

---
 README.md | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/README.md b/README.md
index 070bb80..b3ba34e 100644
--- a/README.md
+++ b/README.md
@@ -176,6 +176,12 @@ The output is published on the following:
     | `/fixposition/gnss1` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height |
     | `/fixposition/gnss2` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height |
 
+-   The Vision-RTK2 can also output an average GNSS-based LLH position (i.e., only GNSS, not Fusion) and heading estimate based on SOG and COG (i.e., the platform must be moving for it to be accurate) by utilizing several NMEA messages, which serves as an auxiliary output until Fusion is fully initialized. To do this, enable the NMEA-GP-GGA_GNSS, NMEA-GP-RMC_GNSS, and NMEA-GP-ZDA_GNSS messages. This message's output frequency equals the lowest frequency of any of these three message types. **Note: This output should only be used until Fusion is fully initialized.**
+
+    | Topic                | Message Type            | Frequency                      | Description                    |
+    | -------------------- | ----------------------- | ------------------------------ | ------------------------------ |
+    | `/fixposition/nmea` | `fixposition_driver/NMEA` | as configured on web-interface | Latitude, Longitude and Height |
+
 #### Vision-RTK2 IMU data
 
 -   From RAWIMU, at 200Hz

From b6747f764638cb0537bb49d67653a28357d1bc95 Mon Sep 17 00:00:00 2001
From: Facundo Garcia <facundo.garcia@fixposition.com>
Date: Mon, 15 Jan 2024 17:14:37 +0100
Subject: [PATCH 9/9] Updated documentation

---
 README.md                                                 | 2 +-
 .../fixposition_driver_ros1/fixposition_driver_node.hpp   | 8 ++++++++
 .../fixposition_driver_ros2/fixposition_driver_node.hpp   | 8 ++++++++
 3 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/README.md b/README.md
index b3ba34e..34c7daf 100644
--- a/README.md
+++ b/README.md
@@ -176,7 +176,7 @@ The output is published on the following:
     | `/fixposition/gnss1` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height |
     | `/fixposition/gnss2` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height |
 
--   The Vision-RTK2 can also output an average GNSS-based LLH position (i.e., only GNSS, not Fusion) and heading estimate based on SOG and COG (i.e., the platform must be moving for it to be accurate) by utilizing several NMEA messages, which serves as an auxiliary output until Fusion is fully initialized. To do this, enable the NMEA-GP-GGA_GNSS, NMEA-GP-RMC_GNSS, and NMEA-GP-ZDA_GNSS messages. This message's output frequency equals the lowest frequency of any of these three message types. **Note: This output should only be used until Fusion is fully initialized.**
+-   The Vision-RTK2 can also output an average GNSS-based LLH position (i.e., **only GNSS, not Fusion**) and heading estimate based on speed over ground (SOG) and course over ground (COG) - (i.e., **the platform must be moving for it to be accurate**) by utilizing several NMEA messages, which serves as an auxiliary output until Fusion is fully initialized. To do this, enable the NMEA-GP-GGA_GNSS, NMEA-GP-RMC_GNSS, and NMEA-GP-ZDA_GNSS messages. This message's output frequency equals the lowest frequency of any of these three message types. **Note: This output should only be used until Fusion is fully initialized.**
 
     | Topic                | Message Type            | Frequency                      | Description                    |
     | -------------------- | ----------------------- | ------------------------------ | ------------------------------ |
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 bd87663..13dc590 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
@@ -57,6 +57,9 @@ class FixpositionDriverNode : public FixpositionDriver {
         GpzdaData gpzda;
         GprmcData gprmc;
         
+        /**
+         * @brief Construct a new Fixposition Driver Node object
+         */
         bool checkEpoch() {
             if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) {
                 return true;
@@ -75,6 +78,11 @@ class FixpositionDriverNode : public FixpositionDriver {
      */
     void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload);
 
+    /**
+     * @brief Observer Function to publish NMEA message from GPGGA, GPRMC, and GPZDA once the GNSS epoch transmission is complete
+     *
+     * @param[in] data
+     */
     void PublishNmea(NmeaMessage data);
 
     ros::NodeHandle nh_;
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 0a53850..6573259 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
@@ -60,6 +60,9 @@ class FixpositionDriverNode : public FixpositionDriver {
         GpzdaData gpzda;
         GprmcData gprmc;
         
+        /**
+         * @brief Check if GNSS epoch is complete
+         */
         bool checkEpoch() {
             if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) {
                 return true;
@@ -78,6 +81,11 @@ class FixpositionDriverNode : public FixpositionDriver {
      */
     void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload);
 
+    /**
+     * @brief Observer Function to publish NMEA message from GPGGA, GPRMC, and GPZDA once the GNSS epoch transmission is complete
+     *
+     * @param[in] data
+     */
     void PublishNmea(NmeaMessage data);
     
     std::shared_ptr<rclcpp::Node> node_;