From 874e8ec2d1388873c22d30a4825118e55bfbc87d Mon Sep 17 00:00:00 2001 From: jason Date: Mon, 13 Oct 2025 16:31:38 -0400 Subject: [PATCH 01/20] added docker files for ROS2 jazzy --- docker/{Dockerfile => Dockerfile.humble} | 0 docker/Dockerfile.jazzy | 49 ++++++++++++++++++++++++ docker/build-humble.bash | 3 ++ docker/build-jazzy.bash | 3 ++ docker/build.bash | 3 -- docker/{run.bash => run-humble.bash} | 0 docker/run-jazzy.bash | 17 ++++++++ 7 files changed, 72 insertions(+), 3 deletions(-) rename docker/{Dockerfile => Dockerfile.humble} (100%) create mode 100644 docker/Dockerfile.jazzy create mode 100755 docker/build-humble.bash create mode 100755 docker/build-jazzy.bash delete mode 100755 docker/build.bash rename docker/{run.bash => run-humble.bash} (100%) create mode 100755 docker/run-jazzy.bash diff --git a/docker/Dockerfile b/docker/Dockerfile.humble similarity index 100% rename from docker/Dockerfile rename to docker/Dockerfile.humble diff --git a/docker/Dockerfile.jazzy b/docker/Dockerfile.jazzy new file mode 100644 index 0000000..14cc602 --- /dev/null +++ b/docker/Dockerfile.jazzy @@ -0,0 +1,49 @@ +FROM osrf/ros:jazzy-desktop-full + +ARG DEBIAN_FRONTEND=noninteractive + +# install the basics +RUN apt-get update \ + && apt-get install -y --no-install-recommends \ + vim \ + tmux \ + cmake \ + gcc \ + g++ \ + git \ + build-essential \ + sudo \ + wget \ + curl \ + zip \ + unzip + +# add a user +ARG user_id +ARG USER noble +RUN userdel ubuntu +RUN useradd -U --uid ${user_id} -ms /bin/bash $USER \ + && echo "$USER:$USER" | chpasswd \ + && adduser $USER sudo \ + && echo "$USER ALL=NOPASSWD: ALL" >> /etc/sudoers.d/$USER + +# add groups to get access to sensors +USER $USER +WORKDIR /home/$USER + +# imu driver +RUN sudo apt-get update \ + && sudo apt-get install -y ros-jazzy-gtsam + +RUN sudo apt-get install -y python3-colcon-common-extensions + +RUN sudo apt install -y libgoogle-glog-dev +# build ros env +RUN mkdir ws + +# make life nice inside docker +RUN sudo chown $USER:$USER ~/.bashrc \ + && /bin/sh -c 'echo ". /opt/ros/jazzy/setup.bash" >> ~/.bashrc' \ + && /bin/sh -c 'echo "source ~/ws/install/setup.bash" >> ~/.bashrc' \ + && echo 'export PS1="\[$(tput setaf 2; tput bold)\]\u\[$(tput setaf 7)\]@\[$(tput setaf 3)\]\h\[$(tput setaf 7)\]:\[$(tput setaf 4)\]\W\[$(tput setaf 7)\]$ \[$(tput sgr0)\]"' >> ~/.bashrc + diff --git a/docker/build-humble.bash b/docker/build-humble.bash new file mode 100755 index 0000000..18f1a39 --- /dev/null +++ b/docker/build-humble.bash @@ -0,0 +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 -f Dockerfile.humble . diff --git a/docker/build-jazzy.bash b/docker/build-jazzy.bash new file mode 100755 index 0000000..7f8de97 --- /dev/null +++ b/docker/build-jazzy.bash @@ -0,0 +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 -f Dockerfile.jazzy . diff --git a/docker/build.bash b/docker/build.bash deleted file mode 100755 index 8f499a7..0000000 --- a/docker/build.bash +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -docker build --build-arg user_id=$(id -u) --build-arg USER=$(whoami) --build-arg NAME=$(hostname) --rm -t glider-ros:humble . diff --git a/docker/run.bash b/docker/run-humble.bash similarity index 100% rename from docker/run.bash rename to docker/run-humble.bash diff --git a/docker/run-jazzy.bash b/docker/run-jazzy.bash new file mode 100755 index 0000000..0fc33bc --- /dev/null +++ b/docker/run-jazzy.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 - From 7cc11e3c63669e968b0715eef2defff1e43bb94c Mon Sep 17 00:00:00 2001 From: Jason Hughes <50918235+jhughes50@users.noreply.github.com> Date: Mon, 13 Oct 2025 16:35:54 -0400 Subject: [PATCH 02/20] Revert "added docker files for ROS2 jazzy" --- docker/{Dockerfile.humble => Dockerfile} | 0 docker/Dockerfile.jazzy | 49 ------------------------ docker/build-humble.bash | 3 -- docker/build-jazzy.bash | 3 -- docker/build.bash | 3 ++ docker/run-jazzy.bash | 17 -------- docker/{run-humble.bash => run.bash} | 0 7 files changed, 3 insertions(+), 72 deletions(-) rename docker/{Dockerfile.humble => Dockerfile} (100%) delete mode 100644 docker/Dockerfile.jazzy delete mode 100755 docker/build-humble.bash delete mode 100755 docker/build-jazzy.bash create mode 100755 docker/build.bash delete mode 100755 docker/run-jazzy.bash rename docker/{run-humble.bash => run.bash} (100%) diff --git a/docker/Dockerfile.humble b/docker/Dockerfile similarity index 100% rename from docker/Dockerfile.humble rename to docker/Dockerfile diff --git a/docker/Dockerfile.jazzy b/docker/Dockerfile.jazzy deleted file mode 100644 index 14cc602..0000000 --- a/docker/Dockerfile.jazzy +++ /dev/null @@ -1,49 +0,0 @@ -FROM osrf/ros:jazzy-desktop-full - -ARG DEBIAN_FRONTEND=noninteractive - -# install the basics -RUN apt-get update \ - && apt-get install -y --no-install-recommends \ - vim \ - tmux \ - cmake \ - gcc \ - g++ \ - git \ - build-essential \ - sudo \ - wget \ - curl \ - zip \ - unzip - -# add a user -ARG user_id -ARG USER noble -RUN userdel ubuntu -RUN useradd -U --uid ${user_id} -ms /bin/bash $USER \ - && echo "$USER:$USER" | chpasswd \ - && adduser $USER sudo \ - && echo "$USER ALL=NOPASSWD: ALL" >> /etc/sudoers.d/$USER - -# add groups to get access to sensors -USER $USER -WORKDIR /home/$USER - -# imu driver -RUN sudo apt-get update \ - && sudo apt-get install -y ros-jazzy-gtsam - -RUN sudo apt-get install -y python3-colcon-common-extensions - -RUN sudo apt install -y libgoogle-glog-dev -# build ros env -RUN mkdir ws - -# make life nice inside docker -RUN sudo chown $USER:$USER ~/.bashrc \ - && /bin/sh -c 'echo ". /opt/ros/jazzy/setup.bash" >> ~/.bashrc' \ - && /bin/sh -c 'echo "source ~/ws/install/setup.bash" >> ~/.bashrc' \ - && echo 'export PS1="\[$(tput setaf 2; tput bold)\]\u\[$(tput setaf 7)\]@\[$(tput setaf 3)\]\h\[$(tput setaf 7)\]:\[$(tput setaf 4)\]\W\[$(tput setaf 7)\]$ \[$(tput sgr0)\]"' >> ~/.bashrc - diff --git a/docker/build-humble.bash b/docker/build-humble.bash deleted file mode 100755 index 18f1a39..0000000 --- a/docker/build-humble.bash +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -docker build --build-arg user_id=$(id -u) --build-arg USER=$(whoami) --build-arg NAME=$(hostname) --rm -t glider-ros:humble -f Dockerfile.humble . diff --git a/docker/build-jazzy.bash b/docker/build-jazzy.bash deleted file mode 100755 index 7f8de97..0000000 --- a/docker/build-jazzy.bash +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -docker build --build-arg user_id=$(id -u) --build-arg USER=$(whoami) --build-arg NAME=$(hostname) --rm -t glider-ros:jazzy -f Dockerfile.jazzy . diff --git a/docker/build.bash b/docker/build.bash new file mode 100755 index 0000000..8f499a7 --- /dev/null +++ b/docker/build.bash @@ -0,0 +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 . diff --git a/docker/run-jazzy.bash b/docker/run-jazzy.bash deleted file mode 100755 index 0fc33bc..0000000 --- a/docker/run-jazzy.bash +++ /dev/null @@ -1,17 +0,0 @@ -#!/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 - diff --git a/docker/run-humble.bash b/docker/run.bash similarity index 100% rename from docker/run-humble.bash rename to docker/run.bash From f7555e99b17da1876ccc8dbf31d98f092e7a6b55 Mon Sep 17 00:00:00 2001 From: jason Date: Mon, 13 Oct 2025 16:48:13 -0400 Subject: [PATCH 03/20] adding ros2 jazzy docker files --- docker/{Dockerfile => Dockerfile.humble} | 0 docker/Dockerfile.jazzy | 49 ++++++++++++++++++++++++ docker/build-humble.bash | 3 ++ docker/build-jazzy.bash | 3 ++ docker/build.bash | 3 -- docker/{run.bash => run-humble.bash} | 0 docker/run-jazzy.bash | 17 ++++++++ 7 files changed, 72 insertions(+), 3 deletions(-) rename docker/{Dockerfile => Dockerfile.humble} (100%) create mode 100644 docker/Dockerfile.jazzy create mode 100755 docker/build-humble.bash create mode 100755 docker/build-jazzy.bash delete mode 100755 docker/build.bash rename docker/{run.bash => run-humble.bash} (100%) create mode 100755 docker/run-jazzy.bash diff --git a/docker/Dockerfile b/docker/Dockerfile.humble similarity index 100% rename from docker/Dockerfile rename to docker/Dockerfile.humble diff --git a/docker/Dockerfile.jazzy b/docker/Dockerfile.jazzy new file mode 100644 index 0000000..14cc602 --- /dev/null +++ b/docker/Dockerfile.jazzy @@ -0,0 +1,49 @@ +FROM osrf/ros:jazzy-desktop-full + +ARG DEBIAN_FRONTEND=noninteractive + +# install the basics +RUN apt-get update \ + && apt-get install -y --no-install-recommends \ + vim \ + tmux \ + cmake \ + gcc \ + g++ \ + git \ + build-essential \ + sudo \ + wget \ + curl \ + zip \ + unzip + +# add a user +ARG user_id +ARG USER noble +RUN userdel ubuntu +RUN useradd -U --uid ${user_id} -ms /bin/bash $USER \ + && echo "$USER:$USER" | chpasswd \ + && adduser $USER sudo \ + && echo "$USER ALL=NOPASSWD: ALL" >> /etc/sudoers.d/$USER + +# add groups to get access to sensors +USER $USER +WORKDIR /home/$USER + +# imu driver +RUN sudo apt-get update \ + && sudo apt-get install -y ros-jazzy-gtsam + +RUN sudo apt-get install -y python3-colcon-common-extensions + +RUN sudo apt install -y libgoogle-glog-dev +# build ros env +RUN mkdir ws + +# make life nice inside docker +RUN sudo chown $USER:$USER ~/.bashrc \ + && /bin/sh -c 'echo ". /opt/ros/jazzy/setup.bash" >> ~/.bashrc' \ + && /bin/sh -c 'echo "source ~/ws/install/setup.bash" >> ~/.bashrc' \ + && echo 'export PS1="\[$(tput setaf 2; tput bold)\]\u\[$(tput setaf 7)\]@\[$(tput setaf 3)\]\h\[$(tput setaf 7)\]:\[$(tput setaf 4)\]\W\[$(tput setaf 7)\]$ \[$(tput sgr0)\]"' >> ~/.bashrc + diff --git a/docker/build-humble.bash b/docker/build-humble.bash new file mode 100755 index 0000000..18f1a39 --- /dev/null +++ b/docker/build-humble.bash @@ -0,0 +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 -f Dockerfile.humble . diff --git a/docker/build-jazzy.bash b/docker/build-jazzy.bash new file mode 100755 index 0000000..7f8de97 --- /dev/null +++ b/docker/build-jazzy.bash @@ -0,0 +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 -f Dockerfile.jazzy . diff --git a/docker/build.bash b/docker/build.bash deleted file mode 100755 index 8f499a7..0000000 --- a/docker/build.bash +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -docker build --build-arg user_id=$(id -u) --build-arg USER=$(whoami) --build-arg NAME=$(hostname) --rm -t glider-ros:humble . diff --git a/docker/run.bash b/docker/run-humble.bash similarity index 100% rename from docker/run.bash rename to docker/run-humble.bash diff --git a/docker/run-jazzy.bash b/docker/run-jazzy.bash new file mode 100755 index 0000000..0fc33bc --- /dev/null +++ b/docker/run-jazzy.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 - From 892498bd6de7951fccb5c41b060d212098d0b55b Mon Sep 17 00:00:00 2001 From: jason Date: Mon, 13 Oct 2025 17:51:05 -0400 Subject: [PATCH 04/20] added workflow and updated readme --- .github/workflows/ci.yml | 38 ++++++++++++++++++++++++++++++++++++++ README.md | 29 ++++++++++++++++------------- 2 files changed, 54 insertions(+), 13 deletions(-) create mode 100644 .github/workflows/ci.yml diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..2a7aba4 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,38 @@ +name: 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_ROS=OFF -DBUILD_TESTS=ON \ + && cd build && cmake --build . && ctest --output-on-failure" + + 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_ROS=OFF -DBUILD_TESTS=ON \ + && cd build && cmake --build . && ctest --output-on-failure" diff --git a/README.md b/README.md index 4845872..71a2945 100644 --- a/README.md +++ b/README.md @@ -1,17 +1,23 @@ -## Glider +# Glider -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`. +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 highly configurable and more features are coming soon. -You can build this as a ros2 package with colcon: `colcon build --packages-select glider`. +## 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` frame +as this is standard for robotics, but we are working on supporting the NED frame. -### Running -Glider can be built as a ros packages in your ros workspace with `catkin build`. -Run glider with: -``` -ros2 launch glider glider-node.launch.py -``` +## 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. + - `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 ### Building and Running Unit Tests We use GTest to run unit tests. You can build the tests with @@ -25,6 +31,3 @@ cd build ctest ``` -#### Authors - - **Corresponding:** Jason Hughes jasonah.at.seas.upenn.edu - - Varun Murali From 326261f335cce55b17df2dbfc8ef7a405be7530f Mon Sep 17 00:00:00 2001 From: jason Date: Mon, 13 Oct 2025 19:55:52 -0400 Subject: [PATCH 05/20] updated readme and license --- LICENSE | 2 +- README.md | 21 ++++++++++++++++++++- 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/LICENSE b/LICENSE index 5b53a1a..e979940 100644 --- a/LICENSE +++ b/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 diff --git a/README.md b/README.md index 71a2945..86b1cce 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ state estimate up to the rate of you IMU. Glider is highly configurable and more ## 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` frame -as this is standard for robotics, but we are working on supporting the NED 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 @@ -18,6 +18,25 @@ the parameters mean: - `subscribers.use_odom`: Still under development ## 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, currently only support ENU but NED support is coming . +### 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 From 891781679a3f339fad405a5d551217f182547949 Mon Sep 17 00:00:00 2001 From: jason Date: Mon, 13 Oct 2025 20:05:08 -0400 Subject: [PATCH 06/20] add workflow to readme --- .github/workflows/{ci.yml => humble-ci.yml} | 14 ------------- .github/workflows/jazzy-ci.yml | 22 +++++++++++++++++++++ README.md | 3 ++- 3 files changed, 24 insertions(+), 15 deletions(-) rename .github/workflows/{ci.yml => humble-ci.yml} (54%) create mode 100644 .github/workflows/jazzy-ci.yml diff --git a/.github/workflows/ci.yml b/.github/workflows/humble-ci.yml similarity index 54% rename from .github/workflows/ci.yml rename to .github/workflows/humble-ci.yml index 2a7aba4..28c6866 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/humble-ci.yml @@ -22,17 +22,3 @@ jobs: bash -c "cd glider && cmake -S . -B build -DBUILD_ROS=OFF -DBUILD_TESTS=ON \ && cd build && cmake --build . && ctest --output-on-failure" - 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_ROS=OFF -DBUILD_TESTS=ON \ - && cd build && cmake --build . && ctest --output-on-failure" diff --git a/.github/workflows/jazzy-ci.yml b/.github/workflows/jazzy-ci.yml new file mode 100644 index 0000000..6a758be --- /dev/null +++ b/.github/workflows/jazzy-ci.yml @@ -0,0 +1,22 @@ +name: CI + +on: + push: + branches: [ ros2 ] + pull_request: + branches: [ ros2 ] + + 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_ROS=OFF -DBUILD_TESTS=ON \ + && cd build && cmake --build . && ctest --output-on-failure" diff --git a/README.md b/README.md index 86b1cce..ad9e8e9 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,6 @@ # Glider - +![Humble CI](https://github.com/KumarRobotics/glider/workflows/humble-ci.yml/badge.svg?branch=ros2) +![Jazzy CI](https://github.com/KumarRobotics/glider/workflows/jazzy-ci.yml/badge.svg?branch=ros2) 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 highly configurable and more features are coming soon. From ddbe77eb8301d4e39813e8985b51a4b0dac63eb4 Mon Sep 17 00:00:00 2001 From: jason Date: Mon, 13 Oct 2025 20:14:20 -0400 Subject: [PATCH 07/20] fix github action bug --- .github/workflows/humble-ci.yml | 2 +- .github/workflows/jazzy-ci.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/humble-ci.yml b/.github/workflows/humble-ci.yml index 28c6866..528368d 100644 --- a/.github/workflows/humble-ci.yml +++ b/.github/workflows/humble-ci.yml @@ -18,7 +18,7 @@ jobs: - name: Run unit test in Docker run: | docker run --rm -v $(pwd):/workspace -w /workspace \ - glider-ros-humble \ + glider-ros:humble \ bash -c "cd glider && cmake -S . -B build -DBUILD_ROS=OFF -DBUILD_TESTS=ON \ && cd build && cmake --build . && ctest --output-on-failure" diff --git a/.github/workflows/jazzy-ci.yml b/.github/workflows/jazzy-ci.yml index 6a758be..51dc88e 100644 --- a/.github/workflows/jazzy-ci.yml +++ b/.github/workflows/jazzy-ci.yml @@ -17,6 +17,6 @@ on: - name: Run unit test in Docker run: | docker run --rm -v $(pwd):/workspace -w /workspace \ - glider-ros-jazzy \ + glider-ros:jazzy \ bash -c "cd glider && cmake -S . -B build -DBUILD_ROS=OFF -DBUILD_TESTS=ON \ && cd build && cmake --build . && ctest --output-on-failure" From f6d484cca1f78f42b85cb24634af220f46eccf54 Mon Sep 17 00:00:00 2001 From: jason Date: Mon, 13 Oct 2025 20:26:47 -0400 Subject: [PATCH 08/20] fix jazzy action --- .github/workflows/humble-ci.yml | 2 +- .github/workflows/jazzy-ci.yml | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/humble-ci.yml b/.github/workflows/humble-ci.yml index 528368d..96f1b9b 100644 --- a/.github/workflows/humble-ci.yml +++ b/.github/workflows/humble-ci.yml @@ -1,4 +1,4 @@ -name: CI +name: Humble CI on: push: diff --git a/.github/workflows/jazzy-ci.yml b/.github/workflows/jazzy-ci.yml index 51dc88e..83f5256 100644 --- a/.github/workflows/jazzy-ci.yml +++ b/.github/workflows/jazzy-ci.yml @@ -1,4 +1,4 @@ -name: CI +name: Jazzy CI on: push: @@ -6,6 +6,7 @@ on: pull_request: branches: [ ros2 ] +jobs: unit-tests-jazzy: runs-on: ubuntu-latest steps: From 0478f66a23b16a49f4828734c9f24c8b0753c615 Mon Sep 17 00:00:00 2001 From: jason Date: Mon, 13 Oct 2025 20:52:25 -0400 Subject: [PATCH 09/20] update readme --- README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index ad9e8e9..19dd9fb 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ # Glider -![Humble CI](https://github.com/KumarRobotics/glider/workflows/humble-ci.yml/badge.svg?branch=ros2) -![Jazzy CI](https://github.com/KumarRobotics/glider/workflows/jazzy-ci.yml/badge.svg?branch=ros2) +![Humble CI](https://github.com/KumarRobotics/glider/actions/workflows/humble-ci.yml/badge.svg?branch=ros2) + +![Jazzy CI](https://github.com/KumarRobotics/glider/actions/workflows/jazzy-ci.yml/badge.svg?branch=ros2) + 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 highly configurable and more features are coming soon. From 771bfbdf652b48aaca6607f1037540e83f4f459c Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 14 Oct 2025 10:40:49 -0400 Subject: [PATCH 10/20] finished unit tests for conversions between eigen and ros --- glider/CMakeLists.txt | 10 +- glider/test/test_ros_conversions.cpp | 203 +++++++++++++++++++++++++++ 2 files changed, 212 insertions(+), 1 deletion(-) create mode 100644 glider/test/test_ros_conversions.cpp diff --git a/glider/CMakeLists.txt b/glider/CMakeLists.txt index 028967b..2e355d2 100644 --- a/glider/CMakeLists.txt +++ b/glider/CMakeLists.txt @@ -199,7 +199,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) @@ -209,4 +216,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() diff --git a/glider/test/test_ros_conversions.cpp b/glider/test/test_ros_conversions.cpp new file mode 100644 index 0000000..22c93a2 --- /dev/null +++ b/glider/test/test_ros_conversions.cpp @@ -0,0 +1,203 @@ +/*! +* Jason Hughes +* October 2025 +* +* Unit tests for ros to eigen and +* eigen to ros convertions +*/ + +#include + +#include "ros/conversions.hpp" + +TEST(RosToEigenTestSuite, Vector3Test) +{ + geometry_msgs::msg::Vector3 msg; + msg.x = 1.0; + msg.y = 1.0; + msg.z = 1.0; + + Eigen::Vector3d gt(msg.x, msg.y, msg.z); + + ASSERT_EQ(GliderROS::Conversions::rosToEigen(msg), gt); +} + +TEST(RosToEigenTestSuite, OrientTest) +{ + geometry_msgs::msg::Quaternion msg; + msg.w = 1.0; + msg.x = 0.0; + msg.y = 0.0; + msg.z = 0.0; + + Eigen::Vector4d gt(msg.w, msg.x, msg.y, msg.z); + + ASSERT_EQ(GliderROS::Conversions::rosToEigen(msg), gt); +} + +TEST(RosToEigenTestSuite, NavSatFixTest) +{ + sensor_msgs::msg::NavSatFix msg; + msg.latitude = 32.925; + msg.longitude = -75.119; + msg.altitude = 10.0; + + Eigen::Vector3d gt(msg.latitude, msg.longitude, msg.altitude); + + ASSERT_EQ(GliderROS::Conversions::rosToEigen(msg), gt); +} + +TEST(RosToEigenTestSuite, PoseTest) +{ + geometry_msgs::msg::PoseStamped msg; + msg.pose.position.x = 1.0; + msg.pose.position.y = 1.0; + msg.pose.position.z = 1.0; + msg.pose.orientation.w = 1.0; + msg.pose.orientation.x = 0.0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + + Eigen::Quaterniond quat(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z); + Eigen::Vector3d trans(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z); + + Eigen::Isometry3d gt = Eigen::Isometry3d::Identity(); + gt.linear() = quat.toRotationMatrix(); + gt.translation() = trans; + + ASSERT_EQ(GliderROS::Conversions::rosToEigen(msg).linear(), gt.linear()); + ASSERT_EQ(GliderROS::Conversions::rosToEigen(msg).translation(), gt.translation()); +} + +TEST(RosToEigenTestSuite, OdometryTest) +{ + nav_msgs::msg::Odometry msg; + msg.pose.pose.position.x = 1.0; + msg.pose.pose.position.y = 1.0; + msg.pose.pose.position.z = 1.0; + msg.pose.pose.orientation.w = 1.0; + msg.pose.pose.orientation.x = 0.0; + msg.pose.pose.orientation.y = 0.0; + msg.pose.pose.orientation.z = 0.0; + msg.twist.twist.linear.x = 1.0; + msg.twist.twist.linear.y = 1.0; + msg.twist.twist.linear.z = 1.0; + + Eigen::Isometry3d odom = GliderROS::Conversions::rosToEigen(msg); + + Eigen::Quaterniond quat(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z); + Eigen::Vector3d trans(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z); + + Eigen::Isometry3d gt = Eigen::Isometry3d::Identity(); + gt.linear() = quat.toRotationMatrix(); + gt.translation() = trans; + + ASSERT_EQ(odom.linear(), gt.linear()); + ASSERT_EQ(odom.translation(), gt.translation()); +} + +TEST(EigenToRosTestSuite, Vector3Test) +{ + Eigen::Vector3d in(1.0, 1.0, 1.0); + + geometry_msgs::msg::Vector3 msg = GliderROS::Conversions::eigenToRos(in); + + ASSERT_EQ(msg.x, in(0)); + ASSERT_EQ(msg.y, in(1)); + ASSERT_EQ(msg.z, in(2)); +} + +TEST(EigenToRosTestSuite, NavSatFixTest) +{ + Eigen::Vector3d in(32.925, -75.119, 10.0); + + sensor_msgs::msg::NavSatFix msg = GliderROS::Conversions::eigenToRos(in); + + ASSERT_EQ(msg.latitude, in(0)); + ASSERT_EQ(msg.longitude, in(1)); + ASSERT_EQ(msg.altitude, in(2)); +} + +TEST(EigenToRosTestSuite, OrientTest) +{ + Eigen::Vector4d in(1.0, 0.0, 0.0, 0.0); + + geometry_msgs::msg::Quaternion msg = GliderROS::Conversions::eigenToRos(in); + + ASSERT_EQ(msg.w, in(0)); + ASSERT_EQ(msg.x, in(1)); + ASSERT_EQ(msg.y, in(2)); + ASSERT_EQ(msg.z, in(3)); +} + +TEST(EigenToRosTestSuite, PoseTest) +{ + Eigen::Isometry3d in = Eigen::Isometry3d::Identity(); + Eigen::Quaterniond quat(1.0, 0.0, 0.0, 0.0); + Eigen::Vector3d t(1.0, 1.0, 1.0); + + in.linear() = quat.toRotationMatrix(); + in.translation() = t; + + geometry_msgs::msg::PoseStamped msg = GliderROS::Conversions::eigenToRos(in); + + ASSERT_EQ(msg.pose.position.x, t(0)); + ASSERT_EQ(msg.pose.position.y, t(1)); + ASSERT_EQ(msg.pose.position.z, t(2)); + + ASSERT_EQ(msg.pose.orientation.w, quat.w()); + ASSERT_EQ(msg.pose.orientation.x, quat.x()); + ASSERT_EQ(msg.pose.orientation.y, quat.y()); + ASSERT_EQ(msg.pose.orientation.z, quat.z()); +} + +TEST(OdometryToRosTestSuite, OdometryTest) +{ + int64_t timestamp = 1; + gtsam::Rot3 rot = gtsam::Rot3::Quaternion(1.0, 0.0, 0.0, 0.0); + gtsam::Point3 t(482981.6098624719, 4421256.568684888, 10.0); + gtsam::Point3 v(0.0, 0.0, 0.0); + + gtsam::NavState ns(rot, t, v); + + Glider::Odometry odom(ns, timestamp, true); + + nav_msgs::msg::Odometry msg = GliderROS::Conversions::odomToRos(odom); + + ASSERT_EQ(msg.pose.pose.position.x, odom.getPosition()(0)); + ASSERT_EQ(msg.pose.pose.position.y, odom.getPosition()(1)); + ASSERT_EQ(msg.pose.pose.position.z, odom.getPosition()(2)); + + ASSERT_EQ(msg.pose.pose.orientation.w, odom.getOrientation()(0)); + ASSERT_EQ(msg.pose.pose.orientation.x, odom.getOrientation()(1)); + ASSERT_EQ(msg.pose.pose.orientation.y, odom.getOrientation()(2)); + ASSERT_EQ(msg.pose.pose.orientation.z, odom.getOrientation()(3)); + + ASSERT_EQ(msg.twist.twist.linear.x, odom.getVelocity()(0)); + ASSERT_EQ(msg.twist.twist.linear.y, odom.getVelocity()(1)); + ASSERT_EQ(msg.twist.twist.linear.z, odom.getVelocity()(2)); + + ASSERT_STREQ(msg.header.frame_id.c_str(), "enu"); + ASSERT_GT(msg.header.stamp.nanosec, 0); +} + +TEST(OdometryToRosTestSuite, NavSatFixTest) +{ + int64_t timestamp = 1; + gtsam::Rot3 rot = gtsam::Rot3::Quaternion(1.0, 0.0, 0.0, 0.0); + gtsam::Point3 t(482981.6098624719, 4421256.568684888, 10.0); + gtsam::Point3 v(0.0, 0.0, 0.0); + + gtsam::NavState ns(rot, t, v); + + Glider::Odometry odom(ns, timestamp, true); + + sensor_msgs::msg::NavSatFix msg = GliderROS::Conversions::odomToRos(odom, "18S"); + + ASSERT_NEAR(msg.latitude, 39.941259, 0.001); + ASSERT_NEAR(msg.longitude, -75.199202, 0.001); + ASSERT_EQ(msg.altitude, 10.0); + ASSERT_STREQ(msg.header.frame_id.c_str(), "enu"); + ASSERT_GT(msg.header.stamp.nanosec, 0); +} + From 8e23e5b486fb71da5ccf7cf6b64102511f287c48 Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 14 Oct 2025 10:41:53 -0400 Subject: [PATCH 11/20] update workflows to include ros tests --- .github/workflows/humble-ci.yml | 2 +- .github/workflows/jazzy-ci.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/humble-ci.yml b/.github/workflows/humble-ci.yml index 96f1b9b..5545d9e 100644 --- a/.github/workflows/humble-ci.yml +++ b/.github/workflows/humble-ci.yml @@ -19,6 +19,6 @@ jobs: run: | docker run --rm -v $(pwd):/workspace -w /workspace \ glider-ros:humble \ - bash -c "cd glider && cmake -S . -B build -DBUILD_ROS=OFF -DBUILD_TESTS=ON \ + bash -c "cd glider && cmake -S . -B build -DBUILD_TESTS=ON \ && cd build && cmake --build . && ctest --output-on-failure" diff --git a/.github/workflows/jazzy-ci.yml b/.github/workflows/jazzy-ci.yml index 83f5256..05572f7 100644 --- a/.github/workflows/jazzy-ci.yml +++ b/.github/workflows/jazzy-ci.yml @@ -19,5 +19,5 @@ jobs: run: | docker run --rm -v $(pwd):/workspace -w /workspace \ glider-ros:jazzy \ - bash -c "cd glider && cmake -S . -B build -DBUILD_ROS=OFF -DBUILD_TESTS=ON \ + bash -c "cd glider && cmake -S . -B build -DBUILD_TESTS=ON \ && cd build && cmake --build . && ctest --output-on-failure" From b111f3737cd5335ad42891c82d73e4257be69533 Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 14 Oct 2025 13:23:09 -0400 Subject: [PATCH 12/20] added support for ned frame imu --- .../{graph-params.yaml => glider-params.yaml} | 1 + glider/include/glider/core/glider.hpp | 8 +++++++ glider/include/glider/utils/parameters.hpp | 3 +++ glider/launch/glider-node.launch.py | 2 +- glider/src/glider.cpp | 22 +++++++++++++++++-- glider/src/parameters.cpp | 1 + glider/test/test_factor_manager.cpp | 10 ++++----- glider/test/test_odometry_w_cov.cpp | 10 ++++----- glider/test/test_params.cpp | 6 ++--- 9 files changed, 47 insertions(+), 16 deletions(-) rename glider/config/{graph-params.yaml => glider-params.yaml} (89%) diff --git a/glider/config/graph-params.yaml b/glider/config/glider-params.yaml similarity index 89% rename from glider/config/graph-params.yaml rename to glider/config/glider-params.yaml index 15cc119..b762568 100644 --- a/glider/config/graph-params.yaml +++ b/glider/config/glider-params.yaml @@ -14,6 +14,7 @@ frame: imu: "enu" logging: stdout: true + directory: "/home/jason/.ros/log/glider" optimizer: smooth: true lag_time: 5.0 diff --git a/glider/include/glider/core/glider.hpp b/glider/include/glider/core/glider.hpp index 999edb5..b6b2ebf 100644 --- a/glider/include/glider/core/glider.hpp +++ b/glider/include/glider/core/glider.hpp @@ -59,6 +59,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_; @@ -69,5 +75,7 @@ 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_; }; } diff --git a/glider/include/glider/utils/parameters.hpp b/glider/include/glider/utils/parameters.hpp index ac5a083..736ab2e 100644 --- a/glider/include/glider/utils/parameters.hpp +++ b/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 diff --git a/glider/launch/glider-node.launch.py b/glider/launch/glider-node.launch.py index 46a9be5..42ba77a 100644 --- a/glider/launch/glider-node.launch.py +++ b/glider/launch/glider-node.launch.py @@ -48,7 +48,7 @@ def generate_launch_description(): graph_params_file = PathJoinSubstitution([ glider_share, 'config', - 'graph-params.yaml' + 'glider-params.yaml' ]) # Create the glider node diff --git a/glider/src/glider.cpp b/glider/src/glider.cpp index f19cdfb..e19cc1d 100644 --- a/glider/src/glider.cpp +++ b/glider/src/glider.cpp @@ -18,6 +18,10 @@ Glider::Glider(const std::string& path) frame_ = params.frame; t_imu_gps_ = params.t_imu_gps; + r_enu_ned_ = Eigen::Matrix3d::Zero(); + r_enu_ned_ << 0.0, 1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, -1.0; LOG(INFO) << "[GLIDER] Using IMU frame: " << frame_; LOG(INFO) << "[GLIDER] Using Fixed Lag Smoother: " << std::boolalpha << params.smooth; @@ -29,7 +33,7 @@ void Glider::initializeLogging(const Parameters& params) const // initialize GLog google::InitGoogleLogging("Glider"); // TODO fix hard coding - FLAGS_log_dir = "/home/jason/.ros/log/glider"; + FLAGS_log_dir = params.log_dir; if (params.log) FLAGS_alsologtostderr = 1; } @@ -57,7 +61,11 @@ void Glider::addImu(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d& if (frame_ == "ned") { //TODO what transforms need to happen here - factor_manager_.addImuFactor(timestamp, accel, gyro, quat); + Eigen::Vector3d accel_enu = r_enu_ned_ * accel; + Eigen::Vector3d gyro_enu = r_enu_ned_ * gyro; + Eigen::Vector4d quat_enu = rotateQuaternion(r_enu_ned_, quat); + + factor_manager_.addImuFactor(timestamp, accel_enu, gyro_enu, quat_enu); } else if (frame_ == "enu") { @@ -96,4 +104,14 @@ OdometryWithCovariance Glider::optimize(int64_t timestamp) return OdometryWithCovariance::Uninitialized(); } } + +Eigen::Vector4d Glider::rotateQuaternion(const Eigen::Matrix3d& rot, const Eigen::Vector4d& quat) const +{ + Eigen::Quaterniond q_ned(quat(0), quat(1), quat(2), quat(3)); + Eigen::Quaterniond q_ned_enu(rot); + + Eigen::Quaterniond q_enu = q_ned_enu * q_ned; + + return Eigen::Vector4d(q_enu.w(), q_enu.x(), q_enu.y(), q_enu.z()); +} } diff --git a/glider/src/parameters.cpp b/glider/src/parameters.cpp index 1b25814..ee998af 100644 --- a/glider/src/parameters.cpp +++ b/glider/src/parameters.cpp @@ -31,6 +31,7 @@ Glider::Parameters::Parameters(const std::string& path) frame = config["frame"]["imu"].as(); log = config["logging"]["stdout"].as(); + log_dir = config["logging"]["directory"].as(); smooth = config["optimizer"]["smooth"].as(); lag_time = config["optimizer"]["lag_time"].as(); diff --git a/glider/test/test_factor_manager.cpp b/glider/test/test_factor_manager.cpp index 07b87a5..32f64c9 100644 --- a/glider/test/test_factor_manager.cpp +++ b/glider/test/test_factor_manager.cpp @@ -10,7 +10,7 @@ TEST(FactorManagerTestSuite, ImuInitialization) { // initialized glider factor manager and params - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); // provide measurements for initialization @@ -43,7 +43,7 @@ TEST(FactorManagerTestSuite, ImuInitialization) TEST(FactorManagerTestSuite, PimParameters) { // initialized glider factor manager and params - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); // provide measurements for initialization @@ -87,7 +87,7 @@ TEST(FactorManagerTestSuite, KeyIndex) double lon = -75.199197; // initialized glider factor manager and params - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); ASSERT_EQ(manager.getKeyIndex(), 0); @@ -115,7 +115,7 @@ TEST(FactorManagerTestSuite, GPSInitialization) double lon = -75.199197; // initialized glider factor manager and params - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); // provide imu measurements for initialization @@ -143,7 +143,7 @@ TEST(FactorManagerTestSuite, SystemInitialization) double lon = -75.199197; // initialized glider factor manager and params - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); // assert system is NOT initialized diff --git a/glider/test/test_odometry_w_cov.cpp b/glider/test/test_odometry_w_cov.cpp index 473f8fc..ec6f9c0 100644 --- a/glider/test/test_odometry_w_cov.cpp +++ b/glider/test/test_odometry_w_cov.cpp @@ -40,7 +40,7 @@ static const double GZ = 0.0; TEST(OdometryWithCovarianceTestSuite, TestInitialization) { // initialized glider factor manager and params - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); Glider::OdometryWithCovariance odom; @@ -71,7 +71,7 @@ TEST(OdometryWithCovarianceTestSuite, TestInitialization) TEST(OdometryWithCovarainceTestSuite, TestCovariances) { // initialized glider factor manager and params - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); Glider::OdometryWithCovariance odom; @@ -125,7 +125,7 @@ TEST(OdometryWithCovarainceTestSuite, TestCovariances) TEST(OdometryWithCovarianceTestSuite, TestBiases) { // initialized glider factor manager and params - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); Glider::OdometryWithCovariance odom; @@ -193,7 +193,7 @@ TEST(OdometryWithCovarianceTestSuite, TestBiases) TEST(OdometryWithCovarianceTestSuite, TestKeyIndex) { // initialized glider factor manager and params - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); Glider::OdometryWithCovariance odom; @@ -230,7 +230,7 @@ TEST(OdometryWithCovarianceTestSuite, TestKeyIndex) TEST(OdometryWithCovarianceTestSuite, TestMovementStatus) { // initialized glider factor manager and params - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); Glider::OdometryWithCovariance odom; diff --git a/glider/test/test_params.cpp b/glider/test/test_params.cpp index f2b9950..7ab2674 100644 --- a/glider/test/test_params.cpp +++ b/glider/test/test_params.cpp @@ -8,7 +8,7 @@ TEST(ParamsTestSuite, Constants) { - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); // gravity can be positive or negative depending on // imu frame @@ -22,7 +22,7 @@ TEST(ParamsTestSuite, Constants) TEST(ParamsTestSuite, Covariances) { - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); // covariances should be positive ASSERT_GE(params.accel_cov, 0.0); @@ -36,7 +36,7 @@ TEST(ParamsTestSuite, Covariances) TEST(ParamsTestSuite, Frame) { - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); EXPECT_TRUE(params.frame == "ned" || params.frame == "enu"); } From 91957a34d4e12d8b3e726e3109d8b83623c66c63 Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 14 Oct 2025 13:41:37 -0400 Subject: [PATCH 13/20] update readme to support ned frame --- README.md | 9 +++++---- glider/test/test_glider.cpp | 2 +- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 19dd9fb..9011ff5 100644 --- a/README.md +++ b/README.md @@ -4,10 +4,10 @@ ![Jazzy CI](https://github.com/KumarRobotics/glider/actions/workflows/jazzy-ci.yml/badge.svg?branch=ros2) 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 highly configurable and more features are coming soon. +state estimate up to the rate of you IMU. Glider is designed to be configured to your system. ## 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` frame +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 @@ -29,7 +29,7 @@ You can configure glider itself in `config/glider-params.yaml`, this is where yo - `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, currently only support ENU but NED support is coming . + - `frame`: What frame the IMU is in, either `enu` or `ned`. ### GPS Parameters - `gps.covariance`: covariance of the gps position estimate. ### Other Parameters @@ -44,7 +44,8 @@ You can configure glider itself in `config/glider-params.yaml`, this is where yo ### 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: diff --git a/glider/test/test_glider.cpp b/glider/test/test_glider.cpp index 24e0bd9..28f3b42 100644 --- a/glider/test/test_glider.cpp +++ b/glider/test/test_glider.cpp @@ -11,7 +11,7 @@ TEST(GliderTestSuite, AddTo) { - Glider::Glider glider("../config/graph-params.yaml"); + Glider::Glider glider("../config/glider-params.yaml"); Eigen::Vector3d accel(0.0, 0.0, 9.81); Eigen::Vector3d gyro(0.0, 0.0, 0.0); From 05478bd80a14291be64309833652202a0aef35c7 Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 14 Oct 2025 13:44:14 -0400 Subject: [PATCH 14/20] fix readme formatting --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 9011ff5..4a515a5 100644 --- a/README.md +++ b/README.md @@ -30,9 +30,9 @@ You can configure glider itself in `config/glider-params.yaml`, this is where yo - `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 Parameters - `gps.covariance`: covariance of the gps position estimate. -### Other Parameters +#### 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. @@ -53,4 +53,5 @@ and run with: cd build ctest ``` +Note these tests are run on PR's and pushes to the primary branch. From 59dc38a45abd313f17e15124566e89ebd41f99c2 Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 14 Oct 2025 18:07:43 -0400 Subject: [PATCH 15/20] squashed mapping bugs --- README.md | 16 +++++++++++++++- glider/config/glider-params.yaml | 2 +- glider/include/ros/glider_node.hpp | 6 ------ glider/launch/glider-node.launch.py | 26 ++++++++++++-------------- glider/ros/glider_node.cpp | 5 ----- glider/src/glider.cpp | 3 ++- 6 files changed, 30 insertions(+), 28 deletions(-) diff --git a/README.md b/README.md index 4a515a5..abbc220 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,20 @@ 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. +## Building Glider +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 --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. @@ -18,7 +32,7 @@ the parameters mean: - `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 + - `subscribers.use_odom`: Still under development. ## 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: diff --git a/glider/config/glider-params.yaml b/glider/config/glider-params.yaml index b762568..c43b931 100644 --- a/glider/config/glider-params.yaml +++ b/glider/config/glider-params.yaml @@ -14,7 +14,7 @@ frame: imu: "enu" logging: stdout: true - directory: "/home/jason/.ros/log/glider" + directory: "/tmp/glider" optimizer: smooth: true lag_time: 5.0 diff --git a/glider/include/ros/glider_node.hpp b/glider/include/ros/glider_node.hpp index b15fcac..5feda06 100644 --- a/glider/include/ros/glider_node.hpp +++ b/glider/include/ros/glider_node.hpp @@ -71,19 +71,13 @@ 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_; // tracker Glider::OdometryWithCovariance current_state_; - - int64_t latest_imu_timestamp_; }; } diff --git a/glider/launch/glider-node.launch.py b/glider/launch/glider-node.launch.py index 42ba77a..e03de5e 100644 --- a/glider/launch/glider-node.launch.py +++ b/glider/launch/glider-node.launch.py @@ -7,17 +7,16 @@ """ import os +import yaml from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - # create logging directory - os.makedirs("/home/jason/.ros/log/glider", exist_ok=True) - # Declare launch arguments use_sim_time_arg = DeclareLaunchArgument( 'use_sim_time', @@ -25,19 +24,13 @@ def generate_launch_description(): description='Use simulation time' ) - use_odom_arg = DeclareLaunchArgument( - 'use_odom', - default_value='false', - description='Use odometry' - ) - # Get launch configurations use_sim_time = LaunchConfiguration('use_sim_time') - use_odom = LaunchConfiguration('use_odom') # Find package share directory glider_share = FindPackageShare('glider') - + glider_share_dir = get_package_share_directory('glider') + # Path to parameter files ros_params_file = PathJoinSubstitution([ glider_share, @@ -50,7 +43,12 @@ def generate_launch_description(): 'config', 'glider-params.yaml' ]) - + + # create logging directory + with open(os.path.join(glider_share_dir, "config", "glider-params.yaml")) as f: + config = yaml.safe_load(f) + os.makedirs(config["logging"]["directory"], exist_ok=True) + # Create the glider node glider_node = Node( package='glider', @@ -61,7 +59,7 @@ def generate_launch_description(): ros_params_file, {'path': graph_params_file, 'use_sim_time': use_sim_time, - 'use_odom': use_odom} + 'use_odom': False} ], remappings=[ ('/gps', '/ublox/fix'), @@ -70,4 +68,4 @@ def generate_launch_description(): ] ) - return LaunchDescription([use_sim_time_arg, use_odom_arg, glider_node]) + return LaunchDescription([use_sim_time_arg, glider_node]) diff --git a/glider/ros/glider_node.cpp b/glider/ros/glider_node.cpp index 7c79497..59f13ce 100644 --- a/glider/ros/glider_node.cpp +++ b/glider/ros/glider_node.cpp @@ -31,9 +31,6 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide bool use_odom = this->get_parameter("subscribers.use_odom").as_bool(); std::string path = this->get_parameter("path").as_string(); - use_sim_time_ = this->get_clock()->get_clock_type() == RCL_ROS_TIME; - - latest_imu_timestamp_ = 0; glider_ = std::make_unique(path); current_state_ = Glider::OdometryWithCovariance::Uninitialized(); @@ -109,8 +106,6 @@ void GliderNode::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int64_t timestamp = getTime(msg->header.stamp); glider_->addImu(timestamp, accel, gyro, orient); - - latest_imu_timestamp_ = timestamp; } void GliderNode::gpsCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) diff --git a/glider/src/glider.cpp b/glider/src/glider.cpp index e19cc1d..bceb720 100644 --- a/glider/src/glider.cpp +++ b/glider/src/glider.cpp @@ -25,6 +25,7 @@ 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] Logging to: " << params.log_dir; LOG(INFO) << "[GLIDER] Glider initialized"; } @@ -32,7 +33,7 @@ void Glider::initializeLogging(const Parameters& params) const { // initialize GLog google::InitGoogleLogging("Glider"); - // TODO fix hard coding + FLAGS_log_dir = params.log_dir; if (params.log) FLAGS_alsologtostderr = 1; } From a4b5072e5f29dcbabd28356db676439c454b29b5 Mon Sep 17 00:00:00 2001 From: jason Date: Tue, 14 Oct 2025 21:11:33 -0400 Subject: [PATCH 16/20] made glider be able to publish at IMU rate --- README.md | 2 +- glider/config/ros-params.yaml | 2 +- glider/include/ros/glider_node.hpp | 1 + glider/ros/glider_node.cpp | 20 ++++++++++++++------ 4 files changed, 17 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index abbc220..9e99866 100644 --- a/README.md +++ b/README.md @@ -27,7 +27,7 @@ as this is standard for robotics, but we are working on supporting the NED frame ## 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. + - `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. diff --git a/glider/config/ros-params.yaml b/glider/config/ros-params.yaml index d223bd8..09dfc6b 100644 --- a/glider/config/ros-params.yaml +++ b/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 diff --git a/glider/include/ros/glider_node.hpp b/glider/include/ros/glider_node.hpp index 5feda06..8a410af 100644 --- a/glider/include/ros/glider_node.hpp +++ b/glider/include/ros/glider_node.hpp @@ -76,6 +76,7 @@ class GliderNode : public rclcpp::Node std::string utm_zone_; double origin_easting_; double origin_northing_; + double freq_; // tracker Glider::OdometryWithCovariance current_state_; diff --git a/glider/ros/glider_node.cpp b/glider/ros/glider_node.cpp index 59f13ce..9d2e089 100644 --- a/glider/ros/glider_node.cpp +++ b/glider/ros/glider_node.cpp @@ -21,7 +21,7 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide declare_parameter("path", ""); // Get parameters - double freq = this->get_parameter("publishers.rate").as_double(); + freq_ = this->get_parameter("publishers.rate").as_double(); publish_nsf_ = this->get_parameter("publishers.nav_sat_fix").as_bool(); viz_ = this->get_parameter("publishers.viz.use").as_bool(); @@ -60,13 +60,13 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide if (publish_nsf_) { LOG(INFO) << "[GLIDER] Publishing NavSatFix msg on /glider/fix"; - LOG(INFO) << "[GLIDER] Using prediction rate: " << freq; + LOG(INFO) << "[GLIDER] Using prediction rate: " << freq_; gps_pub_ = this->create_publisher("/glider/fix", 10); } else { LOG(INFO) << "[GLIDER] Publishing Odometry msg on /glider/odom"; - LOG(INFO) << "[GLIDER] Using prediction rate: " << freq; + LOG(INFO) << "[GLIDER] Using prediction rate: " << freq_; odom_pub_ = this->create_publisher("/glider/odom", 10); } @@ -76,9 +76,11 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide odom_viz_pub_ = this->create_publisher("/glider/odom/viz", 10); } - // TODO add in predictor - std::chrono::milliseconds d = GliderROS::Conversions::hzToDuration(freq); - timer_ = this->create_wall_timer(d, std::bind(&GliderNode::interpolationCallback, this)); + if (freq_ > 0) + { + std::chrono::milliseconds d = GliderROS::Conversions::hzToDuration(freq_); + timer_ = this->create_wall_timer(d, std::bind(&GliderNode::interpolationCallback, this)); + } LOG(INFO) << "[GLIDER] GliderNode Initialized"; } @@ -106,6 +108,12 @@ void GliderNode::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int64_t timestamp = getTime(msg->header.stamp); glider_->addImu(timestamp, accel, gyro, orient); + + if (freq_ == 0 && current_state_.isInitialized()) + { + Glider::Odometry odom = glider_->interpolate(timestamp); + (publish_nsf_) ? publishNavSatFix(odom) : publishOdometry(odom); + } } void GliderNode::gpsCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) From babb5b2b6332ccadbbde72bfee1c029cead05a73 Mon Sep 17 00:00:00 2001 From: jason Date: Wed, 15 Oct 2025 20:10:08 -0400 Subject: [PATCH 17/20] update readme --- README.md | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 9e99866..054bc48 100644 --- a/README.md +++ b/README.md @@ -7,7 +7,15 @@ Glider is a G-INS system built on [GTSAM](https://github.com/borglab/gtsam). It state estimate up to the rate of you IMU. Glider is designed to be configured to your system. ## Building Glider -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: +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: +``` +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 @@ -67,5 +75,5 @@ and run with: cd build ctest ``` -Note these tests are run on PR's and pushes to the primary branch. +Note these tests are run on PR's and pushes to the `ros2` branch. From 783c0154645d2ab12502eb931a4f586981bd9d49 Mon Sep 17 00:00:00 2001 From: jason Date: Thu, 16 Oct 2025 17:38:57 -0400 Subject: [PATCH 18/20] make gravity vector non-configurable --- glider/src/factor_manager.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/glider/src/factor_manager.cpp b/glider/src/factor_manager.cpp index 1647ec7..bd0c901 100644 --- a/glider/src/factor_manager.cpp +++ b/glider/src/factor_manager.cpp @@ -52,14 +52,7 @@ FactorManager::FactorManager(const Parameters& params) boost::shared_ptr FactorManager::defaultImuParams(double g) { boost::shared_ptr params; - if (params_.frame == "enu") - { - params = gtsam::PreintegrationCombinedParams::MakeSharedU(g); - } - else - { - params = gtsam::PreintegrationCombinedParams::MakeSharedD(g); - } + params = gtsam::PreintegrationCombinedParams::MakeSharedU(g); double gyro_sigma = (0.5 * M_PI / 180.0) / 60.0; double accel_sigma = 0.001; From 9ba759d92c7a84b56b7012360c5ee574086af461 Mon Sep 17 00:00:00 2001 From: jason Date: Thu, 16 Oct 2025 21:01:27 -0400 Subject: [PATCH 19/20] dgps module is written --- glider/CMakeLists.txt | 1 + .../include/glider/core/differential_gps.hpp | 43 +++++++++++ glider/include/glider/core/glider.hpp | 10 +-- glider/src/differential_gps.cpp | 71 +++++++++++++++++++ glider/src/glider.cpp | 53 +++++++++----- 5 files changed, 155 insertions(+), 23 deletions(-) create mode 100644 glider/include/glider/core/differential_gps.hpp create mode 100644 glider/src/differential_gps.cpp diff --git a/glider/CMakeLists.txt b/glider/CMakeLists.txt index a9ddefb..b369720 100644 --- a/glider/CMakeLists.txt +++ b/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 diff --git a/glider/include/glider/core/differential_gps.hpp b/glider/include/glider/core/differential_gps.hpp new file mode 100644 index 0000000..3b9d76d --- /dev/null +++ b/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 +#include +#include + +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 diff --git a/glider/include/glider/core/glider.hpp b/glider/include/glider/core/glider.hpp index 113d2b8..b6c4ac0 100644 --- a/glider/include/glider/core/glider.hpp +++ b/glider/include/glider/core/glider.hpp @@ -13,7 +13,7 @@ #include #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 @@ -81,12 +82,11 @@ class Glider // @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 del_threshold_; }; } diff --git a/glider/src/differential_gps.cpp b/glider/src/differential_gps.cpp new file mode 100644 index 0000000..ce2c306 --- /dev/null +++ b/glider/src/differential_gps.cpp @@ -0,0 +1,71 @@ +/*! +* Jason Hughes +* October 2025 +* +*/ + +#include "glider/core/differential_gps.hpp" + +using namespace Glider::Geodetics; + +DifferentialGpsFromMotion::DifferentialGpsFromMotion(const std::string& frame, const double& threshold) +{ + frame_ = frame; + vel_threshold_ = threshold; +} + +double DifferentialGpsFromMotion::getHeading(const Eigen::Vector3d& gps) +{ + if (!initialized_) + { + last_gps_ = gps; + initialized_ = true; + } + + double lat1_rad = last_gps_(0) * M_PI / 180.0; + double lon1_rad = last_gps_(1) * M_PI / 180.0; + double lat2_rad = gps(0) * M_PI / 180.0; + double lon2_rad = gps(1) * M_PI / 180.0; + + double lon_diff = lon2_rad - lon1_rad; + + double y = std::sin(lon_diff) * std::cos(lat2_rad); + double x = std::cos(lat1_rad) * std::sin(lat2_rad) - std::sin(lat1_rad) * std::cos(lat2_rad) * std::cos(lon_diff); + + double heading_rad = std::atan2(y,x); + last_gps_ = gps; + + if (frame_ == "enu") heading_rad = nedToEnu(heading_rad); + + return heading_rad; +} + +bool DifferentialGpsFromMotion::isInitialized() const +{ + return initialized_; +} + +double DifferentialGpsFromMotion::getVelocityThreshold() const +{ + return vel_threshold_; +} + +void DifferentialGpsFromMotion::setLastGps(const Eigen::Vector3d& gps) +{ + last_gps_ = gps; + initialized_ = true; +} + +double DifferentialGpsFromMotion::nedToEnu(const double heading) const +{ + double enu_heading = std::fmod((M_PI/2 - heading + (2*M_PI)), (2*M_PI)); + return enu_heading; +} + +double DifferentialGpsFromMotion::headingRadiansToDegrees(const double heading) const +{ + double heading_deg = heading * (180.0 / M_PI); + + heading_deg = std::fmod(std::fmod(heading_deg, 360.0) + 360.0, 360.0); + return heading_deg; +} diff --git a/glider/src/glider.cpp b/glider/src/glider.cpp index f01c744..9b585f5 100644 --- a/glider/src/glider.cpp +++ b/glider/src/glider.cpp @@ -15,7 +15,7 @@ Glider::Glider(const std::string& path) Parameters params = Parameters::Load(path); initializeLogging(params); factor_manager_ = FactorManager(params); - + frame_ = params.frame; t_imu_gps_ = params.t_imu_gps; r_enu_ned_ = Eigen::Matrix3d::Zero(); @@ -27,7 +27,7 @@ Glider::Glider(const std::string& path) LOG(INFO) << "[GLIDER] Using Fixed Lag Smoother: " << std::boolalpha << params.smooth; LOG(INFO) << "[GLIDER] Logging to: " << params.log_dir; use_dgpsfm_ = params.use_dgpsfm; - vel_threshold_ = params.dgpsfm_threshold; + dgps_ = Geodetics::DifferentialGpsFromMotion(params.frame, params.dgpsfm_threshold); current_odom_ = OdometryWithCovariance::Uninitialized(); @@ -46,7 +46,34 @@ void Glider::initializeLogging(const Parameters& params) const if (params.log) FLAGS_alsologtostderr = 1; } + void Glider::addGps(int64_t timestamp, Eigen::Vector3d& gps) +{ + // route the + if (use_dgpsfm_) + { + addGpsWithHeading(timestamp, gps); + return; + } + + // 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); + + // TODO t_imu_gps_ needs to be rotated!! + meas = meas + t_imu_gps_; + + factor_manager_.addGpsFactor(timestamp, meas); +} + +void Glider::addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps) { // transform from lat lon To UTM Eigen::Vector3d meas = Eigen::Vector3d::Zero(); @@ -62,25 +89,17 @@ void Glider::addGps(int64_t timestamp, Eigen::Vector3d& gps) // TODO t_imu_gps_ needs to be rotated!! meas = meas + t_imu_gps_; - if (use_dgpsfm_ && current_odom_.isInitialized()) + if(factor_manager_.isSystemInitialized() && current_odom_.isMovingFasterThan(dgps_.getVelocityThreshold())) { - if(factor_manager_.isSystemInitialized() && current_odom_.isMovingFasterThan(vel_threshold_)) - { - LOG(INFO) << "[GLIDER] Adding DGPS heading"; - double heading_ne = geodetics::gpsHeading(last_gps_(0), last_gps_(1), gps(0), gps(1)); - double heading_en = geodetics::geodeticToENU(heading_ne); - factor_manager_.addGpsFactor(timestamp, meas, heading_en, true); - } - else - { - factor_manager_.addGpsFactor(timestamp, meas, 0.0, false); - } + LOG(INFO) << "[GLIDER] Adding DGPS heading"; + double heading = dgps_.getHeading(gps); + factor_manager_.addGpsFactor(timestamp, meas, heading, true); } else { - factor_manager_.addGpsFactor(timestamp, meas); + dgps_.setLastGps(gps); + factor_manager_.addGpsFactor(timestamp, meas, 0.0, false); } - last_gps_ = gps; } void Glider::addImu(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d& gyro, Eigen::Vector4d& quat) @@ -92,8 +111,6 @@ void Glider::addImu(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d& Eigen::Vector4d quat_enu = rotateQuaternion(r_enu_ned_, quat); factor_manager_.addImuFactor(timestamp, accel_enu, gyro_enu, quat_enu); - //factor_manager_.addImuFactor(timestamp, accel, gyro, quat);' - LOG(FATAL) << "[GLIDER] NED frame not supported yet"; } else if (frame_ == "enu") { From fb2e50fe27213304c0771b4ecd52c2041b3982d1 Mon Sep 17 00:00:00 2001 From: jason Date: Thu, 16 Oct 2025 21:08:07 -0400 Subject: [PATCH 20/20] update launch file topics --- glider/launch/glider-node.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/glider/launch/glider-node.launch.py b/glider/launch/glider-node.launch.py index d951db3..e03de5e 100644 --- a/glider/launch/glider-node.launch.py +++ b/glider/launch/glider-node.launch.py @@ -62,8 +62,8 @@ def generate_launch_description(): 'use_odom': False} ], remappings=[ - ('/gps', '/ublox_gps_node/fix'), - ('/imu', '/vectornav/imu'), + ('/gps', '/ublox/fix'), + ('/imu', '/VN100T/imu'), ('/odom', '/Odometry'), ] )