Merge pull request #248 from MissouriMRDT/testing/session-2

GPS Nav, Camera, and Overall Code Fixes From Testing Session 2

.devcontainer/devcontainer.json

@@ -29,6 +29,7 @@
     // Mount shared workspace volumes. Bind from host machine to container.
     "mounts": [
         "source=${localWorkspaceFolder}/data/models/zed,target=/usr/local/zed/resources,type=bind,consistency=delegated",
+        "source=${localWorkspaceFolder}/data/calibrations/zed,target=/usr/local/zed/settings,type=bind,consistency=delegated",
         "type=bind,readonly,source=/etc/localtime,target=/etc/localtime"
     ],
     "image": "ghcr.io/missourimrdt/autonomy-jammy:latest",
@@ -83,7 +84,7 @@
                 // Git settings.
                 "git.autofetch": true,
                 "git.terminalAuthentication": false,
-                "git.detectSubmodules": false,
+                "git.detectSubmodules": true,
                 // C/C++ extension settings.
                 "C_Cpp.formatting": "clangFormat",
                 "C_Cpp.default.compilerPath": "/usr/bin/g++-12",

.github/workflows/help_request.yml

@@ -26,7 +26,7 @@ jobs:
           git fetch origin ${{ github.base_ref }}:${{ github.base_ref }} --depth=1
           echo "Scanning for 'LEAD:' keyword..."
           git diff HEAD^ HEAD > diff.txt
-          awk '/^\+.*LEAD:/ && !/^\+\+\+ b\/(devcontainer.json|CONTRIBUTING.md|.github\/workflows\/help_request.yml)/{print NR ":" $0}' diff.txt > lines_with_lead.txt
+          awk '/^\+.*LEAD:/ && !/^\+\+\+ b\/(devcontainer.json|CONTRIBUTING.md|help_request.yml)/{print NR ":" $0}' diff.txt > lines_with_lead.txt
 
           if [ -s lines_with_lead.txt ]; then
             current_file=""

CONTRIBUTING.md

@@ -120,6 +120,7 @@ Examples: `int g_nExampleGlobalInteger` or `int m_nExampleMemberInteger`
 - Thread > `th` > Example: `jthread thExampleUseOfThread`
 - Mutex > `mu` > Example: `mutex muExampleUseOfMutex`
 - Lock > `lk` > Example: `lock lkExampleUseOfLock`
+- Condition Variable > `cd` > Example: `condition_variable cdExampleUseOfConditionVariable`
 - Struct > `st` > Example: `StructName stExampleUseOfStruct`
 - Future > `fu` > Example: `future<void> fuExampleUseOfFuture`
 - Promise > `pm` > Example: `promise<void> pmExampleUseIfPromise`

data/Custom_Dictionaries/Autonomy-Dictionary.txt

@@ -165,6 +165,7 @@ timestep
 tparam
 twxs
 UART
+UBLOX
 unstuckith
 UNVALIDATED
 upsample
@@ -174,6 +175,7 @@ valgrind
 VERIFYINGMARKERSTATE
 VERIFYINGOBJECTSTATE
 VIDEOWRITER
+VIOGPS
 Watchpoints
 Webots
 wslg

data/calibrations/zed/SN15723847.conf

@@ -0,0 +1,100 @@
+[LEFT_CAM_2K]
+fx=1543.78
+fy=1544.57
+cx=1094.66
+cy=615.715
+k1=-0.0345246
+k2=0.0273663
+k3=-0.0276268
+k4=0.0187331
+
+[RIGHT_CAM_2K]
+fx=1545.23
+fy=1544.99
+cx=1081.1
+cy=617.496
+k1=-0.0383875
+k2=0.0402006
+k3=-0.0423503
+k4=0.0227852
+
+[LEFT_CAM_FHD]
+fx=1543.78
+fy=1544.57
+cx=950.66
+cy=534.715
+k1=-0.0345246
+k2=0.0273663
+k3=-0.0276268
+k4=0.0187331
+
+[RIGHT_CAM_FHD]
+fx=1545.23
+fy=1544.99
+cx=937.1
+cy=536.496
+k1=-0.0383875
+k2=0.0402006
+k3=-0.0423503
+k4=0.0227852
+
+[LEFT_CAM_HD]
+fx=771.89
+fy=772.285
+cx=633.83
+cy=355.8575
+k1=-0.0345246
+k2=0.0273663
+k3=-0.0276268
+k4=0.0187331
+
+[RIGHT_CAM_HD]
+fx=772.615
+fy=772.495
+cx=627.05
+cy=356.748
+k1=-0.0383875
+k2=0.0402006
+k3=-0.0423503
+k4=0.0227852
+
+[LEFT_CAM_VGA]
+fx=385.945
+fy=386.1425
+cx=332.415
+cy=185.42875
+k1=-0.0345246
+k2=0.0273663
+k3=-0.0276268
+k4=0.0187331
+
+[RIGHT_CAM_VGA]
+fx=386.3075
+fy=386.2475
+cx=329.025
+cy=185.874
+k1=-0.0383875
+k2=0.0402006
+k3=-0.0423503
+k4=0.0227852
+
+[STEREO]
+Baseline=62.8243
+TY=-0.0180836
+TZ=0.243798
+CV_2K=0.0107685
+CV_FHD=0.0107685
+CV_HD=0.0107685
+CV_VGA=0.0107685
+RX_2K=0.00194703
+RX_FHD=0.00194703
+RX_HD=0.00194703
+RX_VGA=0.00194703
+RZ_2K=0.000228629
+RZ_FHD=0.000228629
+RZ_HD=0.000228629
+RZ_VGA=0.000228629
+
+[MISC]
+Sensor_ID=0
+

data/calibrations/zed/SN31237348.conf

@@ -0,0 +1,108 @@
+[LEFT_CAM_2K]
+fx=1054.87
+fy=1054.73
+cx=1096.24
+cy=614.267
+k1=-0.0394108
+k2=0.00919547
+p1=0.00010587
+p2=0.00037718
+k3=-0.00479
+
+[RIGHT_CAM_2K]
+fx=1054.35
+fy=1053.37
+cx=1099.5
+cy=607.24
+k1=-0.0430047
+k2=0.0115404
+p1=0.00044126
+p2=-0.000174894
+k3=-0.00528563
+
+[LEFT_CAM_FHD]
+fx=1054.87
+fy=1054.73
+cx=952.24
+cy=533.267
+k1=-0.0394108
+k2=0.00919547
+p1=0.00010587
+p2=0.00037718
+k3=-0.00479
+
+[RIGHT_CAM_FHD]
+fx=1054.35
+fy=1053.37
+cx=955.5
+cy=526.24
+k1=-0.0430047
+k2=0.0115404
+p1=0.00044126
+p2=-0.000174894
+k3=-0.00528563
+
+[LEFT_CAM_HD]
+fx=527.435
+fy=527.365
+cx=634.62
+cy=355.1335
+k1=-0.0394108
+k2=0.00919547
+p1=0.00010587
+p2=0.00037718
+k3=-0.00479
+
+[RIGHT_CAM_HD]
+fx=527.175
+fy=526.685
+cx=636.25
+cy=351.62
+k1=-0.0430047
+k2=0.0115404
+p1=0.00044126
+p2=-0.000174894
+k3=-0.00528563
+
+[LEFT_CAM_VGA]
+fx=263.7175
+fy=263.6825
+cx=332.81
+cy=185.06675
+k1=-0.0394108
+k2=0.00919547
+p1=0.00010587
+p2=0.00037718
+k3=-0.00479
+
+[RIGHT_CAM_VGA]
+fx=263.5875
+fy=263.3425
+cx=333.625
+cy=183.31
+k1=-0.0430047
+k2=0.0115404
+p1=0.00044126
+p2=-0.000174894
+k3=-0.00528563
+
+[STEREO]
+Baseline=119.853
+TY=-0.164677
+TZ=0.16404
+CV_2K=0.000238724
+CV_FHD=0.000238724
+CV_HD=0.000238724
+CV_VGA=0.000238724
+RX_2K=0.00255524
+RX_FHD=0.00255524
+RX_HD=0.00255524
+RX_VGA=0.00255524
+RZ_2K=4.5826e-05
+RZ_FHD=4.5826e-05
+RZ_HD=4.5826e-05
+RZ_VGA=4.5826e-05
+
+[MISC]
+Sensor_ID=0
+

examples/vision/cameras/OpenZEDCam.hpp

@@ -95,7 +95,7 @@ void RunExample()
     cv::cuda::GpuMat cvGPUDepthFrame1;
     cv::cuda::GpuMat cvGPUPointCloud1;
     // Declare other data types to store data in.
-    sl::Pose slPose;
+    ZEDCam::Pose stPose;
 
     // Declare FPS counter.
     IPS FPS = IPS();
@@ -124,7 +124,7 @@ void RunExample()
             fuPointCloudCopyStatus = ExampleZEDCam1->RequestPointCloudCopy(cvPointCloud1);
         }
         // Grab other info from camera.
-        std::future<bool> fuPoseCopyStatus = ExampleZEDCam1->RequestPositionalPoseCopy(slPose);
+        std::future<bool> fuPoseCopyStatus = ExampleZEDCam1->RequestPositionalPoseCopy(stPose);
 
         // Wait for the frames to be copied.
         if (fuFrameCopyStatus.get() && fuDepthCopyStatus.get() && fuPointCloudCopyStatus.get())
@@ -155,12 +155,16 @@ void RunExample()
             // Wait for the other info to be copied.
             if (fuPoseCopyStatus.get())
             {
-                // Wait for the
-                sl::Translation slTranslation = slPose.getTranslation();
-                sl::float3 slEulerAngles      = slPose.getEulerAngles(false);
-
-                LOG_INFO(logging::g_qConsoleLogger, "Positional Tracking: X: {} | Y: {} | Z: {}", slTranslation.x, slTranslation.y, slTranslation.z);
-                LOG_INFO(logging::g_qConsoleLogger, "Positional Orientation: Roll: {} | Pitch: {} | Yaw:{}", slEulerAngles[0], slEulerAngles[1], slEulerAngles[2]);
+                LOG_INFO(logging::g_qConsoleLogger,
+                         "Positional Tracking: X: {} | Y: {} | Z: {}",
+                         stPose.stTranslation.dX,
+                         stPose.stTranslation.dY,
+                         stPose.stTranslation.dZ);
+                LOG_INFO(logging::g_qConsoleLogger,
+                         "Positional Orientation: Roll: {} | Pitch: {} | Yaw:{}",
+                         stPose.stEulerAngles.dXO,
+                         stPose.stEulerAngles.dYO,
+                         stPose.stEulerAngles.dZO);
             }
 
             // Print info.

src/AutonomyConstants.h

@@ -70,8 +70,8 @@ namespace constants
     // Power constants.
     const float DRIVE_MAX_POWER  = 1.0;
     const float DRIVE_MIN_POWER  = -1.0;
-    const float DRIVE_MAX_EFFORT = 1.0;
-    const float DRIVE_MIN_EFFORT = -1.0;
+    const float DRIVE_MAX_EFFORT = 0.35;
+    const float DRIVE_MIN_EFFORT = -0.35;
 
     // Control constants.
     const double DRIVE_PID_PROPORTIONAL       = 0.01;     // The proportional gain for the controller used to point the rover at a goal heading during navigation.
@@ -132,11 +132,11 @@ namespace constants
     const float ZED_POSETRACK_USE_GRAVITY_ORIGIN          = true;     // Override 2 of the 3 rotations from initial_world_transform using the IMU.
     // ZedCam Spatial Mapping Config.
     const sl::SpatialMappingParameters::SPATIAL_MAP_TYPE ZED_MAPPING_TYPE = sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::MESH;    // Mesh or point cloud output.
-    const float ZED_MAPPING_RANGE_METER                                   = 40.0;    // The max range in meters that the ZED cameras should use for mapping. 0 = auto.
-    const float ZED_MAPPING_RESOLUTION_METER                              = 0.1;     // The approx goal precision for spatial mapping in METERS. Higher = Faster.
+    const float ZED_MAPPING_RANGE_METER                                   = 20.0;    // The max range in meters that the ZED cameras should use for mapping. 0 = auto.
+    const float ZED_MAPPING_RESOLUTION_METER                              = 0.03;    // The approx goal precision for spatial mapping in METERS. Higher = Faster.
     const int ZED_MAPPING_MAX_MEMORY                                      = 4096;    // The max amount of CPU RAM (MB) that can be allocated for spatial mapping.
     const bool ZED_MAPPING_USE_CHUNK_ONLY   = true;    // Only update chunks that have probably changed or have new data. Faster, less accurate.
-    const int ZED_MAPPING_STABILITY_COUNTER = 4;       // Number of times that a point should be seen before adding to mesh.
+    const int ZED_MAPPING_STABILITY_COUNTER = 3;       // Number of times that a point should be seen before adding to mesh.
     // ZedCam Object Detection Config.
     const bool ZED_OBJDETECTION_TRACK_OBJ                      = true;     // Whether or not to enable object tracking in the scene. Attempts to maintain OBJ UUIDs.
     const bool ZED_OBJDETECTION_SEGMENTATION                   = false;    // Use depth data to compute the segmentation for an object. (exact outline/shape)