Added LIO and local goalpose support

Dockerfile

@@ -0,0 +1,40 @@
+# Start from ROS Jazzy + CUDA base
+FROM dtcpronto/ros-jazzy:cuda
+
+USER root
+
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    vim \
+    tmux \
+    cmake \
+    gcc \
+    g++ \
+    git \
+    build-essential \
+    sudo \
+    wget \
+    curl \
+    zip \
+    unzip \
+    ros-jazzy-gtsam \
+    ros-jazzy-gps-msgs \
+    python3-colcon-common-extensions \
+    libgoogle-glog-dev \
+    && rm -rf /var/lib/apt/lists/*
+
+# Switch to dtc user
+USER dtc
+WORKDIR /home/dtc/ws
+
+# Clone glider
+COPY --chown=dtc:dtc ./glider /home/dtc/ws/src/glider
+COPY --chown=dtc:dtc ./dgps_msgs /home/dtc/ws/src/dgps_msgs
+
+# Build workspace
+RUN /bin/bash -c "source /opt/ros/jazzy/setup.bash && \
+    colcon build --symlink-install"
+
+COPY --chown=dtc:dtc ./entrypoint.bash /home/dtc/entrypoint.bash
+RUN chmod +x /home/dtc/entrypoint.bash
+
+ENTRYPOINT ["/home/dtc/entrypoint.bash"]

README.md

@@ -3,8 +3,8 @@
 
 ![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 designed to be configured to your system. 
+Glider is a G-INS system built on [GTSAM](https://github.com/borglab/gtsam). It accepts GPS, a 9-DOF IMU, and optional local odometry, and provides a full
+state estimate at up to the IMU rate. Glider is designed to be configured for a specific sensor suite.
 
 ## 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:
@@ -18,7 +18,7 @@ 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 
+cmake --build build
 ```
 
 ## Running Glider
@@ -40,7 +40,13 @@ 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.imu_topic`, `gps_topic`, `dgps_topic`, `odom_topic`: input topic names.
+ - `subscribers.use_gps`, `use_dgps`, `use_odom`: enable each aiding source. Avoid enabling both GPS inputs when they represent the same receiver fix.
+ - `subscribers.gps_rejection_variance`: hard GPS variance ceiling in m². Measurements above it are rejected while local odometry remains active.
+
+The checked-in ROS parameters use a VectorNav IMU on `/vectornav/imu`, LIO on
+`/rko_lio/odometry`, and the ENU DGPS fix on `/sept/enu/dfix`. LIO remains active
+while GPS is absent or rejected.
 
 ## 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:
@@ -61,19 +67,18 @@ You can configure glider itself in `config/glider-params.yaml`, this is where yo
  - `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.
+ - `extrinsics`: the single hardware-calibration section. Sensor poses are entered relative to the LiDAR using ROS FLU axes (+X forward, +Y left, +Z up), metres, and XYZ roll/pitch/yaw degrees. Glider converts them to its body frame internally.
 
 ### Building and Running Unit Tests
-We use GTest to run unit tests. You can build the tests with 
-``` 
+We use GTest to run unit tests. You can build the tests with
+```
 cd glider
 cmake -S . -B build -DBUILD_TESTS=ON
 cmake --build build
 ```
 and run with:
 ```
-cd build 
+cd build
 ctest
 ```
 Note these tests are run on PR's and pushes to the `ros2` branch.
-

build.bash

@@ -0,0 +1,2 @@
+#!/bin/bash
+docker build --build-arg user_id=$(id -u) --build-arg USER=$(whoami) --build-arg NAME=glider --rm -t dtc-jackal-`hostname`:glider .

dgps_msgs/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.8)
+project(dgps_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/DifferentialNavSatFix.msg"
+  DEPENDENCIES sensor_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

dgps_msgs/msg/DifferentialNavSatFix.msg

@@ -0,0 +1,11 @@
+# Center/primary antenna navigation fix.
+sensor_msgs/NavSatFix nmea
+
+# Frame-corrected ENU yaw, normalized to [0, 2*pi), in radians.
+float32 heading
+
+# The same heading expressed in degrees, normalized to [0, 360).
+float32 heading_deg
+
+# Heading variance in rad^2.
+float32 heading_covariance

dgps_msgs/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>dgps_msgs</name>
+  <version>0.1.0</version>
+  <description>Differential GPS fix and dual-antenna heading message</description>
+  <maintainer email="shenbax@seas.upenn.edu">Shenbax</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <depend>sensor_msgs</depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

entrypoint.bash

@@ -0,0 +1,13 @@
+#!/bin/bash
+
+source /opt/ros/jazzy/setup.bash
+source /home/dtc/ws/install/setup.bash
+
+if [ "$RUN" = "true" ]; then
+    echo "[GLIDER] Launching glider..."
+    ros2 launch glider glider-node.launch.py use_sim_time:="${USE_SIM_TIME:-false}"
+else
+    echo "[GLIDER] RUN=false, keeping container alive..."
+fi
+
+exec "$@"

glider/CMakeLists.txt

@@ -44,6 +44,9 @@ if (BUILD_ROS)
   find_package(std_msgs REQUIRED)
   find_package(nav_msgs REQUIRED)
   find_package(gps_msgs REQUIRED)
+  find_package(dgps_msgs REQUIRED)
+  find_package(geometry_msgs REQUIRED)
+  find_package(tf2_ros REQUIRED)
 
   set(node_plugins "")
 endif()
@@ -97,6 +100,8 @@ if (BUILD_ROS)
     geometry_msgs
     nav_msgs
     gps_msgs
+    dgps_msgs
+    tf2_ros
   )
 
   add_executable(${PROJECT_NAME}_node
@@ -146,6 +151,7 @@ if (BUILD_ROS)
     sensor_msgs
     geometry_msgs
     nav_msgs
+    dgps_msgs
     Eigen3
   )
 

glider/config/glider-params.yaml

@@ -3,12 +3,14 @@ imu:
     accelerometer: 0.00001
     gyroscope: 0.00001
     integration: 0.001
-    heading: 0.09 
+    heading: 0.09
     roll_pitch: 0.001
     bias: 0.001
   frame: "enu"
-gps: 
+gps:
   covariance: 2.0
+odom:
+  covariance: 0.1
 dgps:
   enable: true
   covariance: 0.03
@@ -24,9 +26,30 @@ logging:
   stdout: true
   directory: "/tmp/glider"
 optimizer:
-  smooth: true
+  # Set true to use the incremental fixed-lag smoother instead of iSAM2.
+  smooth: false
   lag_time: 5.0
-gps_to_imu:
-  x: 0.0
-  y: 0.0
-  z: 0.0
+# All sensor locations are expressed relative to the LiDAR using ROS FLU axes:
+# +X forward, +Y left, +Z up. Thus right/down/behind are negative Y/Z/X.
+# Translations are metres; rotations are XYZ roll/pitch/yaw in degrees.
+extrinsics:
+  reference_frame: "os_sensor"
+  body:
+    frame: "rko_base_link"
+    translation: {x: -0.2, y: 0.0, z: -0.338}
+    rotation_rpy_deg: {roll: 0.0, pitch: 0.0, yaw: 0.0}
+  lidar:
+    frame: "os_sensor"
+    translation: {x: 0.0, y: 0.0, z: 0.0}
+    rotation_rpy_deg: {roll: 0.0, pitch: 0.0, yaw: 0.0}
+  imu:
+    frame: "vectornav"
+    # 12 cm right and 17 cm down from the LiDAR.
+    translation: {x: 0.0, y: -0.12, z: -0.17}
+    rotation_rpy_deg: {roll: 90.6107, pitch: 3.1974, yaw: -148.5656}
+  gps:
+    frame: "navsat_link"
+    # 25 cm directly behind the LiDAR along the X axis.
+    translation: {x: -0.25, y: 0.0, z: 0.0}
+    rotation_rpy_deg: {roll: 0.0, pitch: 0.0, yaw: 0.0}
+    heading_offset_deg: 0.0

glider/config/ros-params.yaml

@@ -2,10 +2,23 @@ glider_node:
   ros__parameters:
     publishers:
       rate: 0.0
-      nav_sat_fix: false
+      nav_sat_fix: true
+      utm_zone: "18S"
+      map_frame: "map"
+      base_link_frame: "base_link"
       viz:
         use: true
         origin_easting: 753912.0063845584
         origin_northing: 3385461.6073698294
     subscribers:
-      use_odom: false
+      use_odom: true
+      use_gps: false
+      use_dgps: true
+      imu_topic: "/vectornav/imu"
+      odom_topic: "/rko_lio/odometry"
+      gps_topic: "/ublox/fix"
+      dgps_topic: "/sept/enu/dfix"
+      # Reject fixes when any reported position variance exceeds this value.
+      gps_rejection_variance: 1.0
+      max_stamp_skew_sec: 1.0
+      gps_loss_timeout_sec: 3.0