update dgpsfm branch

.github/workflows/humble-ci.yml

@@ -0,0 +1,24 @@
+name: Humble CI
+
+on:
+  push:
+    branches: [ ros2 ]
+  pull_request:
+    branches: [ ros2 ]
+
+jobs:
+  unit-tests-humble:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+
+      - name: Build Docker image with dependencies
+        run: cd docker && bash build-humble.bash
+
+      - name: Run unit test in Docker
+        run: |
+          docker run --rm -v $(pwd):/workspace -w /workspace \
+            glider-ros:humble \
+            bash -c "cd glider && cmake -S . -B build -DBUILD_TESTS=ON \
+                      && cd build && cmake --build . && ctest --output-on-failure"
+

.github/workflows/jazzy-ci.yml

@@ -0,0 +1,23 @@
+name: Jazzy CI
+
+on:
+  push:
+    branches: [ ros2 ]
+  pull_request:
+    branches: [ ros2 ]
+
+jobs:
+  unit-tests-jazzy:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+
+      - name: Build Docker image with dependencies
+        run: cd docker && bash build-jazzy.bash
+
+      - name: Run unit test in Docker
+        run: |
+          docker run --rm -v $(pwd):/workspace -w /workspace \
+            glider-ros:jazzy \
+            bash -c "cd glider && cmake -S . -B build -DBUILD_TESTS=ON \
+                      && cd build && cmake --build . && ctest --output-on-failure"

LICENSE

@@ -1,6 +1,6 @@
 BSD 3-Clause License
 
-Copyright (c) 2022, ublox contributors
+Copyright (c) 2025, glider contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

README.md

@@ -1,30 +1,79 @@
-## Glider
+# Glider
+![Humble CI](https://github.com/KumarRobotics/glider/actions/workflows/humble-ci.yml/badge.svg?branch=ros2)
 
-Glider is a GPS-Odometry-INS system, it is designed to fuse GPS and IMU (a GINS system) with any odometry you provide. This can also be run as a standard GINS system by setting `use_odom:=false`. We can also take in unscaled odometry and scale it, just set `scale_odom` parameter to `true` if you want your odmetry scaled. Finally, noise parameters can be configured in `config/graph_params.yaml`.
+![Jazzy CI](https://github.com/KumarRobotics/glider/actions/workflows/jazzy-ci.yml/badge.svg?branch=ros2)
 
-You can build this as a ros2 package with colcon: `colcon build --packages-select glider`.
+Glider is a G-INS system built on [GTSAM](https://github.com/borglab/gtsam). It currently takes in GPS and 9-DOF IMU and provides a full
+state estimate up to the rate of you IMU. Glider is designed to be configured to your system. 
 
-### Running
-Glider can be built as a ros packages in your ros workspace with `catkin build`.
-Run glider with:
+## Building Glider
+To run glider you can use the provided docker images, ROS2 jazzy and humble are both supported, simply use the `build.bash` and `run.bash` files. You can mounted volumes in the run files if necessary. If you want to inlcude this in another ROS2 workspace, you may need to install the following dependencies:
 ```
-ros2 launch glider glider-node.launch.py
+apt install ros-$ROS_DISTRO-gtsam libgoogle-glog-dev libgest-dev
 ```
+Glider can be build with colcon as a ROS2 package with:
+```
+colcon build --packages-select glider
+```
+If you only want the api you can build with:
+```
+cmake -S . -B build -DBUILD_ROS=OFF
+cmake --build build 
+```
+
+## Running Glider
+To run glider with ros use `ros2 run glider glider-node.launch.py`. If you are running with a bag file use:
+```
+ros2 bag run <bag_file_name> --clock
+ros2 launch glider glider-node.launch.py use_sim_time:=true
+```
+
+## Hardware Setup
+You're setup needs a GPS and a 9-DOF IMU, that is an IMU that provides a full orientation. The IMU orientation should be provided in the IMU's frame
+as this is standard for robotics, but we are working on supporting the NED frame. We use a VectorNav VN100T IMU. It is important make sure your IMU magnetometer is calibrated, if it is not aligned correctly the heading output of glider will be incorrect.
+
+## ROS2 Setup
+We recommend using Glider with ROS2, you can configure the ros parameters in `config/ros-params.yaml`. Here's more detail about what
+the parameters mean:
+ - `publishers.rate`: the rate at which odometry is published in hz, setting this to 0 publishes at IMU rate.
+ - `publishers.nav_sat_fix`: if true will publish the odometry as a `NavSatFix` msg, the default is an `Odometry` msg.
+ - `publishers.viz.use`: if true will publish an `Odometry` topic for visualization centered around the origin.
+ - `publishers.viz.origin_easting`: the easting value you want to viz odometry to center around.
+ - `publishers.viz.origin_northing`: the northing value you want the viz odometry to center around.
+ - `subscribers.use_odom`: Still under development.
 
-Everything will be rotated into the ENU frame, includeing the IMU orientation and the Odometry pose. Since GPS is the main prior, we publish state estimates in the ENU frame with UTM coordinates as an `Odometry` ros message or a `NavSatFix` message. This can be configured in `config/ros_params.yaml` with the `publish_nav_sat_fix` parameter.
+## Glider Setup
+You can configure glider itself in `config/glider-params.yaml`, this is where you can specify the parameters for the factor graph. Here's more detail on each parameter:
+#### IMU Parameters
+ - `covariances.accelerometer`: covariance of the accelerometer.
+ - `covariances.gyroscope`: covariance of the gyroscope.
+ - `covariances.integration`: covariance of the IMU preintegration.
+ - `covariances.heading`: covariance of the IMU's magnetometer heading in radians, 0.09 radians is about 5 degrees.
+ - `covariances.roll_pitch`: covariance of the roll and pitch angles in radians.
+ - `covariances.bias`: covariance of the bias estimate.
+ - `frame`: What frame the IMU is in, either `enu` or `ned`.
+#### GPS Parameters
+ - `gps.covariance`: covariance of the gps position estimate.
+#### Other Parameters
+ - `constants.gravity`: gravity in your IMU's frame.
+ - `constants.bias_num_measurements`: number of IMU measurements to use to initially estimate the bias.
+ - `constants.initial_num_measurements`: number of times to let the factor graph optimize before glider starts reporting odometry.
+ - `logging.stdout`: output log statements to terminal in addition to the logfile
+ - `optimizer.smooth`: if true the factor graph will optimize using a fixed lag smoother, otherwise it will use iSAM2.
+ - `optimizer.lag_time`: period of time the fixed lag smoother should look at in seconds.
+ - `gps_to_imu`: the relative transformation from your gps to your imu in the FLU frame.
 
 ### Building and Running Unit Tests
 We use GTest to run unit tests. You can build the tests with 
 ``` 
-cmake -S . -B build
+cd glider
+cmake -S . -B build -DBUILD_TESTS=ON
 cmake --build build
 ```
 and run with:
 ```
 cd build 
 ctest
 ```
+Note these tests are run on PR's and pushes to the `ros2` branch.
 
-#### Authors
- - **Corresponding:** Jason Hughes jasonah.at.seas.upenn.edu
- - Varun Murali

docker/Dockerfile.humble

@@ -37,8 +37,6 @@ RUN sudo apt-get update \
 RUN sudo apt-get install -y python3-colcon-common-extensions
 
 # install visualizer deps
-RUN sudo apt-get install -y python3-pip
-RUN pip3 install flask folium flask_socketio utm
 RUN sudo apt install -y libgoogle-glog-dev
 # build ros env
 RUN mkdir ws

docker/Dockerfile → docker/Dockerfile.jazzy

@@ -37,9 +37,6 @@ RUN sudo apt-get update \
 
 RUN sudo apt-get install -y python3-colcon-common-extensions
 
-# install visualizer deps
-#RUN sudo apt-get install -y python3-pip
-#RUN pip3 install flask folium flask_socketio utm
 RUN sudo apt install -y libgoogle-glog-dev
 # build ros env
 RUN mkdir ws

docker/build-humble.bash

@@ -1,3 +1,3 @@
 #!/bin/bash
 
-docker build --build-arg user_id=$(id -u) --build-arg USER=$(whoami) --build-arg NAME=$(hostname) --rm -t glider-ros:humble .
+docker build --build-arg user_id=$(id -u) --build-arg USER=$(whoami) --build-arg NAME=$(hostname) --rm -t glider-ros:humble -f Dockerfile.humble .

docker/build-jazzy.bash

@@ -1,3 +1,3 @@
 #!/bin/bash
 
-docker build --build-arg user_id=$(id -u) --build-arg USER=$(whoami) --build-arg NAME=$(hostname) --rm -t glider-ros:jazzy .
+docker build --build-arg user_id=$(id -u) --build-arg USER=$(whoami) --build-arg NAME=$(hostname) --rm -t glider-ros:jazzy -f Dockerfile.jazzy .

docker/run-humble.bash

@@ -0,0 +1,17 @@
+#!/bin/bash
+
+xhost +
+docker run -it --rm \
+    --network=host \
+    --privileged \
+    -v "/dev:/dev" \
+    -v "/tmp/.X11-unix:/tmp/.X11-unix" \
+    -v "`pwd`/../glider:/home/`whoami`/ws/src/glider" \
+    -v "/home/`whoami`/Data:/home/`whoami`/data" \
+    -e DISPLAY=$DISPLAY \
+    -e QT_X11_NO_MITSHM=1 \
+    -e XAUTHORITY=$XAUTH \
+    --name glider-ros-jazzy \
+    glider-ros:jazzy \
+    bash
+xhost -

glider/CMakeLists.txt

@@ -59,6 +59,7 @@ add_library(${PROJECT_NAME} SHARED
   src/parameters.cpp
   src/time.cpp
   src/gps_heading.cpp
+  src/differential_gps.cpp
   src/odometry.cpp
   src/odometry_with_covariance.cpp
   src/factor_manager.cpp
@@ -200,7 +201,14 @@ if (BUILD_TESTS)
         GTest::Main
         ${PROJECT_NAME}
     )
-
+
+    add_executable(ros_conversion_test test/test_ros_conversions.cpp)
+    target_link_libraries(ros_conversion_test
+        GTest::GTest
+        GTest::Main
+        ${PROJECT_NAME}
+        ${PROJECT_NAME}_ros
+    )
 
     include(GoogleTest)
     gtest_discover_tests(utm_test)
@@ -210,4 +218,5 @@ if (BUILD_TESTS)
     gtest_discover_tests(glider_test)
     gtest_discover_tests(odometry_test)
     gtest_discover_tests(odometry_with_cov_test)
+    gtest_discover_tests(ros_conversion_test)
 endif()

glider/config/graph-params.yaml → glider/config/glider-params.yaml

@@ -19,6 +19,7 @@ constants:
   initial_num_measurements: 4
 logging:
   stdout: true
+  directory: "/tmp/glider"
 optimizer:
   smooth: true
   lag_time: 5.0

glider/config/ros-params.yaml

@@ -1,7 +1,7 @@
 glider_node:
   ros__parameters:
     publishers:
-      rate: 100.0
+      rate: 0.0
       nav_sat_fix: false
       viz:
         use: true

glider/include/glider/core/differential_gps.hpp

@@ -0,0 +1,43 @@
+/*!
+*  Jason Hughes
+*  October 2025
+*
+*  Object that handles everything for differenctial gps
+*  from motion
+*/
+
+#pragma once 
+
+#include <cmath>
+#include <string>
+#include <Eigen/Dense>
+
+namespace Glider
+{
+namespace Geodetics
+{
+class DifferentialGpsFromMotion
+{
+    public:
+        DifferentialGpsFromMotion() = default;
+        DifferentialGpsFromMotion(const std::string& frame, const double& threshold);
+
+        double getHeading(const Eigen::Vector3d& gps);
+
+        double headingRadiansToDegrees(const double heading) const;
+
+        void setLastGps(const Eigen::Vector3d& gps);
+        bool isInitialized() const;
+        double getVelocityThreshold() const;
+
+    private:
+
+        double nedToEnu(const double heading) const;
+
+        Eigen::Vector3d last_gps_;
+        std::string frame_;
+        double vel_threshold_;
+        bool initialized_{false};
+};
+} // namespace Geodetics
+} // namespace Glider

glider/include/glider/core/glider.hpp

@@ -13,7 +13,7 @@
 #include <glog/logging.h>
 
 #include "glider/core/factor_manager.hpp"
-#include "glider/utils/gps_heading.hpp"
+#include "glider/core/differential_gps.hpp"
 #include "glider/utils/geodetics.hpp"
 
 namespace Glider
@@ -35,6 +35,7 @@ class Glider
          *  should be in degree decimal and altitude in meters. Altitude
          *  frame does not matter */
         void addGps(int64_t timestamp, Eigen::Vector3d& gps);
+        void addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps);
         /*! @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
@@ -60,6 +61,12 @@ class Glider
          *  parameters
          *  @params: the laoded params from the yaml file */
         void initializeLogging(const Parameters& params) const;
+        /*! @brief rotate a quaternion by a rotation matrix
+         *  @params rot: the rotation matrix you want to rotate the quaterniuon by
+         *  @param quat: the orientation we want to rotate
+         *  @return the rotated orientation as quaternion as a Vector4d in
+         *  (w, x, y, z) format */
+        Eigen::Vector4d rotateQuaternion(const Eigen::Matrix3d& rot, const Eigen::Vector4d& quat) const;
 
         // @brief the factor manager, handles the factor graph
         FactorManager factor_manager_;
@@ -70,15 +77,16 @@ class Glider
         // @brief the relative translation from the gps to
         // the imu
         Eigen::Vector3d t_imu_gps_;
+        // @brief the rotation matrix from ned to enu frame
+        Eigen::Matrix3d r_enu_ned_;
         // @brief whether or not to use differential gps
         // from motion for heading
         bool use_dgpsfm_;
+        // @brief object to handle differential gps 
+        // from motion
+        Geodetics::DifferentialGpsFromMotion dgps_;
         // @brief save the state estimate from 
         // the optimizer
         OdometryWithCovariance current_odom_;
-        // @brief save the previous gps measurement for 
-        // dgpsfm 
-        Eigen::Vector3d last_gps_;
-        double vel_threshold_;
 };
 }

glider/include/glider/utils/parameters.hpp

@@ -61,6 +61,9 @@ struct Parameters
     // @brief if true this logs to stdout and log file otherwise it logs
     // just to a file
     bool log;
+    // @brief the directory to save the log file, glog needs an 
+    // absolute path
+    std::string log_dir;
 
     // @brief if true optimize using a fixed lag smoother, otherwise
     // optimize with iSAM2

glider/include/ros/glider_node.hpp

@@ -71,19 +71,14 @@ class GliderNode : public rclcpp::Node
 
         // parameters
         bool initialized_;
-        bool use_sim_time_;
         bool publish_nsf_;
         bool viz_;
         std::string utm_zone_;
-        double declination_;
         double origin_easting_;
         double origin_northing_;
-        int gps_counter_;
-        int gps_init_count_;
+        double freq_;
 
         // tracker
         Glider::OdometryWithCovariance current_state_;
-
-        int64_t latest_imu_timestamp_;
 };
 }