Merge pull request #21 from KumarRobotics/test/diff-gps

added unit test for differential gps object

Author: jhughes50

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()

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:
 

glider/include/glider/utils/gps_heading.hpp

@@ -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_;
+}

glider/src/differential_gps.cpp

@@ -6,7 +6,6 @@
 
 #include "glider/core/factor_manager.hpp"
 #include "glider/utils/geodetics.hpp"
-#include "glider/utils/gps_heading.hpp"
 
 #include <gtsam/slam/expressions.h>
 

glider/src/factor_manager.cpp

@@ -0,0 +1,100 @@
+/*!
+* Jason Hughes
+* October 2025
+*
+* test the differential gps 
+* object
+*/
+
+#include <gtest/gtest.h>
+
+#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)));
+}