Feature/gpgga

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 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                    |
+    | -------------------- | ----------------------- | ------------------------------ | ------------------------------ |
+    | `/fixposition/nmea` | `fixposition_driver/NMEA` | as configured on web-interface | Latitude, Longitude and Height |
+
 #### Vision-RTK2 IMU data
 
 -   From RAWIMU, at 200Hz

fixposition_driver_lib/CMakeLists.txt

@@ -29,6 +29,9 @@ add_library(
   src/tf.cpp
   src/helper.cpp
   src/parser.cpp
+  src/gpgga.cpp
+  src/gpzda.cpp
+  src/gprmc.cpp
 )
 
 target_link_libraries(${PROJECT_NAME} ${fixposition_gnss_tf_LIBRARIES} ${Boost_LIBRARIES} pthread)

fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp

@@ -0,0 +1,64 @@
+/**
+ *  @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 GpggaData&)>;
+    /**
+     * @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:
+    GpggaData msg_;
+    std::vector<GpggaObserver> obs_;
+    const std::string header_ = "LLH";
+    static constexpr const int kSize_ = 15;
+};
+}  // namespace fixposition
+#endif  // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__

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__

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__

fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp

@@ -132,5 +132,35 @@ 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("") {}
+};
+
+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__

fixposition_driver_lib/src/fixposition_driver.cpp

@@ -22,6 +22,9 @@
 #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/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>
@@ -137,6 +140,12 @@ 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 == "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());
@@ -230,12 +239,22 @@ 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") && (tokens.at(0) != "GPZDA") && (tokens.at(0) != "GPRMC")) {
         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 if (tokens.at(0) == "GPZDA") {
+        _header = "GPZDA";
+    } else if (tokens.at(0) == "GPRMC") {
+        _header = "GPRMC";
+    } else {
+        _header = tokens.at(1);
+    }
+    const std::string header = _header;
 
     // If we have a converter available, convert to ros. Currently supported are "FP", "LLH", "TF", "RAWIMU", "CORRIMU"
     if (a_converters_[header] != nullptr) {

fixposition_driver_lib/src/gpgga.cpp

@@ -0,0 +1,82 @@
+/**
+ *  @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_ = GpggaData();
+        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;
+
+    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