ROS2 implementation - Please test

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 {
 

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

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

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);
 }