Added ROS1 and ROS2 support for generic NMEA message

fixposition · Jul 1, 2024 · a669341 · a669341
1 parent 64463c3
commit a669341
Show file tree

Hide file tree

Showing 14 changed files with 359 additions and 403 deletions.
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp
@@ -111,32 +111,6 @@ struct OdometryData {
     TwistWithCovData twist;
 };
 
-struct VrtkData {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    times::GpsTime stamp;
-    std::string frame_id;
-    std::string pose_frame;
-    std::string kin_frame;
-    PoseWithCovData pose;
-    TwistWithCovData velocity;
-    Eigen::Vector3d acceleration;
-    int fusion_status;
-    int imu_bias_status;
-    int gnss1_status;
-    int gnss2_status;
-    int wheelspeed_status;
-    std::string version;
-    VrtkData()
-        : fusion_status(-1),
-          imu_bias_status(-1),
-          gnss1_status(-1),
-          gnss2_status(-1),
-          wheelspeed_status(-1),
-          version("") {
-        acceleration.setZero();
-    }
-};
-
 struct NavSatStatusData {
     enum class Status : int8_t {
         STATUS_NO_FIX   = -1, // Unable to fix position

diff --git a/fixposition_driver_lib/src/messages/fpa/odomenu.cpp b/fixposition_driver_lib/src/messages/fpa/odomenu.cpp
@@ -100,6 +100,21 @@ void FP_ODOMENU::ConvertFromTokens(const std::vector<std::string>& tokens) {
         odom.pose.position = Vector3ToEigen(tokens.at(pos_x_idx), tokens.at(pos_y_idx), tokens.at(pos_z_idx));
         odom.pose.orientation = Vector4ToEigen(tokens.at(orientation_w_idx), tokens.at(orientation_x_idx),
                                                tokens.at(orientation_y_idx), tokens.at(orientation_z_idx));
+
+        if (odom.pose.orientation.vec().isZero() && (odom.pose.orientation.w() == 0)) {
+            // Change Fusion status to invalid state
+            fusion_status = -1;
+
+            // Reset corresponding fields
+            odom.stamp = fixposition::times::GpsTime();
+            odom.frame_id = frame_id;
+            odom.child_frame_id = child_frame_id;
+            odom.pose = PoseWithCovData();
+            odom.twist = TwistWithCovData();
+            acceleration.setZero();
+            return;
+        }
+
         odom.pose.cov = BuildCovMat6D(
             StringToDouble(tokens.at(pos_cov_xx_idx)), StringToDouble(tokens.at(pos_cov_yy_idx)),
             StringToDouble(tokens.at(pos_cov_zz_idx)), StringToDouble(tokens.at(pos_cov_xy_idx)),

diff --git a/fixposition_driver_lib/src/messages/fpa/odometry.cpp b/fixposition_driver_lib/src/messages/fpa/odometry.cpp
@@ -96,12 +96,27 @@ void FP_ODOMETRY::ConvertFromTokens(const std::vector<std::string>& tokens) {
 
     if (fusion_status >= static_cast<int>(FusionStatus::INERTIAL_GNSS_FUSION)) {
         // Message header
-        odom.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));;
+        odom.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
 
         // Pose & Cov
         odom.pose.position = Vector3ToEigen(tokens.at(pos_x_idx), tokens.at(pos_y_idx), tokens.at(pos_z_idx));
         odom.pose.orientation = Vector4ToEigen(tokens.at(orientation_w_idx), tokens.at(orientation_x_idx),
                                                tokens.at(orientation_y_idx), tokens.at(orientation_z_idx));
+
+        if (odom.pose.orientation.vec().isZero() && (odom.pose.orientation.w() == 0)) {
+            // Change Fusion status to invalid state
+            fusion_status = -1;
+
+            // Reset corresponding fields
+            odom.stamp = fixposition::times::GpsTime();
+            odom.frame_id = frame_id;
+            odom.child_frame_id = child_frame_id;
+            odom.pose = PoseWithCovData();
+            odom.twist = TwistWithCovData();
+            acceleration.setZero();
+            return;
+        }
+
         odom.pose.cov = BuildCovMat6D(
             StringToDouble(tokens.at(pos_cov_xx_idx)), StringToDouble(tokens.at(pos_cov_yy_idx)),
             StringToDouble(tokens.at(pos_cov_zz_idx)), StringToDouble(tokens.at(pos_cov_xy_idx)),

diff --git a/fixposition_driver_lib/src/messages/fpa/odomsh.cpp b/fixposition_driver_lib/src/messages/fpa/odomsh.cpp
@@ -100,6 +100,21 @@ void FP_ODOMSH::ConvertFromTokens(const std::vector<std::string>& tokens) {
         odom.pose.position = Vector3ToEigen(tokens.at(pos_x_idx), tokens.at(pos_y_idx), tokens.at(pos_z_idx));
         odom.pose.orientation = Vector4ToEigen(tokens.at(orientation_w_idx), tokens.at(orientation_x_idx),
                                                tokens.at(orientation_y_idx), tokens.at(orientation_z_idx));
+
+        if (odom.pose.orientation.vec().isZero() && (odom.pose.orientation.w() == 0)) {
+            // Change Fusion status to invalid state
+            fusion_status = -1;
+
+            // Reset corresponding fields
+            odom.stamp = fixposition::times::GpsTime();
+            odom.frame_id = frame_id;
+            odom.child_frame_id = child_frame_id;
+            odom.pose = PoseWithCovData();
+            odom.twist = TwistWithCovData();
+            acceleration.setZero();
+            return;
+        }
+
         odom.pose.cov = BuildCovMat6D(
             StringToDouble(tokens.at(pos_cov_xx_idx)), StringToDouble(tokens.at(pos_cov_yy_idx)),
             StringToDouble(tokens.at(pos_cov_zz_idx)), StringToDouble(tokens.at(pos_cov_xy_idx)),

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
@@ -58,7 +58,7 @@ 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
+     * @brief Observer Function to publish NMEA message once the GNSS epoch transmission is complete
      *
      * @param[in] data
      */

diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp
@@ -483,6 +483,11 @@ void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::TwistWit
 
 void OdometryDataToTf(const FP_ODOMETRY& data, tf2_ros::TransformBroadcaster& pub) {
     if (data.fusion_status > 0) {
+        // Ensure Fusion orientation is a valid quaternion
+        if (data.odom.pose.orientation.vec().isZero() && (data.odom.pose.orientation.w() == 0)) {
+            return;
+        }
+
         // Create message
         geometry_msgs::TransformStamped msg;
 

diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt
@@ -25,7 +25,6 @@ find_package(fixposition_gnss_tf REQUIRED)
 find_package(fixposition_driver_lib REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  msg/VRTK.msg
   msg/Speed.msg
   msg/NMEA.msg
   msg/WheelSensor.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
@@ -51,13 +51,15 @@ inline builtin_interfaces::msg::Time GpsTimeToMsgTime(times::GpsTime input) {
  * @param[in] data
  * @param[out] msg
  */
-void FpToRosMsg( const FP_GNSSANT& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSANT>::SharedPtr pub);
-void FpToRosMsg(const FP_GNSSCORR& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSCORR>::SharedPtr pub);
-void FpToRosMsg(     const FP_LLH& data, rclcpp::Publisher<fixposition_driver_ros2::msg::LLH>::SharedPtr pub);
-void FpToRosMsg( const FP_ODOMENU& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMENU>::SharedPtr pub);
-void FpToRosMsg(const FP_ODOMETRY& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMETRY>::SharedPtr pub);
-void FpToRosMsg(  const FP_ODOMSH& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMSH>::SharedPtr pub);
-void FpToRosMsg(    const FP_TEXT& data, rclcpp::Publisher<fixposition_driver_ros2::msg::TEXT>::SharedPtr pub);
+void FpToRosMsg(const OdometryData& data, rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub);
+void FpToRosMsg(     const ImuData& data, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub);
+void FpToRosMsg(  const FP_GNSSANT& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSANT>::SharedPtr pub);
+void FpToRosMsg( const FP_GNSSCORR& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSCORR>::SharedPtr pub);
+void FpToRosMsg(      const FP_LLH& data, rclcpp::Publisher<fixposition_driver_ros2::msg::LLH>::SharedPtr pub);
+void FpToRosMsg(  const FP_ODOMENU& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMENU>::SharedPtr pub);
+void FpToRosMsg( const FP_ODOMETRY& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMETRY>::SharedPtr pub);
+void FpToRosMsg(   const FP_ODOMSH& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMSH>::SharedPtr pub);
+void FpToRosMsg(     const FP_TEXT& data, rclcpp::Publisher<fixposition_driver_ros2::msg::TEXT>::SharedPtr pub);
 
 void FpToRosMsg(const GP_GGA& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GPGGA>::SharedPtr pub);
 void FpToRosMsg(const GP_GLL& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GPGLL>::SharedPtr pub);
@@ -75,15 +77,7 @@ void FpToRosMsg(const GP_ZDA& data, rclcpp::Publisher<fixposition_driver_ros2::m
  * @param[in] data
  * @param[out] msg
  */
-void ImuDataToMsg(const ImuData& data, sensor_msgs::msg::Imu& msg);
-
-/**
- * @brief 
- * 
- * @param[in] data
- * @param[in] msg
- */
-void NavSatStatusDataToMsg(const NavSatStatusData& data, sensor_msgs::msg::NavSatStatus& msg);
+void TfDataToMsg(const TfData& data, geometry_msgs::msg::TransformStamped& msg);
 
 /**
  * @brief
@@ -115,55 +109,31 @@ void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::msg::Twi
  * @param[in] data
  * @param[out] msg
  */
-void OdometryDataToMsg(const OdometryData& data, nav_msgs::msg::Odometry& msg);
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] msg
- */
-void OdometryDataToTf(const OdometryData& data, geometry_msgs::msg::TransformStamped& msg);
+void OdometryDataToTf(const FP_ODOMETRY& data, std::shared_ptr<tf2_ros::TransformBroadcaster> pub);
 
 /**
  * @brief
  *
  * @param[in] data
  * @param[out] msg
  */
-void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, sensor_msgs::msg::NavSatFix& msg);
+void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr pub);
 
 /**
  * @brief
  *
  * @param[in] data
  * @param[out] msg
  */
-void OdomToVrtkMsg(const fixposition::FP_ODOMETRY& data, fixposition_driver_ros2::msg::VRTK& msg);
+void OdomToImuMsg(const fixposition::FP_ODOMETRY& data, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr msg);
 
 /**
  * @brief
  *
  * @param[in] data
  * @param[out] msg
  */
-void OdomToImuMsg(const fixposition::FP_ODOMETRY& data, sensor_msgs::msg::Imu& msg);
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] msg
- */
-void VrtkDataToMsg(const VrtkData& data, fixposition_driver_ros2::msg::VRTK& msg);
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] msg
- */
-void TfDataToMsg(const TfData& data, geometry_msgs::msg::TransformStamped& msg);
+void OdomToYprMsg(const OdometryData& data, rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr pub);
 
 }  // 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
@@ -58,7 +58,7 @@ 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
+     * @brief Observer Function to publish NMEA message once the GNSS epoch transmission is complete
      *
      * @param[in] data
      */
@@ -72,13 +72,6 @@ class FixpositionDriverNode : public FixpositionDriver {
     rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr rtcm_sub_;  //!< RTCM3 message subscriber
 
     // ROS publishers
-    // ODOMETRY
-    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_ecef_pub_;    //!< ECEF Odometry
-    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr odometry_llh_pub_; //!< LLH Odometry
-    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_enu_pub_;     //!< ENU Odometry
-    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_smooth_pub_;  //!< Smooth Odometry (ECEF)
-    rclcpp::Publisher<fixposition_driver_ros2::msg::VRTK>::SharedPtr vrtk_pub_;  //!< FP_A-ODOMETRY message
-
     // FP_A messages
     rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSANT>::SharedPtr fpa_gnssant_pub_;   //!< FP_A-GNSSANT message
     rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSCORR>::SharedPtr fpa_gnsscorr_pub_; //!< FP_A-GNSSCORR message
@@ -99,6 +92,12 @@ class FixpositionDriverNode : public FixpositionDriver {
     rclcpp::Publisher<fixposition_driver_ros2::msg::GPVTG>::SharedPtr nmea_gpvtg_pub_;      //!< NMEA-GP-VTG message
     rclcpp::Publisher<fixposition_driver_ros2::msg::GPZDA>::SharedPtr nmea_gpzda_pub_;      //!< NMEA-GP-ZDA message
 
+    // ODOMETRY
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_ecef_pub_;    //!< ECEF Odometry
+    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr odometry_llh_pub_; //!< LLH Odometry
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_enu_pub_;     //!< ENU Odometry
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_smooth_pub_;  //!< Smooth Odometry (ECEF)
+
     // Orientation
     rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr eul_pub_;              //!< Euler angles Yaw-Pitch-Roll in local ENU
     rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr eul_imu_pub_;          //!< Euler angles Pitch-Roll as estimated from the IMU in local horizontal

diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp
@@ -25,7 +25,6 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <fixposition_driver_ros2/msg/speed.hpp>
-#include <fixposition_driver_ros2/msg/vrtk.hpp>
 #include <fixposition_driver_ros2/msg/nmea.hpp>
 
 #include <fixposition_driver_ros2/msg/gnssant.hpp>

diff --git a/fixposition_driver_ros2/msg/NMEA.msg b/fixposition_driver_ros2/msg/NMEA.msg
@@ -5,59 +5,63 @@
 #
 ####################################################################################################
 #
-# Fixposition NMEA Message
+# Fixposition NMEA Message. Specified using the WGS 84 reference ellipsoid.
 #
 #
 ####################################################################################################
 
-# Navigation Satellite fix for any Global Navigation Satellite System
-#
-# Specified using the WGS 84 reference ellipsoid
+# Format        | Field       | Unit              | Description
+# --------------|-------------|-------------------|----------------------------------------------------------------------|
+std_msgs/Header   header      # [ns]              | Specifies the ROS time and Euclidian reference frame.
+string            time        # [hhmmss.ss(ss)]   | UTC time (hours, minutes and seconds).
+string            date        # [ddmmyy]          | UTC date (day, month and year).
+float64           latitude    # [ddmm.mmmmm(mm)]  | Latitude. Positive is north of equator; negative is south.
+float64           longitude   # [dddmm.mmmmm(mm)] | Longitude. Positive is east of prime meridian; negative is west.
+float64           altitude    # [m]               | Altitude. Positive is above the WGS 84 ellipsoid.
+int8              quality     # [-]               | Quality indicator (see below).
+int8              num_sv      # [-]               | Number of satellites. Strict NMEA: 00-12. High-precision NMEA: 00-99.
+int8[]            ids         # [-]               | ID numbers of satellites used in solution. See the NMEA 0183 version 4.11 standard document.
+float64           hdop_rec    # [0.10-99.99]      | Horizontal dilution of precision.
+float64           pdop        # [-]               | Position dillution of precision.
+float64           hdop        # [-]               | Horizontal dillution of precision.
+float64           vdop        # [-]               | Vertical dillution of precision.
+float64           rms_range   # [-]               | RMS value of the standard deviation of the range inputs to the navigation process.
+float64           std_major   # [m]               | Standard deviation of semi-major axis of error ellipse.
+float64           std_minor   # [m]               | Standard deviation of semi-minor axis of error ellipse.
+float64           angle_major # [deg]             | Angle of semi-major axis of error ellipse from true North.
+float64           std_lat     # [m]               | Standard deviation of latitude error.
+float64           std_lon     # [m]               | Standard deviation of longitude error.
+float64           std_alt     # [m]               | Standard deviation of altitude error.
+float64[9]        covariance  # [m2]              | Position covariance defined relative to a tangential plane (ENU frame).
+int8              cov_type    # [-]               | Method employed to estimate covariance (see below).
+int16             num_sats    # [-]               | Total number of satellites in view.
+int16[]           sat_id      # [-]               | Satellite ID number.               
+int16[]           elev        # [deg]             | Satellite elevation. 
+int16[]           azim        # [deg]             | Satellite azimuth from true North.
+int16[]           cno         # [db-Hz]           | Satellite SNR (C/No).
+float64           heading     # [deg]             | True heading.
+float64           speed       # [m/s]             | Speed over ground.
+float64           course      # [deg]             | Course over ground (w.r.t. True North).
+float64           diff_age    # [-]               | Approximate age of differential data (last GPS MSM message received).
+string            diff_sta    # [-]               | DGPS station ID (0000-1023).
+string            sentence    # [-]               | ASCII string to be used by NTRIP clients.
 
-# header.stamp specifies the ROS time for this measurement (the
-#        corresponding satellite time may be reported using the
-#        sensor_msgs/TimeReference message).
+# Quality indicator table
 #
-# 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.
-std_msgs/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
+# | ID | Signal         |
+# |----|----------------|
+# |  0 | Invalid        |
+# |  1 | Non-RTK fix    |
+# |  4 | RTK fixed      |
+# |  5 | RTK float      |
+# |  6 | Dead-reckoning |
 
-# 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.
+# Covariance type table
 #
-# 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
+# | ID | Signal         |
+# |----|----------------|
+# |  0 | Unknown        |
+# |  1 | Approximated   |
+# |  2 | Diagonal known |
+# |  3 | Known          |