From 1e374fcf13a7b1010f6862a50b7e34ec92ed8707 Mon Sep 17 00:00:00 2001
From: clayjay3 <claytonraycowen@gmail.com>
Date: Wed, 1 Jan 2025 17:13:27 -0600
Subject: [PATCH] IT WORKS!

---
 src/handlers/WaypointHandler.cpp            | 11 ++++++-----
 src/states/NavigatingState.cpp              | 17 +++++++++--------
 tests/Unit/src/util/GeospatialOperations.cc |  2 +-
 3 files changed, 16 insertions(+), 14 deletions(-)

diff --git a/src/handlers/WaypointHandler.cpp b/src/handlers/WaypointHandler.cpp
index c2835ffba..77034e37b 100755
--- a/src/handlers/WaypointHandler.cpp
+++ b/src/handlers/WaypointHandler.cpp
@@ -857,11 +857,12 @@ geoops::RoverPose WaypointHandler::SmartRetrieveRoverPose(bool bVIOTracking)
 
     // Assemble the error metrics into a single string. We are going to include the original GPS positions of the NavBoard and the Camera and then include the error. Same
     // thing for the heading data.
-    std::string szErrorMetrics = "GPS Position Error (UTM for easy reading):\n" + std::to_string(ConvertGPSToUTM(stCurrentGPSPosition).dEasting) + " (NavBoard) vs. " +
-                                 std::to_string(stCurrentUTMPosition.dEasting) + " (Camera) = " + std::to_string(dEastingError) + " (error)\n" +
-                                 std::to_string(ConvertGPSToUTM(stCurrentGPSPosition).dNorthing) + " (NavBoard) vs. " + std::to_string(stCurrentUTMPosition.dNorthing) +
-                                 " (Camera) = " + std::to_string(dNorthingError) + " (error)\n" + "Heading Error:\n" + std::to_string(dCurrentGPSHeading) +
-                                 " (NavBoard) vs. " + std::to_string(dCurrentHeading) + " (Camera) = " + std::to_string(dHeadingError) + " (error)";
+    std::string szErrorMetrics = "--------[ ZED MainCam Pose Tracking Error ]--------\nGPS Position Error (UTM for easy reading):\n" +
+                                 std::to_string(ConvertGPSToUTM(stCurrentGPSPosition).dEasting) + " (NavBoard) vs. " + std::to_string(stCurrentUTMPosition.dEasting) +
+                                 " (Camera) = " + std::to_string(dEastingError) + " (error)\n" + std::to_string(ConvertGPSToUTM(stCurrentGPSPosition).dNorthing) +
+                                 " (NavBoard) vs. " + std::to_string(stCurrentUTMPosition.dNorthing) + " (Camera) = " + std::to_string(dNorthingError) + " (error)\n" +
+                                 "Heading Error:\n" + std::to_string(dCurrentGPSHeading) + " (NavBoard) vs. " + std::to_string(dCurrentHeading) +
+                                 " (Camera) = " + std::to_string(dHeadingError) + " (error)";
     // Submit the error metrics to the logger.
     LOG_DEBUG(logging::g_qSharedLogger, "{}", szErrorMetrics);
 
diff --git a/src/states/NavigatingState.cpp b/src/states/NavigatingState.cpp
index 45062b118..161952446 100644
--- a/src/states/NavigatingState.cpp
+++ b/src/states/NavigatingState.cpp
@@ -134,14 +134,15 @@ namespace statemachine
             // Calculate error between pose and GPS.
             geoops::GeoMeasurement stErrorMeasurement = geoops::CalculateGeoMeasurement(stCurrentRoverPose.GetGPSCoordinate(), stCurrentGPSPosition);
 
-            LOG_INFO(logging::g_qSharedLogger,
-                     "Distance from target: {} and Bearing to target: {}",
-                     stGoalWaypointMeasurement.dDistanceMeters,
-                     stGoalWaypointMeasurement.dStartRelativeBearing);
-            LOG_INFO(logging::g_qSharedLogger,
-                     "Distance from Rover: {} and Bearing to Rover: {}",
-                     stErrorMeasurement.dDistanceMeters,
-                     stErrorMeasurement.dStartRelativeBearing);
+            // Assemble the error metrics into a single string. We are going to include the distance and bearing to the goal waypoint and
+            // the error between the rover pose and the GPS position. The rover pose could be from VIO or GNSS fusion, or just GPS.
+            std::string szErrorMetrics =
+                "--------[ Navigating Error Report ]--------\nDistance to Goal Waypoint: " + std::to_string(stGoalWaypointMeasurement.dDistanceMeters) + " meters\n" +
+                "Bearing to Goal Waypoint: " + std::to_string(stGoalWaypointMeasurement.dStartRelativeBearing) + " degrees\n" +
+                "GPS Position Error (UTM for easy reading):\n" + std::to_string(stErrorMeasurement.dDistanceMeters) + " (distance) " +
+                std::to_string(stErrorMeasurement.dStartRelativeBearing) + " (bearing)";
+            // Submit the error metrics to the logger.
+            LOG_INFO(logging::g_qSharedLogger, "{}", szErrorMetrics);
 
             // Set toggle.
             bAlreadyPrinted = true;
diff --git a/tests/Unit/src/util/GeospatialOperations.cc b/tests/Unit/src/util/GeospatialOperations.cc
index a6892adbd..bacdaaeb2 100644
--- a/tests/Unit/src/util/GeospatialOperations.cc
+++ b/tests/Unit/src/util/GeospatialOperations.cc
@@ -135,7 +135,7 @@ TEST(GeoOpsTest, CalculateGeoMeasurementGPS)
     // Check distance calculation.
     EXPECT_NEAR(stMeasurement.dDistanceMeters, 1751754.48, 0.02);
     EXPECT_NEAR(stMeasurement.dArcLengthDegrees, 15.77, 0.02);
-    EXPECT_NEAR(std::abs(stMeasurement.dStartRelativeBearing - stMeasurement.dEndRelativeBearing), 180.0, 20.0);
+    EXPECT_NEAR(std::abs(stMeasurement.dStartRelativeBearing - stMeasurement.dEndRelativeBearing), 180.0, 0.02);
 
     // Calculate meter distance between the second two GPS points.
     stMeasurement = geoops::CalculateGeoMeasurement(stGPSSDELC1, stGPSSDELC2);