Merge pull request #25 from KumarRobotics/feature/dgps

jhughes50 · web-flow · commit efd2d6408718 · 2026-03-29T22:19:37.000-04:00
DGPS Integration
diff --git a/glider/CMakeLists.txt b/glider/CMakeLists.txt
@@ -43,7 +43,8 @@ if (BUILD_ROS)
   find_package(sensor_msgs REQUIRED)
   find_package(std_msgs REQUIRED)
   find_package(nav_msgs REQUIRED)
-  
+  find_package(gps_msgs REQUIRED)
+
   set(node_plugins "")
 endif()
 # Include directories
@@ -95,6 +96,7 @@ if (BUILD_ROS)
     sensor_msgs
     geometry_msgs
     nav_msgs
+    gps_msgs
   )
 
   add_executable(${PROJECT_NAME}_node
diff --git a/glider/config/glider-params.yaml b/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:
diff --git a/glider/config/ros-params.yaml b/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
diff --git a/glider/include/glider/core/glider.hpp b/glider/include/glider/core/glider.hpp
@@ -36,6 +36,7 @@ class Glider
          *  frame does not matter */
         void addGps(int64_t timestamp, Eigen::Vector3d& gps);
         void addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps);
+        void addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, Eigen::Vector2d& heading);
         /*! @brief converts the imu measurements into the ENU frame if
          *  they are not in that frame already.
          *  @param timestamp: time the imu measurement was taken
diff --git a/glider/include/glider/utils/parameters.hpp b/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;
 };
diff --git a/glider/include/ros/conversions.hpp b/glider/include/ros/conversions.hpp
@@ -4,7 +4,9 @@
 *
 * Header file for ros to eigen conversions
 */
+#pragma once
 
+#include <utility>
 #include <chrono>
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
@@ -14,6 +16,7 @@
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <std_msgs/msg/header.hpp>
+#include <gps_msgs/msg/gps_fix.hpp>
 #include <builtin_interfaces/msg/time.hpp>
 #include <Eigen/Dense>
 
@@ -52,6 +55,7 @@ class Conversions
             static Eigen::Vector4d orientConvert(const geometry_msgs::msg::Quaternion& orient);
 
             static Eigen::Vector3d gpsConvert(const sensor_msgs::msg::NavSatFix& gps);
+            static std::pair<Eigen::Vector3d, Eigen::Vector2d> dgpsConvert(const gps_msgs::msg::GPSFix& gps);
             static Eigen::Isometry3d poseConvert(const geometry_msgs::msg::PoseStamped& msg);
             static Eigen::Isometry3d odomConvert(const nav_msgs::msg::Odometry& msg);
         };
diff --git a/glider/include/ros/glider_node.hpp b/glider/include/ros/glider_node.hpp
@@ -14,6 +14,7 @@
 #include <sensor_msgs/msg/magnetic_field.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <gps_msgs/msg/gps_fix.hpp>
 
 #include "glider/core/glider.hpp"
 #include "glider/core/odometry.hpp"
@@ -36,6 +37,7 @@ class GliderNode : public rclcpp::Node
         void interpolationCallback();
 
         // subscriber callbacks
+        void dgpsCallback(const gps_msgs::msg::GPSFix::ConstSharedPtr msg);
         void gpsCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg);
         void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg);
         void magCallback(const sensor_msgs::msg::MagneticField::ConstSharedPtr msg);
@@ -51,6 +53,7 @@ class GliderNode : public rclcpp::Node
         void publishOdometryViz(nav_msgs::msg::Odometry viz_msg) const;
 
         // subscriptions
+        rclcpp::Subscription<gps_msgs::msg::GPSFix>::ConstSharedPtr dgps_sub_;
         rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::ConstSharedPtr gps_sub_;
         rclcpp::Subscription<sensor_msgs::msg::Imu>::ConstSharedPtr imu_sub_;
         rclcpp::Subscription<sensor_msgs::msg::MagneticField>::ConstSharedPtr mag_sub_;
diff --git a/glider/launch/glider-node.launch.py b/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'),
         ]
diff --git a/glider/package.xml b/glider/package.xml
@@ -16,6 +16,7 @@
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>gps_msgs</depend>
   <depend>tf2_eigen</depend>
   <depend>message_filters</depend>
   
diff --git a/glider/ros/conversions.cpp b/glider/ros/conversions.cpp
@@ -32,6 +32,10 @@ Output Conversions::rosToEigen(const Input& msg)
     {
         return RosToEigen::odomConvert(msg);
     }
+    else if constexpr(std::is_same_v<Input, gps_msgs::msg::GPSFix>)
+    {
+        return RosToEigen::dgpsConvert(msg);
+    }
 }
 
 template<typename Output, typename Input>
@@ -92,6 +96,14 @@ Eigen::Vector3d Conversions::RosToEigen::gpsConvert(const sensor_msgs::msg::NavS
     return Eigen::Vector3d(msg.latitude, msg.longitude, msg.altitude);
 }
 
+std::pair<Eigen::Vector3d, Eigen::Vector2d> Conversions::RosToEigen::dgpsConvert(const gps_msgs::msg::GPSFix& msg)
+{
+    Eigen::Vector3d gps(msg.latitude, msg.longitude, msg.altitude);
+    Eigen::Vector2d heading(msg.track, msg.err_track);
+
+    return std::make_pair(gps, heading);
+}
+
 Eigen::Isometry3d Conversions::RosToEigen::odomConvert(const nav_msgs::msg::Odometry& msg)
 { 
     Eigen::Quaterniond quat(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z);
@@ -352,6 +364,7 @@ template Eigen::Vector3d Conversions::rosToEigen<Eigen::Vector3d>(const sensor_m
 template Eigen::Vector4d Conversions::rosToEigen<Eigen::Vector4d>(const geometry_msgs::msg::Quaternion& msg);
 template Eigen::Isometry3d Conversions::rosToEigen<Eigen::Isometry3d>(const geometry_msgs::msg::PoseStamped& msg);
 template Eigen::Isometry3d Conversions::rosToEigen<Eigen::Isometry3d>(const nav_msgs::msg::Odometry& msg);
+template std::pair<Eigen::Vector3d, Eigen::Vector2d> Conversions::rosToEigen<std::pair<Eigen::Vector3d, Eigen::Vector2d>>(const gps_msgs::msg::GPSFix& msg);
 
 template geometry_msgs::msg::Vector3 Conversions::eigenToRos<geometry_msgs::msg::Vector3>(const Eigen::Vector3d& vec);
 template geometry_msgs::msg::Quaternion Conversions::eigenToRos<geometry_msgs::msg::Quaternion>(const Eigen::Vector4d& vec);
diff --git a/glider/ros/glider_node.cpp b/glider/ros/glider_node.cpp
@@ -51,6 +51,10 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide
                                                                       std::bind(&GliderNode::gpsCallback, this, std::placeholders::_1),
                                                                       gps_sub_options);
 
+    dgps_sub_ = this->create_subscription<gps_msgs::msg::GPSFix>("/dgps", 1,
+                                                                 std::bind(&GliderNode::dgpsCallback, this, std::placeholders::_1),
+                                                                 gps_sub_options);
+
     auto odom_sub_options = rclcpp::SubscriptionOptions();
     odom_sub_options.callback_group = gps_group_;
     odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>("/odom", 1,
@@ -116,6 +120,17 @@ void GliderNode::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
     }
 }
 
+void GliderNode::dgpsCallback(const gps_msgs::msg::GPSFix::ConstSharedPtr msg)
+{
+    LOG_FIRST_N(INFO, 1) << "[GLIDER] Received DGPS measurement";
+    std::pair<Eigen::Vector3d, Eigen::Vector2d> dgps = GliderROS::Conversions::rosToEigen<std::pair<Eigen::Vector3d, Eigen::Vector2d>>(*msg);
+    int64_t timestamp = getTime(msg->header.stamp);
+
+    glider_->addGpsWithHeading(timestamp, dgps.first, dgps.second); 
+    
+    current_state_ = glider_->optimize(timestamp);
+}
+
 void GliderNode::gpsCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg)
 {
     LOG_FIRST_N(INFO, 1) << "[GLIDER] Recieved GPS measurement";
diff --git a/glider/scripts/dgps_to_gps.py b/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()
diff --git a/glider/src/glider.cpp b/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";
 }
 
@@ -73,6 +75,27 @@ void Glider::addGps(int64_t timestamp, Eigen::Vector3d& gps)
     factor_manager_.addGpsFactor(timestamp, meas);
 }
 
+void Glider::addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, Eigen::Vector2d& heading)
+{
+    // transform from lat lon To UTM
+    Eigen::Vector3d meas = Eigen::Vector3d::Zero();
+    
+    double easting, northing;
+    char zone[4];
+    geodetics::LLtoUTM(gps(0), gps(1), northing, easting, zone);
+    
+    // keep everything in the enu frame
+    meas.head(2) << easting, northing;
+    meas(2) = gps(2);
+
+    if (factor_manager_.isSystemInitialized())
+    {
+        factor_manager_.addGpsFactor(timestamp, meas, heading.x(), true);
+    } else {
+        factor_manager_.addGpsFactor(timestamp, meas, 0.0, false);
+    }
+}
+
 void Glider::addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps)
 {
     // transform from lat lon To UTM
diff --git a/glider/src/parameters.cpp b/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>();

Original file line number	Diff line number	Diff line change
`@@ -62,7 +62,7 @@ def generate_launch_description():`
`62`	`62`	`'use_odom': False}`
`63`	`63`	`],`
`64`	`64`	`remappings=[`
`65`		`- ('/gps', '/ublox_gps_node/fix'),`
	`65`	`+ ('/dgps', '/dgps/fix'),`
`66`	`66`	`('/imu', '/vectornav/imu'),`
`67`	`67`	`('/odom', '/Odometry'),`
`68`	`68`	`]`
Original file line number	Diff line number	Diff line change
`@@ -32,6 +32,10 @@ Output Conversions::rosToEigen(const Input& msg)`
`32`	`32`	`{`
`33`	`33`	`return RosToEigen::odomConvert(msg);`
`34`	`34`	`}`
	`35`	`+ else if constexpr(std::is_same_v<Input, gps_msgs::msg::GPSFix>)`
	`36`	`+ {`
	`37`	`+ return RosToEigen::dgpsConvert(msg);`
	`38`	`+ }`
`35`	`39`	`}`
`36`	`40`
`37`	`41`	`template<typename Output, typename Input>`
`@@ -92,6 +96,14 @@ Eigen::Vector3d Conversions::RosToEigen::gpsConvert(const sensor_msgs::msg::NavS`
`92`	`96`	`return Eigen::Vector3d(msg.latitude, msg.longitude, msg.altitude);`
`93`	`97`	`}`
`94`	`98`
	`99`	`+std::pair<Eigen::Vector3d, Eigen::Vector2d> Conversions::RosToEigen::dgpsConvert(const gps_msgs::msg::GPSFix& msg)`
	`100`	`+{`
	`101`	`+ Eigen::Vector3d gps(msg.latitude, msg.longitude, msg.altitude);`
	`102`	`+ Eigen::Vector2d heading(msg.track, msg.err_track);`
	`103`	`+`
	`104`	`+ return std::make_pair(gps, heading);`
	`105`	`+}`
	`106`	`+`
`95`	`107`	`Eigen::Isometry3d Conversions::RosToEigen::odomConvert(const nav_msgs::msg::Odometry& msg)`
`96`	`108`	`{`
`97`	`109`	`Eigen::Quaterniond quat(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z);`
`@@ -352,6 +364,7 @@ template Eigen::Vector3d Conversions::rosToEigen<Eigen::Vector3d>(const sensor_m`
`352`	`364`	`template Eigen::Vector4d Conversions::rosToEigen<Eigen::Vector4d>(const geometry_msgs::msg::Quaternion& msg);`
`353`	`365`	`template Eigen::Isometry3d Conversions::rosToEigen<Eigen::Isometry3d>(const geometry_msgs::msg::PoseStamped& msg);`
`354`	`366`	`template Eigen::Isometry3d Conversions::rosToEigen<Eigen::Isometry3d>(const nav_msgs::msg::Odometry& msg);`
	`367`	`+template std::pair<Eigen::Vector3d, Eigen::Vector2d> Conversions::rosToEigen<std::pair<Eigen::Vector3d, Eigen::Vector2d>>(const gps_msgs::msg::GPSFix& msg);`
`355`	`368`
`356`	`369`	`template geometry_msgs::msg::Vector3 Conversions::eigenToRos<geometry_msgs::msg::Vector3>(const Eigen::Vector3d& vec);`
`357`	`370`	`template geometry_msgs::msg::Quaternion Conversions::eigenToRos<geometry_msgs::msg::Quaternion>(const Eigen::Vector4d& vec);`