Camera Promises and Futures

.clang-format

@@ -48,7 +48,7 @@ BreakBeforeBinaryOperators: None
 BreakBeforeTernaryOperators: true
 BreakConstructorInitializers: AfterColon
 BreakInheritanceList: AfterColon
-BreakStringLiterals: false
+BreakStringLiterals: true
 ColumnLimit: 170
 CompactNamespaces: false
 ContinuationIndentWidth: 4

CONTRIBUTING.md

@@ -93,6 +93,7 @@ Examples: `int g_nExampleGlobalInteger` or `int m_nExampleMemberInteger`
 - Lock      > `lk` > Example: `lock lkExampleUseOfLock`
 - Struct    > `st` > Example: `StructName stExampleUseOfStruct`
 - Future    > `fu` > Example: `future<void> fuExampleUseOfFuture`
+- Promise   > `pm` > Example: `promise<void> pmExampleUseIfPromise`
 
 #### External Types
 - OpenCV > `cv` > Example: `cv::Mat cvExampleMat`

examples/vision/OpenBasicCam.hpp

@@ -13,8 +13,19 @@
 #include "../../src/vision/cameras/BasicCam.h"
 
 /******************************************************************************
- * @brief Main example method.
+ * @brief This example demonstrates the proper way to interact with the CameraHandler.
+ *      A pointer to a BasicCam is retrieved and then a couple of local cv::Mat are created
+ *      for storing frames. Then, the frames are passed to the RequestFrameCopy function of the
+ *      camera and a future is IMMEDIATELY returned. The method call doesn't wait for frame to be
+ *      retrieved/copied before returning. This allows you to request multiple frames/data from the
+ *      camera non-sequentially.
  *
+ *      Inside the camera thread, the cv::Mat pointer that points to the cv::Mat within THIS class
+ *      is written to and an std::promise is set to TRUE. The future that was return now contains this
+ *      TRUE value. When the get() method is called on the returned future, the code will block until
+ *      the promise is fullfilled (set to TRUE). Once the get() method returns, the cv::Mat within
+ *      this class now contains a complete frame and can be display or used in other computer vision
+ *      things.
  *
  * @author ClayJay3 ([email protected])
  * @date 2023-07-22
@@ -26,26 +37,62 @@ void RunExample()
     g_pCameraHandler->StartAllCameras();
 
     // Get reference to camera.
-    BasicCam* TestCamera1 = g_pCameraHandler->GetBasicCam(CameraHandlerThread::eHeadLeftArucoEye);
+    BasicCam* ExampleBasicCam1 = g_pCameraHandler->GetBasicCam(CameraHandlerThread::eHeadLeftArucoEye);
     // Declare mats to store images in.
     cv::Mat cvNormalFrame1;
+    cv::Mat cvNormalFrame2;
+
+    // Declare FPS counter.
+    IPS FPS = IPS();
 
     // Loop forever, or until user hits ESC.
     while (true)
     {
         // Grab normal frame from camera.
-        if (TestCamera1->GrabFrame(cvNormalFrame1))
+        std::future<bool> fuCopyStatus1 = ExampleBasicCam1->RequestFrameCopy(cvNormalFrame1);
+        std::future<bool> fuCopyStatus2 = ExampleBasicCam1->RequestFrameCopy(cvNormalFrame2);
+
+        // Show first frame copy.
+        if (fuCopyStatus1.get() && !cvNormalFrame1.empty())
         {
             // Print info.
-            LOG_INFO(g_qConsoleLogger, "BasicCam Getter FPS: {} | 1% Low: {}", TestCamera1->GetIPS().GetAverageIPS(), TestCamera1->GetIPS().Get1PercentLow());
+            LOG_INFO(g_qConsoleLogger, "BasicCam Getter FPS: {} | 1% Low: {}", ExampleBasicCam1->GetIPS().GetAverageIPS(), ExampleBasicCam1->GetIPS().Get1PercentLow());
 
             // Put FPS on normal frame.
-            cv::putText(cvNormalFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255));
+            cv::putText(cvNormalFrame1,
+                        std::to_string(ExampleBasicCam1->GetIPS().GetExactIPS()),
+                        cv::Point(50, 50),
+                        cv::FONT_HERSHEY_COMPLEX,
+                        1,
+                        cv::Scalar(255, 255, 255));
 
             // Display frame.
-            cv::imshow("BasicCamExample", cvNormalFrame1);
+            cv::imshow("BasicCamExample Frame1", cvNormalFrame1);
         }
 
+        // Show second frame copy.
+        if (fuCopyStatus2.get() && !cvNormalFrame2.empty())
+        {
+            // Print info.
+            LOG_INFO(g_qConsoleLogger, "BasicCam Getter FPS: {} | 1% Low: {}", ExampleBasicCam1->GetIPS().GetAverageIPS(), ExampleBasicCam1->GetIPS().Get1PercentLow());
+
+            // Put FPS on normal frame.
+            cv::putText(cvNormalFrame2,
+                        std::to_string(ExampleBasicCam1->GetIPS().GetExactIPS()),
+                        cv::Point(50, 50),
+                        cv::FONT_HERSHEY_COMPLEX,
+                        1,
+                        cv::Scalar(255, 255, 255));
+
+            // Display frame.
+            cv::imshow("BasicCamExample Frame2", cvNormalFrame2);
+        }
+
+        // Tick FPS counter.
+        FPS.Tick();
+        // Print FPS of main loop.
+        LOG_INFO(g_qConsoleLogger, "Main FPS: {}", FPS.GetAverageIPS());
+
         char chKey = cv::waitKey(1);
         if (chKey == 27)    // Press 'Esc' key to exit
             break;

examples/vision/OpenZEDCam.hpp

@@ -7,6 +7,7 @@
  * @date 2023-09-16
  ******************************************************************************/
 
+#include "../../src/AutonomyConstants.h"
 #include "../../src/AutonomyGlobals.h"
 #include "../../src/AutonomyLogging.h"
 #include "../../src/util/ExampleChecker.h"
@@ -16,7 +17,23 @@
 const bool ENABLE_SPATIAL_MAPPING = false;
 
 /******************************************************************************
- * @brief Main example method.
+ * @brief This example demonstrates the proper way to interact with the CameraHandler.
+ *      A pointer to a ZEDCam is retrieved and then a couple of local cv::Mat are created
+ *      for storing frames. Then, the frames are passed to the RequestFrameCopy function of the
+ *      camera and a future is IMMEDIATELY returned. The method call doesn't wait for frame to be
+ *      retrieved/copied before returning. This allows you to request multiple frames/data from the
+ *      camera non-sequentially.
+ *
+ *      Inside the camera thread, the cv::Mat pointer that points to the cv::Mat within THIS class
+ *      is written to and an std::promise is set to TRUE. The future that was return now contains this
+ *      TRUE value. When the get() method is called on the returned future, the code will block until
+ *      the promise is fullfilled (set to TRUE). Once the get() method returns, the cv::Mat within
+ *      this class now contains a complete frame and can be display or used in other computer vision
+ *      things.
+ *
+ *      The same exact process happens for the positional tracking pose that is retrieved from the camera.
+ *      Multiple other methods of the ZEDCam class work this way as it allows this thread and other threads
+ *      to get multiple pieces of from the camera without slowing it down to an unusable speed.
  *
  *
  * @author ClayJay3 ([email protected])
@@ -29,66 +46,101 @@ void RunExample()
     g_pCameraHandler->StartAllCameras();
 
     // Get reference to camera.
-    ZEDCam* TestCamera1 = g_pCameraHandler->GetZED(CameraHandlerThread::eHeadMainCam);
+    ZEDCam* ExampleZEDCam1 = g_pCameraHandler->GetZED(CameraHandlerThread::eHeadMainCam);
 
     // Turn on ZED features.
-    TestCamera1->EnablePositionalTracking();
+    ExampleZEDCam1->EnablePositionalTracking();
     // Check if we should turn on spatial mapping.
     if (ENABLE_SPATIAL_MAPPING)
     {
         // Enable spatial mapping.
-        TestCamera1->EnableSpatialMapping();
+        ExampleZEDCam1->EnableSpatialMapping();
     }
 
     // Declare mats to store images in.
     cv::Mat cvNormalFrame1;
     cv::Mat cvDepthFrame1;
     cv::cuda::GpuMat cvGPUNormalFrame1;
     cv::cuda::GpuMat cvGPUDepthFrame1;
+    // Declare other data types to store data in.
+    sl::Pose slPose;
 
     // Declare FPS counter.
     IPS FPS = IPS();
 
     // Loop forever, or until user hits ESC.
     while (true)
     {
-        // Grab normal frame from camera.
-        if (TestCamera1->GrabFrame(cvGPUNormalFrame1) && TestCamera1->GrabDepth(cvGPUDepthFrame1, false))
+        // Create instance variables.
+        std::future<bool> fuFrameCopyStatus;
+        std::future<bool> fuDepthCopyStatus;
+
+        // Check if the camera is setup to use CPU or GPU mats.
+        if (constants::ZED_MAINCAM_USE_GPU_MAT)
         {
-            // Download memory from gpu mats if necessary.
-            cvGPUNormalFrame1.download(cvNormalFrame1);
-            cvGPUDepthFrame1.download(cvDepthFrame1);
+            // Grab frames from camera.
+            fuFrameCopyStatus = ExampleZEDCam1->RequestFrameCopy(cvGPUNormalFrame1);
+            fuDepthCopyStatus = ExampleZEDCam1->RequestDepthCopy(cvGPUDepthFrame1, false);
+        }
+        else
+        {
+            // Grab frames from camera.
+            fuFrameCopyStatus = ExampleZEDCam1->RequestFrameCopy(cvNormalFrame1);
+            fuDepthCopyStatus = ExampleZEDCam1->RequestDepthCopy(cvDepthFrame1, false);
+        }
+        // Grab other info from camera.
+        std::future<bool> fuPoseCopyStatus = ExampleZEDCam1->RequestPositionalPoseCopy(slPose);
+
+        // Wait for the frames to be copied.
+        if (fuFrameCopyStatus.get() && fuDepthCopyStatus.get())
+        {
+            // Check if the camera is setup to use CPU or GPU mats.
+            if (constants::ZED_MAINCAM_USE_GPU_MAT)
+            {
+                // Download memory from gpu mats if necessary.
+                cvGPUNormalFrame1.download(cvNormalFrame1);
+                cvGPUDepthFrame1.download(cvDepthFrame1);
+            }
 
             // Put FPS on normal frame.
-            cv::putText(cvNormalFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255));
+            cv::putText(cvNormalFrame1,
+                        std::to_string(ExampleZEDCam1->GetIPS().GetExactIPS()),
+                        cv::Point(50, 50),
+                        cv::FONT_HERSHEY_COMPLEX,
+                        1,
+                        cv::Scalar(255, 255, 255));
 
             // Put FPS on depth frame.
-            cv::putText(cvDepthFrame1, std::to_string(TestCamera1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255));
+            cv::putText(cvDepthFrame1, std::to_string(ExampleZEDCam1->GetIPS().GetExactIPS()), cv::Point(50, 50), cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 255, 255));
 
             // Display frame.
-            cv::imshow("TEST1", cvNormalFrame1);
+            cv::imshow("FRAME1", cvNormalFrame1);
             cv::imshow("DEPTH1", cvDepthFrame1);
 
-            // Get info about position.
-            sl::Pose slPose;
-            TestCamera1->GetPositionalPose(slPose);
-            sl::Translation slTranslation = slPose.getTranslation();
-            sl::float3 slEulerAngles      = slPose.getEulerAngles(false);
+            // 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(g_qConsoleLogger, "Positional Tracking: X: {} | Y: {} | Z: {}", slTranslation.x, slTranslation.y, slTranslation.z);
+                LOG_INFO(g_qConsoleLogger, "Positional Orientation: Roll: {} | Pitch: {} | Yaw:{}", slEulerAngles[0], slEulerAngles[1], slEulerAngles[2]);
+            }
 
             // Print info.
-            LOG_INFO(g_qConsoleLogger, "ZED Getter FPS: {} | 1% Low: {}", TestCamera1->GetIPS().GetAverageIPS(), TestCamera1->GetIPS().Get1PercentLow());
-            LOG_INFO(g_qConsoleLogger, "Main FPS: {}", FPS.GetExactIPS());
-            LOG_INFO(g_qConsoleLogger, "Positional Tracking: X: {} | Y: {} | Z: {}", slTranslation.x, slTranslation.y, slTranslation.z);
-            LOG_INFO(g_qConsoleLogger, "Positional Orientation: Roll: {} | Pitch: {} | Yaw:{}", slEulerAngles[0], slEulerAngles[1], slEulerAngles[2]);
+            LOG_INFO(g_qConsoleLogger, "ZED Getter FPS: {} | 1% Low: {}", ExampleZEDCam1->GetIPS().GetAverageIPS(), ExampleZEDCam1->GetIPS().Get1PercentLow());
             // Check if spatial mapping is enabled.
             if (ENABLE_SPATIAL_MAPPING)
             {
-                LOG_INFO(g_qConsoleLogger, "Spatial Mapping State: {}", sl::toString(TestCamera1->GetSpatialMappingState()).get());
+                LOG_INFO(g_qConsoleLogger, "Spatial Mapping State: {}", sl::toString(ExampleZEDCam1->GetSpatialMappingState()).get());
             }
         }
 
         // Tick FPS counter.
         FPS.Tick();
+        // Print FPS of main loop.
+        LOG_INFO(g_qConsoleLogger, "Main FPS: {}", FPS.GetAverageIPS());
 
         char chKey = cv::waitKey(1);
         if (chKey == 27)    // Press 'Esc' key to exit
@@ -102,7 +154,7 @@ void RunExample()
     {
         // Extract spatial map.
         std::future<sl::Mesh> fuSpatialMap;
-        TestCamera1->ExtractSpatialMapAsync(fuSpatialMap);
+        ExampleZEDCam1->ExtractSpatialMapAsync(fuSpatialMap);
         sl::Mesh slSpatialMap = fuSpatialMap.get();
         slSpatialMap.save("test.obj", sl::MESH_FILE_FORMAT::PLY);
     }

src/AutonomyConstants.h

@@ -11,6 +11,7 @@
 #ifndef CONSTS_H
 #define CONSTS_H
 
+#include <opencv2/opencv.hpp>
 #include <sl/Camera.hpp>
 
 #include "./interfaces/Camera.hpp"
@@ -55,6 +56,7 @@ 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                                   = 20.0;    // The max range in meters that the ZED cameras should use for mapping. 0 = auto.
     const float ZED_MAPPING_RESOLUTION_METER                              = 0.01;    // 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.
@@ -67,6 +69,9 @@ namespace constants
     const float ZED_OBJDETECTION_TRACKING_PREDICTION_TIMEOUT   = 0.5;    // 0-1 second. Timeout to keep guessing object position when not in sight.
     const float ZED_OBJDETECTION_BATCH_RETENTION_TIME          = 240;    // The time in seconds to search for an object UUID before expiring the object.
     const float ZED_OBJDETECTION_BATCH_LATENCY = 2;    // Short latency will limit the search for previously seen object IDs but will be closer to real time output.
+
+    // BasicCam Basic Config.
+    const cv::InterpolationFlags BASICCAM_RESIZE_INTERPOLATION_METHOD = cv::InterpolationFlags::INTER_LINEAR;    // The algorithm used to fill in pixels when resizing.
     ///////////////////////////////////////////////////////////////////////////
 
     ///////////////////////////////////////////////////////////////////////////
@@ -85,14 +90,14 @@ namespace constants
     const int ZED_MAINCAN_SERIAL                    = 31237348;    // The serial number of the camera.
 
     // Left Side Cam.
-    const int BASIC_LEFTCAM_RESOLUTIONX             = 1280;    // The horizontal pixel resolution to resize the maincam images to.
-    const int BASIC_LEFTCAM_RESOLUTIONY             = 720;     // The vertical pixel resolution to resize the maincam images to.
-    const int BASIC_LEFTCAM_FPS                     = 30;      // The FPS to use for the maincam.
-    const int BASIC_LEFTCAM_HORIZONTAL_FOV          = 110;     // The horizontal FOV of the camera. Useful for future calculations.
-    const int BASIC_LEFTCAM_VERTICAL_FOV            = 70;      // The vertical FOV of the camera. Useful for future calculations.
-    const int BASIC_LEFTCAM_FRAME_RETRIEVAL_THREADS = 10;      // The number of threads allocated to the threadpool for performing frame copies to other threads.
-    const int BASIC_LEFTCAM_INDEX                   = 0;       // The /dev/video index of the camera.
-    const PIXEL_FORMATS BASIC_LEFTCAM_PIXELTYPE     = PIXEL_FORMATS::eBGR;    // The pixel layout of the camera.
+    const int BASICCAM_LEFTCAM_RESOLUTIONX             = 1280;    // The horizontal pixel resolution to resize the maincam images to.
+    const int BASICCAM_LEFTCAM_RESOLUTIONY             = 720;     // The vertical pixel resolution to resize the maincam images to.
+    const int BASICCAM_LEFTCAM_FPS                     = 30;      // The FPS to use for the maincam.
+    const int BASICCAM_LEFTCAM_HORIZONTAL_FOV          = 110;     // The horizontal FOV of the camera. Useful for future calculations.
+    const int BASICCAM_LEFTCAM_VERTICAL_FOV            = 70;      // The vertical FOV of the camera. Useful for future calculations.
+    const int BASICCAM_LEFTCAM_FRAME_RETRIEVAL_THREADS = 10;      // The number of threads allocated to the threadpool for performing frame copies to other threads.
+    const int BASICCAM_LEFTCAM_INDEX                   = 0;       // The /dev/video index of the camera.
+    const PIXEL_FORMATS BASICCAM_LEFTCAM_PIXELTYPE     = PIXEL_FORMATS::eBGR;    // The pixel layout of the camera.
     ///////////////////////////////////////////////////////////////////////////
 
 }    // namespace constants

src/interfaces/Camera.hpp

@@ -12,36 +12,10 @@
 #define CAMERA_HPP
 
 #include "../util/IPS.hpp"
+#include "../util/vision/FetchContainers.hpp"
 
-// Declare global/file-scope enumerator.
-enum PIXEL_FORMATS
-{
-    eRGB,
-    eBGR,
-    eRGBA,
-    eBGRA,
-    eARGB,
-    eABGR,
-    eRGBE,
-    eXYZ,
-    eRGBXZY,
-    eRGBAXYZ,
-    eZED,
-    eGrayscale,
-    eDepthImage,
-    eDepthMeasure,
-    eCMYK,
-    eYUV,
-    eYUYV,
-    eYUVJ,
-    eHSV,
-    eHSL,
-    eSRGB,
-    eLAB,
-    eUNKNOWN
-};
-
-///////////////////////////////////////////////////////////////////////////////
+#include <future>
+#include <shared_mutex>
 
 /******************************************************************************
  * @brief This interface class serves as a base for all other classes that will
@@ -66,9 +40,14 @@ class Camera
         double m_dPropHorizontalFOV;
         double m_dPropVerticalFOV;
 
+        // Queues and mutexes for scheduling and copying camera frames and data to other threads.
+        std::queue<containers::FrameFetchContainer<T>> m_qFrameCopySchedule;
+        std::shared_mutex m_muPoolScheduleMutex;
+        std::mutex m_muFrameCopyMutex;
+
         // Declare interface class pure virtual functions. (These must be overriden by inheritor.)
-        virtual bool GrabFrame(T& tFrame) = 0;    // This is where the code to retrieve an image from the camera is put.
-        virtual bool GetCameraIsOpen()    = 0;    // This is where the code to check if the camera is current open goes.
+        virtual std::future<bool> RequestFrameCopy(T& tFrame) = 0;    // This is where the code to retrieve an image from the camera is put.
+        virtual bool GetCameraIsOpen()                        = 0;    // This is where the code to check if the camera is current open goes.
 
         // Declare protected object pointers.
         IPS m_IPS = IPS();

src/main.cpp

@@ -13,7 +13,7 @@
 #include <sstream>
 #include <string>
 
-#include "../examples/vision/OpenZEDCam.hpp"
+#include "../examples/vision/OpenBasicCam.hpp"
 #include "./AutonomyGlobals.h"
 #include "./AutonomyLogging.h"
 #include "./interfaces/StateMachine.hpp"