diff --git a/.github/workflows/humble-ci.yml b/.github/workflows/humble-ci.yml new file mode 100644 index 0000000..5545d9e --- /dev/null +++ b/.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" + diff --git a/.github/workflows/jazzy-ci.yml b/.github/workflows/jazzy-ci.yml new file mode 100644 index 0000000..05572f7 --- /dev/null +++ b/.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" 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 4845872..054bc48 100644 --- a/README.md +++ b/README.md @@ -1,22 +1,73 @@ -## 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 --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: @@ -24,7 +75,5 @@ 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 diff --git a/docker/Dockerfile.humble b/docker/Dockerfile.humble index e125157..f650711 100644 --- a/docker/Dockerfile.humble +++ b/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 diff --git a/docker/Dockerfile b/docker/Dockerfile.jazzy similarity index 91% rename from docker/Dockerfile rename to docker/Dockerfile.jazzy index b819384..14cc602 100644 --- a/docker/Dockerfile +++ b/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 diff --git a/docker/build-humble.bash b/docker/build-humble.bash index 8f499a7..18f1a39 100755 --- a/docker/build-humble.bash +++ b/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 . diff --git a/docker/build-jazzy.bash b/docker/build-jazzy.bash index 9a5f397..7f8de97 100755 --- a/docker/build-jazzy.bash +++ b/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 . diff --git a/docker/run-humble.bash b/docker/run-humble.bash new file mode 100755 index 0000000..0fc33bc --- /dev/null +++ b/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 - diff --git a/glider/CMakeLists.txt b/glider/CMakeLists.txt index 3bcabc5..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 @@ -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() diff --git a/glider/config/graph-params.yaml b/glider/config/glider-params.yaml similarity index 94% rename from glider/config/graph-params.yaml rename to glider/config/glider-params.yaml index 4512873..3b4108e 100644 --- a/glider/config/graph-params.yaml +++ b/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 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/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 766255c..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 @@ -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_; }; } diff --git a/glider/include/glider/utils/parameters.hpp b/glider/include/glider/utils/parameters.hpp index 7ee53c5..29355a1 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/include/ros/glider_node.hpp b/glider/include/ros/glider_node.hpp index b15fcac..8a410af 100644 --- a/glider/include/ros/glider_node.hpp +++ b/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_; }; } diff --git a/glider/launch/glider-node.launch.py b/glider/launch/glider-node.launch.py index 664b697..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, @@ -48,9 +41,14 @@ def generate_launch_description(): graph_params_file = PathJoinSubstitution([ glider_share, 'config', - 'graph-params.yaml' + '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,13 +59,13 @@ 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_gps_node/fix'), - ('/imu', '/vectornav/imu'), + ('/gps', '/ublox/fix'), + ('/imu', '/VN100T/imu'), ('/odom', '/Odometry'), ] ) - 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..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(); @@ -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(); @@ -63,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); } @@ -79,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"; } @@ -110,7 +109,11 @@ void GliderNode::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) glider_->addImu(timestamp, accel, gyro, orient); - latest_imu_timestamp_ = timestamp; + 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) 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/factor_manager.cpp b/glider/src/factor_manager.cpp index 55ac6a6..a4fb7ca 100644 --- a/glider/src/factor_manager.cpp +++ b/glider/src/factor_manager.cpp @@ -53,14 +53,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; diff --git a/glider/src/glider.cpp b/glider/src/glider.cpp index e2c81cb..9b585f5 100644 --- a/glider/src/glider.cpp +++ b/glider/src/glider.cpp @@ -15,11 +15,19 @@ 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(); + 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; + 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(); @@ -33,13 +41,21 @@ 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; } + 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(); @@ -53,35 +69,48 @@ void Glider::addGps(int64_t timestamp, Eigen::Vector3d& gps) // 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(); - if (use_dgpsfm_ && current_odom_.isInitialized()) + 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_; + + 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) { if (frame_ == "ned") { - //TODO what transforms need to happen here - //factor_manager_.addImuFactor(timestamp, accel, gyro, quat);' - LOG(FATAL) << "[GLIDER] NED frame not supported yet"; + 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") { @@ -120,4 +149,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 ee78a6f..d1d43a2 100644 --- a/glider/src/parameters.cpp +++ b/glider/src/parameters.cpp @@ -31,6 +31,7 @@ Glider::Parameters::Parameters(const std::string& path) frame = config["imu"]["frame"].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_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); 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"); } 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); +} +