Reworked the ROS driver to include ODOMSH and ODOMENU messages, simpl…

…ified the TF tree, and solved several bugs in the LLH message.

CHANGELOG.md

@@ -45,7 +45,7 @@ _For questions about compatibility, please contact Fixpositions Support support@
 ## [4.4.0](https://github.com/fixposition/fixposition_driver/releases/tag/4.4.0)
 
     -   Pitch-Roll estimation from IMU data parsed from Vision-RTK 2, output available before fusion initialization.
-    -   OdometryConverter imeplemented as an example for wheelspeed data integration.
+    -   OdometryConverter implemented as an example for wheelspeed data integration.
 
 ## [4.3.0](https://github.com/fixposition/fixposition_driver/releases/tag/4.3.0)
 

fixposition_driver_lib/CMakeLists.txt

@@ -24,8 +24,9 @@ include_directories(include
 add_library(
   ${PROJECT_NAME} SHARED
   src/fixposition_driver.cpp
+  src/odomenu.cpp
   src/odometry.cpp
-  src/llh.cpp
+  src/odomsh.cpp
   src/imu.cpp
   src/tf.cpp
   src/helper.cpp

fixposition_driver_lib/include/fixposition_driver_lib/converter/base_converter.hpp

@@ -99,5 +99,67 @@ inline times::GpsTime ConvertGpsTime(const std::string& gps_wno, const std::stri
         return times::GpsTime(0, 0);
     }
 }
+
+/**
+ * @brief Build a 6x6 covariance matrix which is 2 independent 3x3 matrices
+ *
+ * [xx, xy, xz, 0, 0, 0,
+ *  xy, yy, yz, 0, 0, 0,
+ *  xz, yz, zz, 0, 0, 0,
+ *  0, 0, 0, xx1, xy1, xz1,
+ *  0, 0, 0, xy1, yy1, yz1,
+ *  0, 0, 0, xz1, yz1, zz1]
+ *
+ * @param[in] xx
+ * @param[in] yy
+ * @param[in] zz
+ * @param[in] xy
+ * @param[in] yz
+ * @param[in] xz
+ * @param[in] xx1
+ * @param[in] yy1
+ * @param[in] zz1
+ * @param[in] xy1
+ * @param[in] yz1
+ * @param[in] xz1
+ * @return Eigen::Matrix<double, 6, 6> the 6x6 matrix
+ */
+inline Eigen::Matrix<double, 6, 6> BuildCovMat6D(const double xx, const double yy, const double zz, const double xy,
+                                                 const double yz, const double xz, double xx1, const double yy1,
+                                                 const double zz1, const double xy1, const double yz1, double xz1) {
+    Eigen::Matrix<double, 6, 6> cov;
+    // Diagonals
+    cov(0, 0) = xx;   // 0
+    cov(1, 1) = yy;   // 7
+    cov(2, 2) = zz;   // 14
+    cov(3, 3) = xx1;  // 21
+    cov(4, 4) = yy1;  // 28
+    cov(5, 5) = zz1;  // 35
+
+    // Rest of values
+    cov(1, 0) = cov(0, 1) = xy;   // 1 = 6
+    cov(2, 1) = cov(1, 2) = yz;   // 8 = 13
+    cov(2, 0) = cov(0, 2) = xz;   // 2 = 12
+    cov(4, 3) = cov(3, 4) = xy1;  // 22 = 27
+    cov(5, 4) = cov(4, 5) = yz1;  // 29 = 34
+    cov(5, 3) = cov(3, 5) = xz1;  // 23 = 33
+
+    return cov;
+}
+
+/**
+ * @brief Convert radians to degrees
+ *
+ * @tparam T data type
+ * @param radians angle in [rad]
+ *
+ * @returns the angle in [deg]
+ */
+template <typename T>
+constexpr inline T RadToDeg(T radians) {
+    static_assert(std::is_floating_point<T>::value, "Only floating point types allowed!");
+    return radians * 57.295779513082320876798154814105;
+}
+
 }  // namespace fixposition
 #endif  // __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER__

fixposition_driver_lib/include/fixposition_driver_lib/converter/odomenu.hpp

@@ -0,0 +1,84 @@
+/**
+ *  @file
+ *  @brief Declaration of OdomenuConverter
+ *
+ * \verbatim
+ *  ___    ___
+ *  \  \  /  /
+ *   \  \/  /   Fixposition AG
+ *   /  /\  \   All right reserved.
+ *  /__/  \__\
+ * \endverbatim
+ *
+ */
+
+#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_ODOMENU__
+#define __FIXPOSITION_DRIVER_LIB_CONVERTER_ODOMENU__
+
+/* SYSTEM / STL */
+
+/* EXTERNAL */
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Geometry>
+
+/* 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 OdomenuConverter : public BaseAsciiConverter {
+   public:
+    /**
+     * @brief Data for Messages published from the ODOMENU msg
+     *
+     */
+    struct Msgs {
+        OdometryData odometry;
+        Eigen::Vector3d eul;
+    };
+
+    using OdomenuObserver = std::function<void(const Msgs&)>;
+
+    /**
+     * @brief Construct a new Fixposition Msg Converter object
+     *
+     */
+    OdomenuConverter() : BaseAsciiConverter() {}
+
+    ~OdomenuConverter() = default;
+
+    /**
+     * @brief Comma Delimited FP,ODOMENU message, convert to Data structs and if available,
+     * call observers
+     * Example:
+     * $FP,ODOMENU,1,2180,298591.500000,-1.8339,2.6517,-0.0584,0.556794,-0.042551,-0.007850,0.829523,2.2993,
+     * -1.6994,-0.0222,0.20063,0.08621,-1.21972,-3.6947,-3.3827,9.7482,4,1,8,8,1,0.00415,0.00946,0.00746,
+     * -0.00149,-0.00084,0.00025,0.00003,0.00003,0.00012,0.00000,0.00000,0.00000,0.01742,0.01146,0.01612,
+     * -0.00550,-0.00007,-0.00050*74\r\n
+     *
+     * @param[in] state state message as string
+     * @return nav_msgs::Odometry message
+     */
+    virtual void ConvertTokens(const std::vector<std::string>& tokens) final;
+
+    /**
+     * @brief Add Observer to call at the end of ConvertTokens()
+     *
+     * @param[in] ob
+     */
+    void AddObserver(OdomenuObserver ob) { obs_.push_back(ob); }
+
+   private:
+    const std::string header_ = "ODOMENU";
+    static constexpr const int kVersion_ = 1;
+    static constexpr const int kSize_ = 44;
+
+    Msgs msgs_;
+    std::vector<OdomenuObserver> obs_;
+};
+
+}  // namespace fixposition
+#endif  // __FIXPOSITION_DRIVER_LIB_CONVERTER_ODOMENU__

fixposition_driver_lib/include/fixposition_driver_lib/converter/odometry.hpp

@@ -38,12 +38,9 @@ class OdometryConverter : public BaseAsciiConverter {
     struct Msgs {
         OdometryData odometry;
         ImuData imu;
-        OdometryData odometry_enu0;
         VrtkData vrtk;
-        Eigen::Vector3d eul;
         TfData tf_ecef_poi;
-        TfData tf_ecef_enu;
-        TfData tf_ecef_enu0;
+        NavSatFixData odom_llh;
     };
 
     using OdometryObserver = std::function<void(const Msgs&)>;
@@ -52,10 +49,7 @@ class OdometryConverter : public BaseAsciiConverter {
      * @brief Construct a new Fixposition Msg Converter object
      *
      */
-    OdometryConverter() : BaseAsciiConverter(), tf_ecef_enu0_set_(false) {
-        msgs_.tf_ecef_enu0.frame_id = "ECEF";
-        msgs_.tf_ecef_enu0.child_frame_id = "FP_ENU0";
-    }
+    OdometryConverter() : BaseAsciiConverter() {}
 
     ~OdometryConverter() = default;
 
@@ -85,61 +79,9 @@ class OdometryConverter : public BaseAsciiConverter {
     static constexpr const int kVersion_ = 2;
     static constexpr const int kSize_ = 45;
 
-    //! transform between ECEF and ENU0
-    bool tf_ecef_enu0_set_;  //!< flag to indicate if the tf is already set
-    Eigen::Vector3d t_ecef_enu0_;
-    Eigen::Quaterniond q_ecef_enu0_;
-
     Msgs msgs_;
     std::vector<OdometryObserver> obs_;
 };
 
-/**
- * @brief Build a 6x6 covariance matrix which is 2 independent 3x3 matrices
- *
- * [xx, xy, xz, 0, 0, 0,
- *  xy, yy, yz, 0, 0, 0,
- *  xz, yz, zz, 0, 0, 0,
- *  0, 0, 0, xx1, xy1, xz1,
- *  0, 0, 0, xy1, yy1, yz1,
- *  0, 0, 0, xz1, yz1, zz1]
- *
- * @param[in] xx
- * @param[in] yy
- * @param[in] zz
- * @param[in] xy
- * @param[in] yz
- * @param[in] xz
- * @param[in] xx1
- * @param[in] yy1
- * @param[in] zz1
- * @param[in] xy1
- * @param[in] yz1
- * @param[in] xz1
- * @return Eigen::Matrix<double, 6, 6> the 6x6 matrix
- */
-inline Eigen::Matrix<double, 6, 6> BuildCovMat6D(const double xx, const double yy, const double zz, const double xy,
-                                                 const double yz, const double xz, double xx1, const double yy1,
-                                                 const double zz1, const double xy1, const double yz1, double xz1) {
-    Eigen::Matrix<double, 6, 6> cov;
-    // Diagonals
-    cov(0, 0) = xx;   // 0
-    cov(1, 1) = yy;   // 7
-    cov(2, 2) = zz;   // 14
-    cov(3, 3) = xx1;  // 21
-    cov(4, 4) = yy1;  // 28
-    cov(5, 5) = zz1;  // 35
-
-    // Rest of values
-    cov(1, 0) = cov(0, 1) = xy;   // 1 = 6
-    cov(2, 1) = cov(1, 2) = yz;   // 8 = 13
-    cov(2, 0) = cov(0, 2) = xz;   // 2 = 12
-    cov(4, 3) = cov(3, 4) = xy1;  // 22 = 27
-    cov(5, 4) = cov(4, 5) = yz1;  // 29 = 34
-    cov(5, 3) = cov(3, 5) = xz1;  // 23 = 33
-
-    return cov;
-}
-
 }  // namespace fixposition
 #endif  // __FIXPOSITION_DRIVER_LIB_CONVERTER_ODOMETRY__