added failsafe for dgps startup

Author: jhughes50

glider/config/glider-params.yaml

@@ -9,8 +9,11 @@ imu:
   frame: "enu"
 gps: 
   covariance: 2.0
-dgpsfm:
+dgps:
   enable: true
+  covariance: 0.03
+dgpsfm:
+  enable: false
   integration_threshold: 1.0
   covariance: 0.03
 constants:

glider/config/ros-params.yaml

@@ -5,7 +5,7 @@ glider_node:
       nav_sat_fix: false
       viz:
         use: true
-        origin_easting: 482933.2819920078
-        origin_northing: 4421345.135559781
+        origin_easting: 753912.0063845584
+        origin_northing: 3385461.6073698294
     subscribers:
       use_odom: false

glider/include/glider/utils/parameters.hpp

@@ -81,6 +81,9 @@ struct Parameters
     // @brief heading noise for differential gps from motion 
     double dgpsfm_cov;
 
+    bool use_dgps;
+    double dgps_cov;
+
     // @brief translation from the GPS to the IMU
     Eigen::Vector3d t_imu_gps;
 };

glider/launch/glider-node.launch.py

@@ -62,7 +62,7 @@ def generate_launch_description():
              'use_odom': False}
         ],
         remappings=[
-            ('/gps', '/ublox_gps_node/fix'),
+            ('/dgps', '/dgps/fix'),
             ('/imu', '/vectornav/imu'),
             ('/odom', '/Odometry'),
         ]

glider/scripts/dgps_to_gps.py

@@ -0,0 +1,82 @@
+import rclpy
+from rclpy.node import Node
+
+from gps_msgs.msg import GPSFix
+from dgps_msgs.msg import DifferentialNavSatFix
+from sensor_msgs.msg import NavSatStatus
+
+
+def differential_nav_sat_fix_to_gps_fix(msg: DifferentialNavSatFix) -> GPSFix:
+    """Convert a DifferentialNavSatFix message to a GPSFix message."""
+    fix = GPSFix()
+
+    # --- Header ---
+    fix.header = msg.nmea.header
+
+    # --- Position (lat / lon / alt) ---
+    fix.latitude  = msg.nmea.latitude
+    fix.longitude = msg.nmea.longitude
+    fix.altitude  = msg.nmea.altitude
+
+    # --- Position covariance ---
+    # NavSatFix carries a 3x3 ENU position covariance (row-major).
+    # GPSFix uses the same layout, so copy it directly.
+    fix.position_covariance      = list(msg.nmea.position_covariance)
+    fix.position_covariance_type = msg.nmea.position_covariance_type
+
+    # --- Heading (track) and its error ---
+    # DifferentialNavSatFix provides a heading (degrees) and scalar covariance.
+    # GPSFix stores course-over-ground as `track` and its 1-sigma error as
+    # `err_track` (also degrees).
+    fix.track     = float(msg.heading)
+    fix.err_track = float(msg.heading_covariance) ** 0.5  # variance -> std-dev
+
+    return fix
+
+
+class DgpsConverterNode(Node):
+
+    INPUT_TOPIC  = '/dgps/dfix'
+    OUTPUT_TOPIC = '/dgps/fix'
+    QUEUE_DEPTH  = 10
+
+    def __init__(self):
+        super().__init__('dgps_converter')
+
+        self._pub = self.create_publisher(
+            GPSFix,
+            self.OUTPUT_TOPIC,
+            self.QUEUE_DEPTH,
+        )
+
+        self._sub = self.create_subscription(
+            DifferentialNavSatFix,
+            self.INPUT_TOPIC,
+            self._callback,
+            self.QUEUE_DEPTH,
+        )
+
+        self.get_logger().info(
+            f'dgps_converter ready: '
+            f'{self.INPUT_TOPIC} (DifferentialNavSatFix) -> '
+            f'{self.OUTPUT_TOPIC} (GPSFix)'
+        )
+
+    def _callback(self, msg: DifferentialNavSatFix) -> None:
+        self._pub.publish(differential_nav_sat_fix_to_gps_fix(msg))
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = DgpsConverterNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()

glider/src/glider.cpp

@@ -34,6 +34,8 @@ Glider::Glider(const std::string& path)
     LOG(INFO) << "[GLIDER] Using IMU frame: " << frame_;
     LOG(INFO) << "[GLIDER] Using Fixed Lag Smoother: " << std::boolalpha << params.smooth;
     LOG(INFO) << "[GLIDER] Using DGPS From Motion: " << std::boolalpha << params.use_dgpsfm;
+    LOG(INFO) << "[GLIDER] Using DGPS: " << std::boolalpha << params.use_dgps;
+    assert(!(params.use_dgpsfm && params.use_dgps) && "Both DGPS and DGPS From Motion are set to true, this is not allowed");
     LOG(INFO) << "[GLIDER] Glider initialized";
 }
 

glider/src/parameters.cpp

@@ -40,6 +40,9 @@ Glider::Parameters::Parameters(const std::string& path)
         dgpsfm_threshold = config["dgpsfm"]["integration_threshold"].as<double>();
         dgpsfm_cov = config["dgpsfm"]["covariance"].as<double>();
 
+        use_dgps = config["dgps"]["enable"].as<bool>();
+        dgps_cov = config["dgps"]["covariance"].as<double>();
+
         t_imu_gps(0) = config["gps_to_imu"]["x"].as<double>();
         t_imu_gps(1) = config["gps_to_imu"]["y"].as<double>();
         t_imu_gps(2) = config["gps_to_imu"]["z"].as<double>();