Update repository to be compatible with the new VRTK2 software

fixposition_driver_lib/CMakeLists.txt

@@ -21,15 +21,19 @@ include_directories(include
 add_library(
   ${PROJECT_NAME} SHARED
   src/messages/fpa/corrimu.cpp
+  src/messages/fpa/eoe.cpp
   src/messages/fpa/gnssant.cpp
   src/messages/fpa/gnsscorr.cpp
+  src/messages/fpa/imubias.cpp
   src/messages/fpa/llh.cpp
   src/messages/fpa/odomenu.cpp
   src/messages/fpa/odometry.cpp
   src/messages/fpa/odomsh.cpp
+  src/messages/fpa/odomstatus.cpp
   src/messages/fpa/rawimu.cpp
   src/messages/fpa/text.cpp
   src/messages/fpa/tf.cpp
+  src/messages/fpa/tp.cpp
   src/messages/nmea/gpgga.cpp
   src/messages/nmea/gpgll.cpp
   src/messages/nmea/gngsa.cpp

fixposition_driver_lib/include/fixposition_driver_lib/messages/fpa_type.hpp

@@ -180,6 +180,90 @@ struct FP_ODOMSH {
     }
 };
 
+// ------------ FP_A-ODOMSTATUS ------------
+
+struct FP_ODOMSTATUS {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    // Message fields
+    times::GpsTime stamp;
+    int init_status;
+    int fusion_imu;
+    int fusion_gnss1;
+    int fusion_gnss2;
+    int fusion_corr;
+    int fusion_cam1;
+    int fusion_ws;
+    int fusion_markers;
+    int imu_status;
+    int imu_noise;
+    int imu_conv;
+    int gnss1_status;
+    int gnss2_status;
+    int baseline_status;
+    int corr_status;
+    int cam1_status;
+    int ws_status;
+    int ws_conv;
+    int markers_status;
+    int markers_conv;
+
+    // Message structure
+    const std::string header_ = "ODOMSTATUS";
+    static constexpr unsigned int kVersion_ = 1;
+    static constexpr unsigned int kSize_ = 41;
+
+    FP_ODOMSTATUS() {
+        stamp = fixposition::times::GpsTime();
+        init_status = -1;
+        fusion_imu = -1;
+        fusion_gnss1 = -1;
+        fusion_gnss2 = -1;
+        fusion_corr = -1;
+        fusion_cam1 = -1;
+        fusion_ws = -1;
+        fusion_markers = -1;
+        imu_status = -1;
+        imu_noise = -1;
+        imu_conv = -1;
+        gnss1_status = -1;
+        gnss2_status = -1;
+        baseline_status = -1;
+        corr_status = -1;
+        cam1_status = -1;
+        ws_status = -1;
+        ws_conv = -1;
+        markers_status = -1;
+        markers_conv = -1;
+    }
+
+    void ConvertFromTokens(const std::vector<std::string>& tokens);
+
+    void ResetData() {
+        stamp = fixposition::times::GpsTime();
+        init_status = -1;
+        fusion_imu = -1;
+        fusion_gnss1 = -1;
+        fusion_gnss2 = -1;
+        fusion_corr = -1;
+        fusion_cam1 = -1;
+        fusion_ws = -1;
+        fusion_markers = -1;
+        imu_status = -1;
+        imu_noise = -1;
+        imu_conv = -1;
+        gnss1_status = -1;
+        gnss2_status = -1;
+        baseline_status = -1;
+        corr_status = -1;
+        cam1_status = -1;
+        ws_status = -1;
+        ws_conv = -1;
+        markers_status = -1;
+        markers_conv = -1;
+    }
+};
+
 // ------------ FP_A-LLH ------------
 
 struct FP_LLH {
@@ -277,6 +361,57 @@ struct FP_RAWIMU {
     }
 };
 
+// ------------ FP_A-IMUBIAS ------------
+
+struct FP_IMUBIAS {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    // Message fields
+    times::GpsTime stamp;
+    std::string frame_id;
+    int fusion_imu;
+    int imu_status;
+    int imu_noise;
+    int imu_conv;
+    Eigen::Vector3d bias_acc;
+    Eigen::Vector3d bias_gyr;
+    Eigen::Vector3d bias_cov_acc;
+    Eigen::Vector3d bias_cov_gyr;
+
+    // Message structure
+    const std::string header_ = "IMUBIAS";
+    static constexpr unsigned int kVersion_ = 1;
+    static constexpr unsigned int kSize_ = 21;
+
+    FP_IMUBIAS() {
+        stamp = fixposition::times::GpsTime();
+        frame_id = "";
+        fusion_imu = -1;
+        imu_status = -1;
+        imu_noise = -1;
+        imu_conv = -1;
+        bias_acc.setZero();
+        bias_gyr.setZero();
+        bias_cov_acc.setZero();
+        bias_cov_gyr.setZero();
+    }
+
+    void ConvertFromTokens(const std::vector<std::string>& tokens);
+
+    void ResetData() {
+        stamp = fixposition::times::GpsTime();
+        frame_id = "";
+        fusion_imu = -1;
+        imu_status = -1;
+        imu_noise = -1;
+        imu_conv = -1;
+        bias_acc.setZero();
+        bias_gyr.setZero();
+        bias_cov_acc.setZero();
+        bias_cov_gyr.setZero();
+    }
+};
+
 // ------------ FP_A-CORRIMU ------------
 
 struct FP_CORRIMU {
@@ -362,10 +497,10 @@ struct FP_GNSSCORR {
     int gnss2_fix;
     int gnss2_nsig_l1;
     int gnss2_nsig_l2;
-    float corr_latency;
-    float corr_update_rate;
-    float corr_data_rate;
-    float corr_msg_rate;
+    double corr_latency;
+    double corr_update_rate;
+    double corr_data_rate;
+    double corr_msg_rate;
     int sta_id;
     Eigen::Vector3d sta_llh;
     int sta_dist;
@@ -439,5 +574,71 @@ struct FP_TEXT {
     }
 };
 
+// ------------ FP_A-EOE ------------
+
+struct FP_EOE {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    // Message fields
+    times::GpsTime stamp;
+    std::string epoch;
+
+    // Message structure
+    const std::string header_ = "EOE";
+    static constexpr unsigned int kVersion_ = 1;
+    static constexpr unsigned int kSize_ = 6;
+
+    FP_EOE() {
+        stamp = fixposition::times::GpsTime();
+        epoch = "";
+    }
+
+    void ConvertFromTokens(const std::vector<std::string>& tokens);
+
+    void ResetData() {
+        stamp = fixposition::times::GpsTime();
+        epoch = "";
+    }
+};
+
+// ------------ FP_A-TP ------------
+
+struct FP_TP {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    // Message fields
+    std::string tp_name;
+    std::string timebase;
+    std::string timeref;
+    int tp_tow_sec;
+    double tp_tow_psec;
+    int gps_leaps;
+
+    // Message structure
+    const std::string header_ = "TP";
+    static constexpr unsigned int kVersion_ = 1;
+    static constexpr unsigned int kSize_ = 9;
+
+    FP_TP() {
+        tp_name = "";
+        timebase = "";
+        timeref = "";
+        tp_tow_sec = 0;
+        tp_tow_psec = 0.0;
+        gps_leaps = 0;
+    }
+
+    void ConvertFromTokens(const std::vector<std::string>& tokens);
+
+    void ResetData() {
+        tp_name = "";
+        timebase = "";
+        timeref = "";
+        tp_tow_sec = 0;
+        tp_tow_psec = 0.0;
+        gps_leaps = 0;
+    }
+};
+
 }  // namespace fixposition
 #endif  // __FIXPOSITION_DRIVER_LIB_CONVERTER_FPA_TYPE__

fixposition_driver_lib/include/fixposition_driver_lib/params.hpp

@@ -29,6 +29,7 @@ struct FpOutputParams {
     INPUT_TYPE type;                   //!< TCP or SERIAL
     std::vector<std::string> formats;  //!< data formats to convert, supports "FP" for now
     std::string qos_type;              //!< ROS QoS type, supports "sensor_<short/long>" and "default_<short/long>"
+    bool cov_warning;                  //!< enable/disable covariance warning
 
     std::string ip;    //!< IP address for TCP connection
     std::string port;  //!< Port for TCP connection

fixposition_driver_lib/src/fixposition_driver.cpp

@@ -198,20 +198,28 @@ bool FixpositionDriver::InitializeConverters() {
             a_converters_["ODOMENU"] = std::unique_ptr<NmeaConverter<FP_ODOMENU>>(new NmeaConverter<FP_ODOMENU>());
         } else if (format == "ODOMSH") {
             a_converters_["ODOMSH"] = std::unique_ptr<NmeaConverter<FP_ODOMSH>>(new NmeaConverter<FP_ODOMSH>());
+        } else if (format == "ODOMSTATUS") {
+            a_converters_["ODOMSTATUS"] = std::unique_ptr<NmeaConverter<FP_ODOMSTATUS>>(new NmeaConverter<FP_ODOMSTATUS>());
         } else if (format == "LLH") {
             a_converters_["LLH"] = std::unique_ptr<NmeaConverter<FP_LLH>>(new NmeaConverter<FP_LLH>());
         } else if (format == "TF") {
             a_converters_["TF"] = std::unique_ptr<NmeaConverter<FP_TF>>(new NmeaConverter<FP_TF>());
+        } else if (format == "TP") {
+            a_converters_["TP"] = std::unique_ptr<NmeaConverter<FP_TP>>(new NmeaConverter<FP_TP>());
         } else if (format == "RAWIMU") {
             a_converters_["RAWIMU"] = std::unique_ptr<NmeaConverter<FP_RAWIMU>>(new NmeaConverter<FP_RAWIMU>());
         } else if (format == "CORRIMU") {
             a_converters_["CORRIMU"] = std::unique_ptr<NmeaConverter<FP_CORRIMU>>(new NmeaConverter<FP_CORRIMU>());
+        } else if (format == "IMUBIAS") {
+            a_converters_["IMUBIAS"] = std::unique_ptr<NmeaConverter<FP_IMUBIAS>>(new NmeaConverter<FP_IMUBIAS>());
         } else if (format == "GNSSANT") {
             a_converters_["GNSSANT"] = std::unique_ptr<NmeaConverter<FP_GNSSANT>>(new NmeaConverter<FP_GNSSANT>());
         } else if (format == "GNSSCORR") {
             a_converters_["GNSSCORR"] = std::unique_ptr<NmeaConverter<FP_GNSSCORR>>(new NmeaConverter<FP_GNSSCORR>());
         } else if (format == "TEXT") {
             a_converters_["TEXT"] = std::unique_ptr<NmeaConverter<FP_TEXT>>(new NmeaConverter<FP_TEXT>());
+        } else if (format == "EOE") {
+            a_converters_["EOE"] = std::unique_ptr<NmeaConverter<FP_EOE>>(new NmeaConverter<FP_EOE>());
 
         // NMEA messages
         } else if (format == "GPGGA") {

fixposition_driver_lib/src/messages/fpa/eoe.cpp

@@ -0,0 +1,57 @@
+/**
+ *  @file
+ *  @brief Implementation of FP_A-EOE parser
+ *
+ * \verbatim
+ *  ___    ___
+ *  \  \  /  /
+ *   \  \/  /   Fixposition AG
+ *   /  /\  \   All right reserved.
+ *  /__/  \__\
+ * \endverbatim
+ *
+ */
+
+/* PACKAGE */
+#include <fixposition_driver_lib/messages/fpa_type.hpp>
+#include <fixposition_driver_lib/messages/base_converter.hpp>
+
+namespace fixposition {
+
+/// msg field indices
+static constexpr int msg_type_idx = 1;
+static constexpr int msg_version_idx = 2;
+static constexpr int gps_week_idx = 3;
+static constexpr int gps_tow_idx = 4;
+static constexpr int epoch_idx = 5;
+
+void FP_EOE::ConvertFromTokens(const std::vector<std::string>& tokens) {
+    bool ok = tokens.size() == kSize_;
+    if (!ok) {
+        // Size is wrong
+        std::cout << "Error in parsing FP_A-EOE string with " << tokens.size() << " fields!\n";
+    } else {
+        // If size is ok, check version
+        const int _version = std::stoi(tokens.at(msg_version_idx));
+
+        ok = _version == kVersion_;
+        if (!ok) {
+            // Version is wrong
+            std::cout << "Error in parsing FP_A-EOE string with version " << _version << "!\n";
+        }
+    }
+
+    if (!ok) {
+        // Reset message and return
+        ResetData();
+        return;
+    }
+
+    // Parse time
+    stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
+
+    // Populate fields
+    epoch = tokens.at(epoch_idx);
+}
+
+}  // namespace fixposition

fixposition_driver_lib/src/messages/fpa/gnssant.cpp

@@ -53,7 +53,7 @@ void FP_GNSSANT::ConvertFromTokens(const std::vector<std::string>& tokens) {
     }
 
     // Populate VRTK message header
-    stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));;
+    stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
 
     // GNSS status data
     gnss1_state = tokens.at(gnss1_state_idx);

fixposition_driver_lib/src/messages/fpa/gnsscorr.cpp

@@ -62,7 +62,7 @@ void FP_GNSSCORR::ConvertFromTokens(const std::vector<std::string>& tokens) {
     }
 
     // Populate VRTK message header
-    stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));;
+    stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
 
     // GNSS status data
     gnss1_fix     = ParseStatusFlag(tokens, gnss1_fix_idx);