IT WORKS!

MissouriMRDT · Jan 1, 2025 · 1e374fc · 1e374fc
1 parent 2ae963c
commit 1e374fc
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 14 deletions.
diff --git 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
@@ -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
@@ -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);