From da59197a0dbd263eee05313d2d4927d40aecaee9 Mon Sep 17 00:00:00 2001 From: jason Date: Fri, 17 Oct 2025 15:41:27 -0400 Subject: [PATCH] added unit test for differential gps objet --- glider/CMakeLists.txt | 18 ++-- .../include/glider/core/differential_gps.hpp | 1 + glider/include/glider/utils/gps_heading.hpp | 39 ------- glider/src/differential_gps.cpp | 6 ++ glider/src/factor_manager.cpp | 1 - glider/src/gps_heading.cpp | 38 ------- glider/test/test_differential_gps.cpp | 100 ++++++++++++++++++ 7 files changed, 116 insertions(+), 87 deletions(-) delete mode 100644 glider/include/glider/utils/gps_heading.hpp delete mode 100644 glider/src/gps_heading.cpp create mode 100644 glider/test/test_differential_gps.cpp diff --git a/glider/CMakeLists.txt b/glider/CMakeLists.txt index b369720..4b8c980 100644 --- a/glider/CMakeLists.txt +++ b/glider/CMakeLists.txt @@ -58,7 +58,6 @@ include_directories( 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 @@ -160,13 +159,6 @@ if (BUILD_TESTS) ${PROJECT_NAME} ) - add_executable(heading_test test/test_heading.cpp) - target_link_libraries(heading_test - GTest::GTest - GTest::Main - ${PROJECT_NAME} - ) - add_executable(params_test test/test_params.cpp) target_link_libraries(params_test GTest::GTest @@ -202,6 +194,14 @@ if (BUILD_TESTS) ${PROJECT_NAME} ) + add_executable(diff_gps_test test/test_differential_gps.cpp) + target_link_libraries(diff_gps_test + GTest::GTest + GTest::Main + ${PROJECT_NAME} + ) + + add_executable(ros_conversion_test test/test_ros_conversions.cpp) target_link_libraries(ros_conversion_test GTest::GTest @@ -212,11 +212,11 @@ if (BUILD_TESTS) include(GoogleTest) gtest_discover_tests(utm_test) - gtest_discover_tests(heading_test) gtest_discover_tests(params_test) gtest_discover_tests(manager_test) gtest_discover_tests(glider_test) gtest_discover_tests(odometry_test) gtest_discover_tests(odometry_with_cov_test) + gtest_discover_tests(diff_gps_test) gtest_discover_tests(ros_conversion_test) endif() diff --git a/glider/include/glider/core/differential_gps.hpp b/glider/include/glider/core/differential_gps.hpp index 3b9d76d..5a417b0 100644 --- a/glider/include/glider/core/differential_gps.hpp +++ b/glider/include/glider/core/differential_gps.hpp @@ -29,6 +29,7 @@ class DifferentialGpsFromMotion void setLastGps(const Eigen::Vector3d& gps); bool isInitialized() const; double getVelocityThreshold() const; + bool isIntegratable(const Eigen::Vector3d& vel) const; private: diff --git a/glider/include/glider/utils/gps_heading.hpp b/glider/include/glider/utils/gps_heading.hpp deleted file mode 100644 index 5b23831..0000000 --- a/glider/include/glider/utils/gps_heading.hpp +++ /dev/null @@ -1,39 +0,0 @@ -/*! - * Jason Hughes - * June 2025 - * - * This header only file implements helpful - * utility functions for calculating heading from - * differential GPS and converting that to the - * appropiate frame. -*/ - -#include - -namespace Glider -{ -namespace geodetics -{ - -/*! @brief calculate the heading between two GPS points - * FROM (lat1, lon1) TO (lat2, lon2) - * @param lat1: the FROM latitude in degrees decimal - * @param lon1: the FROM longitude in degrees decimal - * @param lat2: the TO latitude in degrees decimal - * @param lon2: the TO longitude in degrees decimal - * @return heading_rad: the heading in radians in the NED frame -*/ -double gpsHeading(double lat1, double lon1, double lat2, double lon2); -/*! @brief convert a heading in radians to degrees and normalize to [0,360) - * @param heading: the heading in radians, can be ENU or NED frame - * @return heading_deg: normalized heading in degrees in the same frame - * that was input -*/ -double headingRadiansToDegrees(double heading); -/*! @brief convert a heading from the geodetic frame (NED) to the ENU frame - * @param geodetic_heading: heading in radians in the geodetic (NED) frame - * @return enu_heading: heading in radians in the ENU frame -*/ -double geodeticToENU(double geodetic_heading); -} // namespace geodetics -} // namespace glider diff --git a/glider/src/differential_gps.cpp b/glider/src/differential_gps.cpp index ce2c306..98c081f 100644 --- a/glider/src/differential_gps.cpp +++ b/glider/src/differential_gps.cpp @@ -69,3 +69,9 @@ double DifferentialGpsFromMotion::headingRadiansToDegrees(const double heading) heading_deg = std::fmod(std::fmod(heading_deg, 360.0) + 360.0, 360.0); return heading_deg; } + +bool DifferentialGpsFromMotion::isIntegratable(const Eigen::Vector3d& velocity) const +{ + double vel = velocity.head(2).norm(); + return vel > vel_threshold_; +} diff --git a/glider/src/factor_manager.cpp b/glider/src/factor_manager.cpp index a4fb7ca..e93eae8 100644 --- a/glider/src/factor_manager.cpp +++ b/glider/src/factor_manager.cpp @@ -6,7 +6,6 @@ #include "glider/core/factor_manager.hpp" #include "glider/utils/geodetics.hpp" -#include "glider/utils/gps_heading.hpp" #include diff --git a/glider/src/gps_heading.cpp b/glider/src/gps_heading.cpp deleted file mode 100644 index 8f5a835..0000000 --- a/glider/src/gps_heading.cpp +++ /dev/null @@ -1,38 +0,0 @@ -/*! -* Jason Hughes -* June 2025 -*/ - -#include "glider/utils/gps_heading.hpp" - -double Glider::geodetics::gpsHeading(double lat1, double lon1, double lat2, double lon2) -{ - double lat1_rad = lat1 * M_PI / 180.0; - double lon1_rad = lon1 * M_PI / 180.0; - double lat2_rad = lat2 * M_PI / 180.0; - double lon2_rad = lon2 * 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); - - return heading_rad; -} - -double Glider::geodetics::headingRadiansToDegrees(double heading) -{ - 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; -} - -double Glider::geodetics::geodeticToENU(double geodetic_heading) -{ - double enu_heading = std::fmod((M_PI/2 - geodetic_heading + (2*M_PI)), (2*M_PI)); - - return enu_heading; -} diff --git a/glider/test/test_differential_gps.cpp b/glider/test/test_differential_gps.cpp new file mode 100644 index 0000000..c44d1da --- /dev/null +++ b/glider/test/test_differential_gps.cpp @@ -0,0 +1,100 @@ +/*! +* Jason Hughes +* October 2025 +* +* test the differential gps +* object +*/ + +#include + +#include "glider/core/differential_gps.hpp" + +static const double LAT = 32.925; +static const double LON = -75.199; +static const double ALT = 10.0; +static const double VEL = 1.0; + +TEST(DifferentialGpsTestSuite, TestInitialization) +{ + Glider::Geodetics::DifferentialGpsFromMotion dgps("enu", VEL); + Eigen::Vector3d gps(LAT, LON, ALT); + + dgps.setLastGps(gps); + + ASSERT_TRUE(dgps.isInitialized()); +} + +TEST(DifferentialGpsTestSuite, TestNorthHeading) +{ + Glider::Geodetics::DifferentialGpsFromMotion dgps("enu", VEL); + + double nlat = 39.942136; + double nlon = -75.19969; + double slat = 39.942041; + double slon = -75.199694; + + dgps.setLastGps(Eigen::Vector3d(slat, slon, ALT)); + + double heading = dgps.getHeading(Eigen::Vector3d(nlat, nlon, ALT)); + + ASSERT_NEAR(heading, M_PI/2, 0.1); +} + +TEST(DifferentialGpsTestSuite, TestSouthHeading) +{ + Glider::Geodetics::DifferentialGpsFromMotion dgps("enu", VEL); + + double nlat = 39.942136; + double nlon = -75.19969; + double slat = 39.942041; + double slon = -75.199694; + + dgps.setLastGps(Eigen::Vector3d(nlat, nlon, ALT)); + + double heading = dgps.getHeading(Eigen::Vector3d(slat, slon, ALT)); + + ASSERT_NEAR(heading, 3*M_PI/2, 0.1); +} + +TEST(DifferentialGpsTestSuite, TestEastHeading) +{ + Glider::Geodetics::DifferentialGpsFromMotion dgps("enu", VEL); + + double wlat = 39.942197; + double wlon = -75.199524; + double elat = 39.942193; + double elon = -75.199374; + + dgps.setLastGps(Eigen::Vector3d(wlat, wlon, ALT)); + + double heading = dgps.getHeading(Eigen::Vector3d(elat, elon, ALT)); + + ASSERT_NEAR(heading, 2*M_PI, 0.1); +} + +TEST(DifferentialGpsTestSuite, TestWestHeading) +{ + Glider::Geodetics::DifferentialGpsFromMotion dgps("enu", VEL); + + double wlat = 39.942197; + double wlon = -75.199524; + double elat = 39.942193; + double elon = -75.199374; + + dgps.setLastGps(Eigen::Vector3d(elat, elon, ALT)); + + double heading = dgps.getHeading(Eigen::Vector3d(wlat, wlon, ALT)); + + ASSERT_NEAR(heading, M_PI, 0.1); +} + +TEST(DifferentialGpsTestSuite, TestVelocityThreshold) +{ + Glider::Geodetics::DifferentialGpsFromMotion dgps("enu", VEL); + + ASSERT_EQ(VEL, dgps.getVelocityThreshold()); + + ASSERT_TRUE(dgps.isIntegratable(Eigen::Vector3d(2.0, 0.0, 0.0))); + ASSERT_FALSE(dgps.isIntegratable(Eigen::Vector3d(0.5, 0.0, 0.0))); +}