From 9ba759d92c7a84b56b7012360c5ee574086af461 Mon Sep 17 00:00:00 2001 From: jason Date: Thu, 16 Oct 2025 21:01:27 -0400 Subject: [PATCH 1/2] 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 2/2] 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'), ] )