diff --git a/.clang-format b/.clang-format index a7147bf0..c80deb35 100644 --- a/.clang-format +++ b/.clang-format @@ -48,7 +48,7 @@ BreakBeforeBinaryOperators: None BreakBeforeTernaryOperators: true BreakConstructorInitializers: AfterColon BreakInheritanceList: AfterColon -BreakStringLiterals: false +BreakStringLiterals: true ColumnLimit: 170 CompactNamespaces: false ContinuationIndentWidth: 4 diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index d658aab5..88d34631 100644 --- a/CONTRIBUTING.md +++ b/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 fuExampleUseOfFuture` +- Promise > `pm` > Example: `promise pmExampleUseIfPromise` #### External Types - OpenCV > `cv` > Example: `cv::Mat cvExampleMat` diff --git a/examples/vision/OpenBasicCam.hpp b/examples/vision/OpenBasicCam.hpp index 31907f97..c1433120 100644 --- a/examples/vision/OpenBasicCam.hpp +++ b/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 (claytonraycowen@gmail.com) * @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 fuCopyStatus1 = ExampleBasicCam1->RequestFrameCopy(cvNormalFrame1); + std::future 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; diff --git a/examples/vision/OpenZEDCam.hpp b/examples/vision/OpenZEDCam.hpp index 25759bc3..d6a0e3c3 100644 --- a/examples/vision/OpenZEDCam.hpp +++ b/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 (claytonraycowen@gmail.com) @@ -29,15 +46,15 @@ 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. @@ -45,6 +62,8 @@ void RunExample() 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(); @@ -52,43 +71,76 @@ void RunExample() // 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 fuFrameCopyStatus; + std::future 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 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 fuSpatialMap; - TestCamera1->ExtractSpatialMapAsync(fuSpatialMap); + ExampleZEDCam1->ExtractSpatialMapAsync(fuSpatialMap); sl::Mesh slSpatialMap = fuSpatialMap.get(); slSpatialMap.save("test.obj", sl::MESH_FILE_FORMAT::PLY); } diff --git a/src/AutonomyConstants.h b/src/AutonomyConstants.h index 30758279..6da00a96 100644 --- a/src/AutonomyConstants.h +++ b/src/AutonomyConstants.h @@ -11,6 +11,7 @@ #ifndef CONSTS_H #define CONSTS_H +#include #include #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 diff --git a/src/interfaces/Camera.hpp b/src/interfaces/Camera.hpp index 48046a9a..54dcc9ab 100644 --- a/src/interfaces/Camera.hpp +++ b/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 +#include /****************************************************************************** * @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> 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 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(); diff --git a/src/main.cpp b/src/main.cpp index 26d79db3..507ac33c 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,7 +13,7 @@ #include #include -#include "../examples/vision/OpenZEDCam.hpp" +#include "../examples/vision/OpenBasicCam.hpp" #include "./AutonomyGlobals.h" #include "./AutonomyLogging.h" #include "./interfaces/StateMachine.hpp" diff --git a/src/threads/CameraHandlerThread.cpp b/src/threads/CameraHandlerThread.cpp index c7cefd2c..161a0f22 100644 --- a/src/threads/CameraHandlerThread.cpp +++ b/src/threads/CameraHandlerThread.cpp @@ -30,16 +30,18 @@ CameraHandlerThread::CameraHandlerThread() constants::ZED_DEFAULT_MAXIMUM_DISTANCE, constants::ZED_MAINCAM_USE_GPU_MAT, constants::ZED_MAINCAM_USE_HALF_PRECISION_DEPTH, + constants::ZED_MAINCAM_FRAME_RETRIEVAL_THREADS, constants::ZED_MAINCAN_SERIAL); // Initialize Left acruco eye. - m_pLeftCam = new BasicCam(constants::BASIC_LEFTCAM_INDEX, - constants::BASIC_LEFTCAM_RESOLUTIONX, - constants::BASIC_LEFTCAM_RESOLUTIONY, - constants::BASIC_LEFTCAM_FPS, - constants::BASIC_LEFTCAM_PIXELTYPE, - constants::BASIC_LEFTCAM_HORIZONTAL_FOV, - constants::BASIC_LEFTCAM_VERTICAL_FOV); + m_pLeftCam = new BasicCam(constants::BASICCAM_LEFTCAM_INDEX, + constants::BASICCAM_LEFTCAM_RESOLUTIONX, + constants::BASICCAM_LEFTCAM_RESOLUTIONY, + constants::BASICCAM_LEFTCAM_FPS, + constants::BASICCAM_LEFTCAM_PIXELTYPE, + constants::BASICCAM_LEFTCAM_HORIZONTAL_FOV, + constants::BASICCAM_LEFTCAM_VERTICAL_FOV, + constants::BASICCAM_LEFTCAM_FRAME_RETRIEVAL_THREADS); } /****************************************************************************** diff --git a/src/threads/CameraHandlerThread.h b/src/threads/CameraHandlerThread.h index 4a30ce90..d96cc776 100644 --- a/src/threads/CameraHandlerThread.h +++ b/src/threads/CameraHandlerThread.h @@ -30,12 +30,18 @@ class CameraHandlerThread { private: + ///////////////////////////////////////// // Declare private class member variables. + ///////////////////////////////////////// + ZEDCam* m_pMainCam; BasicCam* m_pLeftCam; public: + ///////////////////////////////////////// // Define public enumerators specific to this class. + ///////////////////////////////////////// + enum ZEDCamName // Enum for different zed cameras. { eHeadMainCam @@ -47,12 +53,18 @@ class CameraHandlerThread eHeadRightAcuroEye }; + ///////////////////////////////////////// // Declare public class methods and variables. + ///////////////////////////////////////// + CameraHandlerThread(); ~CameraHandlerThread(); void StartAllCameras(); + ///////////////////////////////////////// // Accessors. + ///////////////////////////////////////// + ZEDCam* GetZED(ZEDCamName eCameraName); BasicCam* GetBasicCam(BasicCamName eCameraName); }; diff --git a/src/util/vision/FetchContainers.hpp b/src/util/vision/FetchContainers.hpp index ea575c3c..5f7670f2 100644 --- a/src/util/vision/FetchContainers.hpp +++ b/src/util/vision/FetchContainers.hpp @@ -14,9 +14,42 @@ #ifndef FETCH_CONTAINERS_HPP #define FETCH_CONTAINERS_HPP +#include "../../interfaces/Camera.hpp" + +#include #include #include +// 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 +}; + +/////////////////////////////////////////////////////////////////////////////// + /****************************************************************************** * @brief Namespace containing functions or objects/struct used to aid in data * storage and passage between threads. @@ -28,9 +61,16 @@ namespace containers { /****************************************************************************** - * @brief This struct is used to contain camera frames for scheduling and copying. - * It also stores a condition variable and mutex for signaling when the copy - * has been completed. + * @brief This struct is used to carry references to camera frames for scheduling and copying. + * It is constructed so that a reference to a frame is passed into the container and the pointer + * to that reference is stored internally. Then a shared_pointer to a new std::promise is created, + * which allows the programmer to return a future from whatever method created this container. + * The future can then be waited on before the passed in frame is used. The future will return a true or false. + * + * The idea is that any threads that call a grab/retrieve method of a camera object (which also runs in a different thread) + * will have the empty frame that they passed in be put into the camera queues and then the grab/retrieve + * method will immediately return. This allows BOTH threads to continue processing other things or request + * multiple frame copies non-sequentially/serially. * * @tparam T - The mat type that the struct will be containing. * @@ -42,10 +82,9 @@ namespace containers { public: // Declare and define public struct member variables. - std::condition_variable cdMatWriteSuccess; - std::mutex muConditionMutex; - T& tFrame; + T* pFrame; PIXEL_FORMATS eFrameType; + std::shared_ptr> pCopiedFrameStatus; /****************************************************************************** * @brief Construct a new Frame Fetch Container object. @@ -57,13 +96,57 @@ namespace containers * @author ClayJay3 (claytonraycowen@gmail.com) * @date 2023-09-09 ******************************************************************************/ - FrameFetchContainer(T& tFrame, PIXEL_FORMATS eFrameType) : tFrame(tFrame), eFrameType(eFrameType) {} + FrameFetchContainer(T& tFrame, PIXEL_FORMATS eFrameType) : pFrame(&tFrame), eFrameType(eFrameType), pCopiedFrameStatus(std::make_shared>()) + {} + + /****************************************************************************** + * @brief Copy Construct a new Frame Fetch Container object. + * + * @param stOtherFrameContainer - FrameFetchContainer to copy pointers and values from. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-26 + ******************************************************************************/ + FrameFetchContainer(const FrameFetchContainer& stOtherFrameContainer) : + pFrame(stOtherFrameContainer.pFrame), eFrameType(stOtherFrameContainer.eFrameType), pCopiedFrameStatus(stOtherFrameContainer.pCopiedFrameStatus) + {} + + /****************************************************************************** + * @brief Operator equals for FrameFetchContainer. Shallow Copy. + * + * @param stOtherFrameContainer - FrameFetchContainer to copy pointers and values from. + * @return FrameFetchContainer& - A reference to this object. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-26 + ******************************************************************************/ + FrameFetchContainer& operator=(const FrameFetchContainer& stOtherFrameContainer) + { + // Check if the passed in container is the same as this one. + if (this != &stOtherFrameContainer) + { + // Copy struct attributes. + this->pFrame = stOtherFrameContainer.pFrame; + this->eFrameType = stOtherFrameContainer.eFrameType; + this->pCopiedFrameStatus = stOtherFrameContainer.pCopiedFrameStatus; + } + + // Return pointer to this object which now contains the copied values. + return *this; + } }; /****************************************************************************** - * @brief The struct is used to contain any datatype for scheduling and copying. - * It also stores a condition variable and mutex for signaling when the copy - * has been completed. + * @brief This struct is used to carry references to any datatype for scheduling and copying. + * It is constructed so that a reference to the data object is passed into the container and the pointer + * to that reference is stored internally. Then a shared_pointer to a new std::promise is created, + * which allows the programmer to return a future from whatever method created this container. + * The future can then be waited on before the data is used. The future will return a true or false. + * + * The idea is that any threads that call a grab/retrieve method of an object (which also runs in a different thread) + * will have the data reference that they passed in be put into the called objects queues and then the grab/retrieve + * method will immediately return. This allows BOTH threads to continue processing other things or request + * multiple data copies non-sequentially/serially. * * @tparam T - The type of data object that this struct will be containing. * @@ -75,9 +158,8 @@ namespace containers { public: // Declare and define public struct member variables. - std::condition_variable cdMatWriteSuccess; - std::mutex muConditionMutex; - T& tData; + T* pData; + std::shared_ptr> pCopiedDataStatus; /****************************************************************************** * @brief Construct a new Frame Fetch Container object. @@ -87,7 +169,42 @@ namespace containers * @author ClayJay3 (claytonraycowen@gmail.com) * @date 2023-09-09 ******************************************************************************/ - DataFetchContainer(T& tData) : tData(tData) {} + DataFetchContainer(T& tData) : pData(&tData), pCopiedDataStatus(std::make_shared>()) {} + + /****************************************************************************** + * @brief Copy Construct a new Frame Fetch Container object. + * + * @param stOtherDataContainer - DataFetchContainer to copy pointers and values from. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-26 + ******************************************************************************/ + DataFetchContainer(const DataFetchContainer& stOtherDataContainer) : + pData(stOtherDataContainer.pData), pCopiedDataStatus(stOtherDataContainer.pCopiedDataStatus) + {} + + /****************************************************************************** + * @brief Operator equals for FrameFetchContainer. Shallow Copy. + * + * @param stOtherDataContainer - DataFetchContainer to copy pointers and values from. + * @return DataFetchContainer& - A reference to this object. + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-26 + ******************************************************************************/ + DataFetchContainer& operator=(const DataFetchContainer& stOtherDataContainer) + { + // Check if the passed in container is the same as this one. + if (this != &stOtherDataContainer) + { + // Copy struct attributes. + this->pData = stOtherDataContainer.pData; + this->pCopiedDataStatus = stOtherDataContainer.pCopiedDataStatus; + } + + // Return pointer to this object which now contains the copied values. + return *this; + } }; } // namespace containers diff --git a/src/util/vision/ImageOperations.hpp b/src/util/vision/ImageOperations.hpp index 97ebb139..cb175652 100644 --- a/src/util/vision/ImageOperations.hpp +++ b/src/util/vision/ImageOperations.hpp @@ -46,6 +46,7 @@ namespace imgops case sl::MAT_TYPE::F32_C2: nOpenCVType = CV_32FC2; break; case sl::MAT_TYPE::F32_C3: nOpenCVType = CV_32FC3; break; case sl::MAT_TYPE::F32_C4: nOpenCVType = CV_32FC4; break; + case sl::MAT_TYPE::U16_C1: nOpenCVType = CV_16SC1; break; case sl::MAT_TYPE::U8_C1: nOpenCVType = CV_8UC1; break; case sl::MAT_TYPE::U8_C2: nOpenCVType = CV_8UC2; break; case sl::MAT_TYPE::U8_C3: nOpenCVType = CV_8UC3; break; @@ -78,6 +79,7 @@ namespace imgops case CV_32FC2: slMatType = sl::MAT_TYPE::F32_C2; break; case CV_32FC3: slMatType = sl::MAT_TYPE::F32_C3; break; case CV_32FC4: slMatType = sl::MAT_TYPE::F32_C4; break; + case CV_16SC1: slMatType = sl::MAT_TYPE::U16_C1; break; case CV_8UC1: slMatType = sl::MAT_TYPE::U8_C1; break; case CV_8UC2: slMatType = sl::MAT_TYPE::U8_C2; break; case CV_8UC3: slMatType = sl::MAT_TYPE::U8_C3; break; diff --git a/src/vision/cameras/BasicCam.cpp b/src/vision/cameras/BasicCam.cpp index be1d8aeb..bf54ab8b 100644 --- a/src/vision/cameras/BasicCam.cpp +++ b/src/vision/cameras/BasicCam.cpp @@ -22,6 +22,7 @@ * @param ePropPixelFormat - The pixel layout/format of the image. * @param dPropHorizontalFOV - The horizontal field of view. * @param dPropVerticalFOV - The vertical field of view. + * @param nNumFrameRetrievalThreads - The number of threads to use for frame queueing and copying. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-08-20 @@ -32,12 +33,14 @@ BasicCam::BasicCam(const std::string szCameraPath, const int nPropFramesPerSecond, const PIXEL_FORMATS ePropPixelFormat, const double dPropHorizontalFOV, - const double dPropVerticalFOV) : + const double dPropVerticalFOV, + const int nNumFrameRetrievalThreads) : Camera(nPropResolutionX, nPropResolutionY, nPropFramesPerSecond, ePropPixelFormat, dPropHorizontalFOV, dPropVerticalFOV) { // Assign member variables. - m_szCameraPath = szCameraPath; - m_nCameraIndex = -1; + m_szCameraPath = szCameraPath; + m_nCameraIndex = -1; + m_nNumFrameRetrievalThreads = nNumFrameRetrievalThreads; // Set flag specifying that the camera is located at a dev/video index. m_bCameraIsConnectedOnVideoIndex = false; @@ -66,6 +69,7 @@ BasicCam::BasicCam(const std::string szCameraPath, * @param ePropPixelFormat - The pixel layout/format of the image. * @param dPropHorizontalFOV - The horizontal field of view. * @param dPropVerticalFOV - The vertical field of view. + * @param nNumFrameRetrievalThreads - The number of threads to use for frame queueing and copying. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-08-20 @@ -76,12 +80,14 @@ BasicCam::BasicCam(const int nCameraIndex, const int nPropFramesPerSecond, const PIXEL_FORMATS ePropPixelFormat, const double dPropHorizontalFOV, - const double dPropVerticalFOV) : + const double dPropVerticalFOV, + const int nNumFrameRetrievalThreads) : Camera(nPropResolutionX, nPropResolutionY, nPropFramesPerSecond, ePropPixelFormat, dPropHorizontalFOV, dPropVerticalFOV) { // Assign member variables. - m_nCameraIndex = nCameraIndex; - m_szCameraPath = ""; + m_nCameraIndex = nCameraIndex; + m_szCameraPath = ""; + m_nNumFrameRetrievalThreads = nNumFrameRetrievalThreads; // Set flag specifying that the camera is located at a dev/video index. m_bCameraIsConnectedOnVideoIndex = true; @@ -154,6 +160,9 @@ void BasicCam::ThreadedContinuousCode() // Check if new frame was computed successfully. if (m_cvCamera.read(m_cvFrame)) { + // Resize the frame. + cv::resize(m_cvFrame, m_cvFrame, cv::Size(m_nPropResolutionX, m_nPropResolutionY), 0.0, 0.0, constants::BASICCAM_RESIZE_INTERPOLATION_METHOD); + // Call FPS tick. m_IPS.Tick(); } @@ -169,7 +178,7 @@ void BasicCam::ThreadedContinuousCode() if (!m_qFrameCopySchedule.empty()) { // Start the thread pool to store multiple copies of the sl::Mat into the given cv::Mats. - this->RunDetachedPool(m_qFrameCopySchedule.size(), constants::BASIC_LEFTCAM_FRAME_RETRIEVAL_THREADS); + this->RunDetachedPool(m_qFrameCopySchedule.size(), m_nNumFrameRetrievalThreads); // Wait for thread pool to finish. this->JoinPool(); // Release lock on frame copy queue. @@ -196,39 +205,34 @@ void BasicCam::PooledLinearCode() if (!m_qFrameCopySchedule.empty()) { // Get frame container out of queue. - containers::FrameFetchContainer& stContainer = m_qFrameCopySchedule.front(); + containers::FrameFetchContainer stContainer = m_qFrameCopySchedule.front(); // Pop out of queue. m_qFrameCopySchedule.pop(); // Release lock. lkFrameQueue.unlock(); - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); // Copy frame to data container. - stContainer.tFrame = m_cvFrame.clone(); - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); + *(stContainer.pFrame) = m_cvFrame.clone(); + // Signal future that the frame has been successfully retrieved. + stContainer.pCopiedFrameStatus->set_value(true); } } /****************************************************************************** - * @brief Retrieves a frame from the camera. The given mat reference is placed - * into a queue for copying. Remember, this code will be ran in whatever, - * class/thread calls it. + * @brief Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. + * Remember, this code will be ran in whatever, class/thread calls it. * * @param cvFrame - A reference to the cv::Mat to store the frame in. - * @return true - Frame successfully retrieved and stored. - * @return false - Frame was not stored successfully. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. * * @author ClayJay3 (claytonraycowen@gmail.com) * @date 2023-09-09 ******************************************************************************/ -bool BasicCam::GrabFrame(cv::Mat& cvFrame) +std::future BasicCam::RequestFrameCopy(cv::Mat& cvFrame) { // Assemble the FrameFetchContainer. - containers::FrameFetchContainer stContainer(cvFrame, m_ePropPixelFormat); + containers::FrameFetchContainer stContainer(cvFrame, m_ePropPixelFormat); // Acquire lock on frame copy queue. std::unique_lock lkScheduler(m_muPoolScheduleMutex); @@ -237,26 +241,8 @@ bool BasicCam::GrabFrame(cv::Mat& cvFrame) // Release lock on the frame schedule queue. lkScheduler.unlock(); - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving frame from threadpool queue."); - // Image was not written successfully. - return false; - } + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); } /****************************************************************************** diff --git a/src/vision/cameras/BasicCam.h b/src/vision/cameras/BasicCam.h index 499bef2d..d47c5ebb 100644 --- a/src/vision/cameras/BasicCam.h +++ b/src/vision/cameras/BasicCam.h @@ -12,12 +12,20 @@ #define BASICCAM_H #include -#include #include "../../interfaces/AutonomyThread.hpp" #include "../../interfaces/Camera.hpp" -#include "../../util/vision/FetchContainers.hpp" +/****************************************************************************** + * @brief This class implements and interfaces with the most common USB cameras + * and features. It is designed in such a way that multiple other classes/threads + * can safely call any method of an object of this class withing resource corruption + * or slowdown of the camera. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-21 + ******************************************************************************/ class BasicCam : public Camera, public AutonomyThread { public: @@ -30,16 +38,18 @@ class BasicCam : public Camera, public AutonomyThread const int nPropFramesPerSecond, const PIXEL_FORMATS ePropPixelFormat, const double dPropHorizontalFOV, - const double dPropVerticalFOV); + const double dPropVerticalFOV, + const int nNumFrameRetrievalThreads = 10); BasicCam(const int nCameraIndex, const int nPropResolutionX, const int nPropResolutionY, const int nPropFramesPerSecond, const PIXEL_FORMATS ePropPixelFormat, const double dPropHorizontalFOV, - const double dPropVerticalFOV); + const double dPropVerticalFOV, + const int nNumFrameRetrievalThreads = 10); ~BasicCam(); - bool GrabFrame(cv::Mat& cvFrame) override; + std::future RequestFrameCopy(cv::Mat& cvFrame) override; ///////////////////////////////////////// // Getters. @@ -57,15 +67,11 @@ class BasicCam : public Camera, public AutonomyThread std::string m_szCameraPath; bool m_bCameraIsConnectedOnVideoIndex; int m_nCameraIndex; + int m_nNumFrameRetrievalThreads; // Mats for storing frames. cv::Mat m_cvFrame; - // Queues and mutexes for scheduling and copying camera frames and data to other threads. - std::queue>> m_qFrameCopySchedule; - std::shared_mutex m_muPoolScheduleMutex; - std::mutex m_muFrameCopyMutex; - ///////////////////////////////////////// // Declare private methods. ///////////////////////////////////////// diff --git a/src/vision/cameras/ZEDCam.cpp b/src/vision/cameras/ZEDCam.cpp index 50eea642..7097c83a 100644 --- a/src/vision/cameras/ZEDCam.cpp +++ b/src/vision/cameras/ZEDCam.cpp @@ -51,9 +51,9 @@ struct ZEDCam::ZedObjectData struct Corner CornerTR; // The top right corner of the bounding box. struct Corner CornerBL; // The bottom left corner of the bounding box. struct Corner CornerBR; // The bottom right corner of bounding box. - int nClassNumber; // This info is passed through from your detection algorithm and will improve tracking be ensure the type of object remains the same. + int nClassNumber; // This info is passed through from your detection algorithm and will improve tracking be ensure the type of object remains the float fConfidence; // This info is passed through from your detection algorithm and will help improve tracking by throwing out bad detections. - // Whether of not this object remains on the floor plane. This parameter can't be changed for a given object tracking ID, it's advised to set it by class number + // Whether of not this object remains on the floor plane. This parameter can't be changed for a given object tracking ID, it's advised to set it by class // to avoid issues. bool bObjectRemainsOnFloorPlane = false; @@ -88,13 +88,15 @@ ZEDCam::ZEDCam(const int nPropResolutionX, const float fMaxSenseDistance, const bool bMemTypeGPU, const bool bUseHalfDepthPrecision, + const int nNumFrameRetrievalThreads, const unsigned int unCameraSerialNumber) : Camera(nPropResolutionX, nPropResolutionY, nPropFramesPerSecond, PIXEL_FORMATS::eZED, dPropHorizontalFOV, dPropVerticalFOV) { // Assign member variables. bMemTypeGPU ? m_slMemoryType = sl::MEM::GPU : m_slMemoryType = sl::MEM::CPU; bUseHalfDepthPrecision ? m_slDepthMeasureType = sl::MEASURE::DEPTH_U16_MM : m_slDepthMeasureType = sl::MEASURE::DEPTH; - m_unCameraSerialNumber = unCameraSerialNumber; + m_nNumFrameRetrievalThreads = nNumFrameRetrievalThreads; + m_unCameraSerialNumber = unCameraSerialNumber; // Setup camera params. m_slCameraParams.camera_resolution = constants::ZED_BASE_RESOLUTION; @@ -130,6 +132,17 @@ ZEDCam::ZEDCam(const int nPropResolutionX, m_slSpatialMappingParams.save_texture = true; m_slSpatialMappingParams.use_chunk_only = constants::ZED_MAPPING_USE_CHUNK_ONLY; m_slSpatialMappingParams.stability_counter = constants::ZED_MAPPING_STABILITY_COUNTER; + // Set or auto-set max depth range for mapping. + if (constants::ZED_MAPPING_RANGE_METER >= 0) + { + // Automatically guess the best mapping depth range. + m_slSpatialMappingParams.range_meter = m_slSpatialMappingParams.getRecommendedRange(constants::ZED_MAPPING_RESOLUTION_METER, m_slCamera); + } + else + { + // Manually set. + m_slSpatialMappingParams.range_meter = constants::ZED_MAPPING_RANGE_METER; + } // Setup object detection/tracking parameters. m_slObjectDetectionParams.detection_model = sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS; @@ -367,7 +380,7 @@ void ZEDCam::ThreadedContinuousCode() m_qObjectBatchedDataCopySchedule.size()}); // Start the thread pool to store multiple copies of the sl::Mat into the given cv::Mats. - this->RunDetachedPool(siMaxQueueLength, constants::ZED_MAINCAM_FRAME_RETRIEVAL_THREADS); + this->RunDetachedPool(siMaxQueueLength, m_nNumFrameRetrievalThreads); // Wait for thread pool to finish. this->JoinPool(); // Release lock on frame copy queue. @@ -379,7 +392,7 @@ void ZEDCam::ThreadedContinuousCode() /****************************************************************************** * @brief This method holds the code that is ran in the thread pool started by * the ThreadedLinearCode() method. It copies the data from the different - * data objects to references of the same type stored in a vector queued up by the + * data objects to references of the same type stored in a queue filled by the * Grab methods. * * @@ -400,63 +413,73 @@ void ZEDCam::PooledLinearCode() if (!m_qFrameCopySchedule.empty()) { // Get frame container out of queue. - containers::FrameFetchContainer& stContainer = m_qFrameCopySchedule.front(); + containers::FrameFetchContainer stContainer = m_qFrameCopySchedule.front(); // Pop out of queue. m_qFrameCopySchedule.pop(); // Release lock. lkFrameQueue.unlock(); - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Determine which frame should be copied. switch (stContainer.eFrameType) { - case eBGRA: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slFrame); break; - case eDepthMeasure: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slDepthMeasure); break; - case eDepthImage: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slDepthImage); break; - case eXYZ: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slPointCloud); break; - default: stContainer.tFrame = imgops::ConvertSLMatToCVMat(m_slFrame); break; + case eBGRA: *(stContainer.pFrame) = imgops::ConvertSLMatToCVMat(m_slFrame); break; + case eDepthMeasure: *(stContainer.pFrame) = imgops::ConvertSLMatToCVMat(m_slDepthMeasure); break; + case eDepthImage: *(stContainer.pFrame) = imgops::ConvertSLMatToCVMat(m_slDepthImage); break; + case eXYZ: *(stContainer.pFrame) = imgops::ConvertSLMatToCVMat(m_slPointCloud); break; + default: *(stContainer.pFrame) = imgops::ConvertSLMatToCVMat(m_slFrame); break; } - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); + // Signal future that the frame has been successfully retrieved. + stContainer.pCopiedFrameStatus->set_value(true); + } + + // Check if anything has been added to the GPU queue. + if (!m_qGPUFrameCopySchedule.empty()) + { + // Submit logger error. + LOG_ERROR(g_qSharedLogger, + "ZEDCam ({}) is in CPU sl::Mat mode but a GPU mat has been added to the copy queue! Whichever thread queued the frame will now appear frozen if " + "future.get() is called. Either switch the camera to GPU Mat mode in AutonomyConstants.h or stop queueing frames of type cv::Mat.", + m_unCameraSerialNumber); } } // Use GPU mat. else { + // Aqcuire mutex for getting frames out of the queue. + std::unique_lock lkFrameQueue(m_muFrameCopyMutex); // Check if the queue is empty. if (!m_qGPUFrameCopySchedule.empty()) { - // Aqcuire mutex for getting frames out of the queue. - std::unique_lock lkFrameQueue(m_muFrameCopyMutex); // Get frame container out of queue. - containers::FrameFetchContainer& stContainer = m_qGPUFrameCopySchedule.front(); + containers::FrameFetchContainer stContainer = m_qGPUFrameCopySchedule.front(); // Pop out of queue. m_qGPUFrameCopySchedule.pop(); // Release lock. lkFrameQueue.unlock(); - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Determine which frame should be copied. switch (stContainer.eFrameType) { - case eBGRA: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slFrame); break; - case eDepthMeasure: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slDepthMeasure); break; - case eDepthImage: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slDepthImage); break; - case eXYZ: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slPointCloud); break; - default: stContainer.tFrame = imgops::ConvertSLMatToGPUMat(m_slFrame); break; + case eBGRA: *(stContainer.pFrame) = imgops::ConvertSLMatToGPUMat(m_slFrame); break; + case eDepthMeasure: *(stContainer.pFrame) = imgops::ConvertSLMatToGPUMat(m_slDepthMeasure); break; + case eDepthImage: *(stContainer.pFrame) = imgops::ConvertSLMatToGPUMat(m_slDepthImage); break; + case eXYZ: *(stContainer.pFrame) = imgops::ConvertSLMatToGPUMat(m_slPointCloud); break; + default: *(stContainer.pFrame) = imgops::ConvertSLMatToGPUMat(m_slFrame); break; } - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); + // Signal future that the frame has been successfully retrieved. + stContainer.pCopiedFrameStatus->set_value(true); + } + + // Check if anything has been added to the GPU queue. + if (!m_qFrameCopySchedule.empty()) + { + // Submit logger error. + LOG_ERROR(g_qSharedLogger, + "ZEDCam ({}) is in GPU sl::Mat mode but a CPU mat has been added to the copy queue! Whichever thread queued the frame will now appear frozen if " + "future.get() is called. Either switch the camera to GPU Mat mode in AutonomyConstants.h or stop queueing frames of type cv::cuda::GpuMat.", + m_unCameraSerialNumber); } } @@ -469,20 +492,17 @@ void ZEDCam::PooledLinearCode() if (!m_qPoseCopySchedule.empty()) { // Get frame container out of queue. - containers::DataFetchContainer& stContainer = m_qPoseCopySchedule.front(); + containers::DataFetchContainer stContainer = m_qPoseCopySchedule.front(); // Pop out of queue. m_qPoseCopySchedule.pop(); // Release lock. lkPoseQueue.unlock(); - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); // Copy pose. - stContainer.tData = sl::Pose(m_slCameraPose); - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); + *(stContainer.pData) = sl::Pose(m_slCameraPose); + + // Signal future that the data has been successfully retrieved. + stContainer.pCopiedDataStatus->set_value(true); } ///////////////////////////// @@ -494,31 +514,28 @@ void ZEDCam::PooledLinearCode() if (!m_qIMUDataCopySchedule.empty()) { // Get frame container out of queue. - containers::DataFetchContainer&>& stContainer = m_qIMUDataCopySchedule.front(); + containers::DataFetchContainer> stContainer = m_qIMUDataCopySchedule.front(); // Pop out of queue. m_qIMUDataCopySchedule.pop(); // Release lock. lkIMUQueue.unlock(); - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Get IMU orientation in degrees. sl::float3 slAngles = m_slSensorData.imu.pose.getEulerAngles(false); // Get IMU linear acceleration. sl::float3 slLinearAccels = m_slSensorData.imu.linear_acceleration; + // Clear the data vector that the IMUData will be copied to. + stContainer.pData->clear(); // Repackage angles and accels into vector. - stContainer.tData.emplace_back(slAngles.x); - stContainer.tData.emplace_back(slAngles.y); - stContainer.tData.emplace_back(slAngles.z); - stContainer.tData.emplace_back(slLinearAccels.x); - stContainer.tData.emplace_back(slLinearAccels.y); - stContainer.tData.emplace_back(slLinearAccels.z); - - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); + stContainer.pData->emplace_back(slAngles.x); + stContainer.pData->emplace_back(slAngles.y); + stContainer.pData->emplace_back(slAngles.z); + stContainer.pData->emplace_back(slLinearAccels.x); + stContainer.pData->emplace_back(slLinearAccels.y); + stContainer.pData->emplace_back(slLinearAccels.z); + + // Signal future that the data has been successfully retrieved. + stContainer.pCopiedDataStatus->set_value(true); } ///////////////////////////// @@ -530,20 +547,17 @@ void ZEDCam::PooledLinearCode() if (!m_qObjectDataCopySchedule.empty()) { // Get frame container out of queue. - containers::DataFetchContainer&>& stContainer = m_qObjectDataCopySchedule.front(); + containers::DataFetchContainer> stContainer = m_qObjectDataCopySchedule.front(); // Pop out of queue. m_qObjectDataCopySchedule.pop(); // Release lock. lkIMUQueue.unlock(); - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); // Make copy of object vector. (Apparently the assignement operator actually does a deep copy) - stContainer.tData = m_slDetectedObjects.object_list; - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); + *(stContainer.pData) = m_slDetectedObjects.object_list; + + // Signal future that the data has been successfully retrieved. + stContainer.pCopiedDataStatus->set_value(true); } ///////////////////////////// @@ -555,38 +569,36 @@ void ZEDCam::PooledLinearCode() if (!m_qObjectBatchedDataCopySchedule.empty()) { // Get frame container out of queue. - containers::DataFetchContainer&>& stContainer = m_qObjectBatchedDataCopySchedule.front(); + containers::DataFetchContainer> stContainer = m_qObjectBatchedDataCopySchedule.front(); // Pop out of queue. m_qObjectBatchedDataCopySchedule.pop(); // Release lock. lkIMUQueue.unlock(); - // Acquire unique lock on container. - std::unique_lock lkConditionLock(stContainer.muConditionMutex); // Make copy of object vector. (Apparently the assignement operator actually does a deep copy) - stContainer.tData = m_slDetectedObjectsBatched; - // Release lock. - lkConditionLock.unlock(); - // Use the condition variable to notify other waiting threads that this thread is finished. - stContainer.cdMatWriteSuccess.notify_all(); + *(stContainer.pData) = m_slDetectedObjectsBatched; + + // Signal future that the data has been successfully retrieved. + stContainer.pCopiedDataStatus->set_value(true); } } /****************************************************************************** - * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera. + * @brief Requests a regular BGRA image from the LEFT eye of the zed camera. + * Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. * Remember this code will be ran in whatever class/thread calls it. * * @param cvFrame - A reference to the cv::Mat to copy the normal frame to. - * @return true - The frame was successfully copied. - * @return false - The frame was not copied successfully. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. * * @author ClayJay3 (claytonraycowen@gmail.com) * @date 2023-09-09 ******************************************************************************/ -bool ZEDCam::GrabFrame(cv::Mat& cvFrame) +std::future ZEDCam::RequestFrameCopy(cv::Mat& cvFrame) { // Assemble the FrameFetchContainer. - containers::FrameFetchContainer stContainer(cvFrame, eBGRA); + containers::FrameFetchContainer stContainer(cvFrame, eBGRA); // Acquire lock on frame copy queue. std::unique_lock lkSchedulers(m_muPoolScheduleMutex); @@ -595,44 +607,26 @@ bool ZEDCam::GrabFrame(cv::Mat& cvFrame) // Release lock on the frame schedule queue. lkSchedulers.unlock(); - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving frame from threadpool queue."); - // Image was not written successfully. - return false; - } + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); } /****************************************************************************** - * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera and - * stores it in a GPU mat. + * @brief Grabs a regular BGRA image from the LEFT eye of the zed camera and stores it in a GPU mat. + * Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. * Remember this code will be ran in whatever class/thread calls it. * * @param cvGPUFrame - A reference to the cv::Mat to copy the normal frame to. - * @return true - The frame was successfully copied. - * @return false - The frame was not copied successfully. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. * * @author ClayJay3 (claytonraycowen@gmail.com) * @date 2023-09-09 ******************************************************************************/ -bool ZEDCam::GrabFrame(cv::cuda::GpuMat& cvGPUFrame) +std::future ZEDCam::RequestFrameCopy(cv::cuda::GpuMat& cvGPUFrame) { // Assemble the FrameFetchContainer. - containers::FrameFetchContainer stContainer(cvGPUFrame, eBGRA); + containers::FrameFetchContainer stContainer(cvGPUFrame, eBGRA); // Acquire lock on frame copy queue. std::unique_lock lkSchedulers(m_muPoolScheduleMutex); @@ -641,43 +635,26 @@ bool ZEDCam::GrabFrame(cv::cuda::GpuMat& cvGPUFrame) // Release lock on the frame schedule queue. lkSchedulers.unlock(); - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving frame from threadpool queue."); - // Image was not written successfully. - return false; - } + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); } /****************************************************************************** - * @brief Grabs a depth measure or image from the camera. This image has the same shape as - * a grayscale image, but the values represent the depth in ZED_MEASURE_UNITS that is set in - * AutonomyConstants.h. + * @brief Requests a depth measure or image from the camera. + * Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. + * This image has the same shape as a grayscale image, but the values represent the depth in + * MILLIMETERS. The ZEDSDK will always return this measure in MILLIMETERS. * * @param cvDepth - A reference to the cv::Mat to copy the depth frame to. * @param bRetrieveMeasure - False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image * purposes other than displaying depth. - * @return true - The frame was successfully copied. - * @return false - The frame was not copied successfully. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-08-26 ******************************************************************************/ -bool ZEDCam::GrabDepth(cv::Mat& cvDepth, const bool bRetrieveMeasure) +std::future ZEDCam::RequestDepthCopy(cv::Mat& cvDepth, const bool bRetrieveMeasure) { // Create instance variables. PIXEL_FORMATS eFrameType; @@ -685,7 +662,7 @@ bool ZEDCam::GrabDepth(cv::Mat& cvDepth, const bool bRetrieveMeasure) // Check if the container should be set to retrieve an image or a measure. bRetrieveMeasure ? eFrameType = eDepthMeasure : eFrameType = eDepthImage; // Assemble container. - containers::FrameFetchContainer stContainer(cvDepth, eFrameType); + containers::FrameFetchContainer stContainer(cvDepth, eFrameType); // Acquire lock on frame copy queue. std::unique_lock lkSchedulers(m_muPoolScheduleMutex); @@ -694,43 +671,26 @@ bool ZEDCam::GrabDepth(cv::Mat& cvDepth, const bool bRetrieveMeasure) // Release lock on the frame schedule queue. lkSchedulers.unlock(); - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving depth from threadpool queue."); - // Image was not written successfully. - return false; - } + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); } /****************************************************************************** - * @brief Grabs a depth measure or image from the camera and stores it in GPU mat. This image has the same shape as - * a grayscale image, but the values represent the depth in ZED_MEASURE_UNITS that is set in - * AutonomyConstants.h. + * @brief Requests a depth measure or image from the camera. + * Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. + * This image has the same shape as a grayscale image, but the values represent the depth in + * MILLIMETERS. The ZEDSDK will always return this measure in MILLIMETERS. * * @param cvGPUDepth - A reference to the cv::Mat to copy the depth frame to. * @param bRetrieveMeasure - False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image * purposes other than displaying depth. - * @return true - The frame was successfully copied. - * @return false - The frame was not copied successfully. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-08-26 ******************************************************************************/ -bool ZEDCam::GrabDepth(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure) +std::future ZEDCam::RequestDepthCopy(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure) { // Create instance variables. PIXEL_FORMATS eFrameType; @@ -738,7 +698,7 @@ bool ZEDCam::GrabDepth(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure // Check if the container should be set to retrieve an image or a measure. bRetrieveMeasure ? eFrameType = eDepthMeasure : eFrameType = eDepthImage; // Assemble container. - containers::FrameFetchContainer stContainer(cvGPUDepth, eFrameType); + containers::FrameFetchContainer stContainer(cvGPUDepth, eFrameType); // Acquire lock on frame copy queue. std::unique_lock lkSchedulers(m_muPoolScheduleMutex); @@ -747,45 +707,29 @@ bool ZEDCam::GrabDepth(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure // Release lock on the frame schedule queue. lkSchedulers.unlock(); - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving depth from threadpool queue."); - // Image was not written successfully. - return false; - } + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); } /****************************************************************************** - * @brief Grabs a point cloud image from the camera. This image has the same resolution as a normal + * @brief Requests a point cloud image from the camera. This image has the same resolution as a normal * image but with three XYZ values replacing the old color values in the 3rd dimension. * The units and sign of the XYZ values are determined by ZED_MEASURE_UNITS and ZED_COORD_SYSTEM * constants set in AutonomyConstants.h. * + * Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. + * * @param cvPointCloud - A reference to the cv::Mat to copy the point cloud frame to. - * @return true - The frame was successfully copied. - * @return false - The frame was not copied successfully. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-08-26 ******************************************************************************/ -bool ZEDCam::GrabPointCloud(cv::Mat& cvPointCloud) +std::future ZEDCam::RequestPointCloudCopy(cv::Mat& cvPointCloud) { // Assemble the FrameFetchContainer. - containers::FrameFetchContainer stContainer(cvPointCloud, eXYZ); + containers::FrameFetchContainer stContainer(cvPointCloud, eXYZ); // Acquire lock on frame copy queue. std::unique_lock lkSchedulers(m_muPoolScheduleMutex); @@ -794,26 +738,8 @@ bool ZEDCam::GrabPointCloud(cv::Mat& cvPointCloud) // Release lock on the frame schedule queue. lkSchedulers.unlock(); - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving point cloud from threadpool queue."); - // Image was not written successfully. - return false; - } + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); } /****************************************************************************** @@ -822,17 +748,19 @@ bool ZEDCam::GrabPointCloud(cv::Mat& cvPointCloud) * The units and sign of the XYZ values are determined by ZED_MEASURE_UNITS and ZED_COORD_SYSTEM * constants set in AutonomyConstants.h. * + * Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it. + * * @param cvGPUPointCloud - A reference to the cv::Mat to copy the point cloud frame to. - * @return true - The frame was successfully copied. - * @return false - The frame was not copied successfully. + * @return std::future - A future that should be waited on before the passed in frame is used. + * Value will be true if frame was succesfully retrieved. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-08-26 ******************************************************************************/ -bool ZEDCam::GrabPointCloud(cv::cuda::GpuMat& cvGPUPointCloud) +std::future ZEDCam::RequestPointCloudCopy(cv::cuda::GpuMat& cvGPUPointCloud) { // Assemble the FrameFetchContainer. - containers::FrameFetchContainer stContainer(cvGPUPointCloud, eXYZ); + containers::FrameFetchContainer stContainer(cvGPUPointCloud, eXYZ); // Acquire lock on frame copy queue. std::unique_lock lkSchedulers(m_muPoolScheduleMutex); @@ -841,26 +769,8 @@ bool ZEDCam::GrabPointCloud(cv::cuda::GpuMat& cvGPUPointCloud) // Release lock on the frame schedule queue. lkSchedulers.unlock(); - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving point cloud from threadpool queue."); - // Image was not written successfully. - return false; - } + // Return the future from the promise stored in the container. + return stContainer.pCopiedFrameStatus->get_future(); } /****************************************************************************** @@ -1273,23 +1183,24 @@ unsigned int ZEDCam::GetCameraSerial() } /****************************************************************************** - * @brief Returns the current pose of the camera relative to it's start pose or the origin of the set pose. + * @brief Requests the current pose of the camera relative to it's start pose or the origin of the set pose. + * Puts a Pose pointer into a queue so a copy of a frame from the camera can be written to it. * If positional tracking is not enabled, this method will return false and the sl::Pose may be uninitialized. * * @param slPose - A reference to the sl::Pose object to copy the current camera pose to. - * @return true - Pose was successfully retrieved and copied. - * @return false - Pose was not successfully retrieved and/or copied. + * @return std::future - A future that should be waited on before the passed in sl::Pose is used. + * Value will be true if frame was succesfully retrieved. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-08-27 ******************************************************************************/ -bool ZEDCam::GetPositionalPose(sl::Pose& slPose) +std::future ZEDCam::RequestPositionalPoseCopy(sl::Pose& slPose) { // Check if positional tracking has been enabled. if (m_slCamera.isPositionalTrackingEnabled()) { // Assemble the data container. - containers::DataFetchContainer stContainer(slPose); + containers::DataFetchContainer stContainer(slPose); // Acquire lock on frame copy queue. std::unique_lock lkSchedulers(m_muPoolScheduleMutex); @@ -1298,33 +1209,22 @@ bool ZEDCam::GetPositionalPose(sl::Pose& slPose) // Release lock on the frame schedule queue. lkSchedulers.unlock(); - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving sl::Pose from threadpool queue."); - // Image was not written successfully. - return false; - } + // Return the future from the promise stored in the container. + return stContainer.pCopiedDataStatus->get_future(); } else { // Submit logger message. LOG_WARNING(g_qSharedLogger, "Attempted to get ZED positional pose but positional tracking is not enabled!"); + + // Create dummy promise to return the future. + std::promise pmDummyPromise; + std::future fuDummyFuture = pmDummyPromise.get_future(); + // Set future value. + pmDummyPromise.set_value(false); + // Return unsuccessful. - return false; + return fuDummyFuture; } } @@ -1343,20 +1243,21 @@ bool ZEDCam::GetPositionalTrackingEnabled() } /****************************************************************************** - * @brief Gets the IMU data from the ZED camera. If getting the data fails, the - * last successfully retrieved value is returned. + * @brief Requests the IMU data from the ZED camera. + * Puts a std::vector pointer into a queue so a copy of a frame from the camera can be written to it. + * If getting the data fails, the last successfully retrieved value is returned. * * @param vIMUValues - A 1x6 vector containing X_deg, Y_deg, Z_deg, X_liner_accel, Y_liner_accel, Z_liner_accel. - * @return true - The IMU vals were copied and stored successfully. - * @return false - The IMU vals were copied and stored successfully. + * @return std::future - A future that should be waited on before the passed in vector is used. + * Value will be true if frame was succesfully retrieved. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-08-27 ******************************************************************************/ -bool ZEDCam::GetIMUData(std::vector& vIMUValues) +std::future ZEDCam::RequestIMUDataCopy(std::vector& vIMUValues) { // Assemble the data container. - containers::DataFetchContainer&> stContainer(vIMUValues); + containers::DataFetchContainer> stContainer(vIMUValues); // Acquire lock on frame copy queue. std::unique_lock lkSchedulers(m_muPoolScheduleMutex); @@ -1365,26 +1266,8 @@ bool ZEDCam::GetIMUData(std::vector& vIMUValues) // Release lock on the frame schedule queue. lkSchedulers.unlock(); - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving IMU data from threadpool queue."); - // Image was not written successfully. - return false; - } + // Return the future from the promise stored in the container. + return stContainer.pCopiedDataStatus->get_future(); } /****************************************************************************** @@ -1485,23 +1368,23 @@ bool ZEDCam::GetObjectDetectionEnabled() } /****************************************************************************** - * @brief Accessor for getting the tracked objects from the camera. Current objects are copied to the - * given sl::ObjectData vector. + * @brief Requests a current copy of the tracked objects from the camera. + * Puts a pointer to a vector containing sl::ObjectData into a queue so a copy of a frame from the camera can be written to it. * * @param vObjectData - A vector that will have data copied to it containing sl::ObjectData objects. - * @return true - The object data was successfully copied. - * @return false - The object data was not successfully copied. + * @return std::future - A future that should be waited on before the passed in vector is used. + * Value will be true if frame was succesfully retrieved. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-08-27 ******************************************************************************/ -bool ZEDCam::GetObjects(std::vector& vObjectData) +std::future ZEDCam::RequestObjectsCopy(std::vector& vObjectData) { // Check if object detection has been enabled. if (m_slCamera.isObjectDetectionEnabled()) { // Assemble the data container. - containers::DataFetchContainer&> stContainer(vObjectData); + containers::DataFetchContainer> stContainer(vObjectData); // Acquire lock on frame copy queue. std::unique_lock lkSchedulers(m_muPoolScheduleMutex); @@ -1510,38 +1393,27 @@ bool ZEDCam::GetObjects(std::vector& vObjectData) // Release lock on the frame schedule queue. lkSchedulers.unlock(); - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving object data from threadpool queue."); - // Image was not written successfully. - return false; - } + // Return the future from the promise stored in the container. + return stContainer.pCopiedDataStatus->get_future(); } else { // Submit logger message. LOG_WARNING(g_qSharedLogger, "Attempted to get ZED object data but object detection/tracking is not enabled!"); + + // Create dummy promise to return the future. + std::promise pmDummyPromise; + std::future fuDummyFuture = pmDummyPromise.get_future(); + // Set future value. + pmDummyPromise.set_value(false); + // Return unsuccessful. - return false; + return fuDummyFuture; } } /****************************************************************************** - * @brief If batching is enabled, this retrieves the normal objects and passes them to + * @brief If batching is enabled, this requests the normal objects and passes them to * the the iternal batching queue of the zed api. This performs short-term re-identification * with deep learning and trajectories filtering. Batching must have been set to enabled when * EnableObjectDetection() was called. Most of the time the vector will be empty and will be @@ -1549,19 +1421,19 @@ bool ZEDCam::GetObjects(std::vector& vObjectData) * * @param vBatchedObjectData - A vector containing objects of sl::ObjectsBatch object that will * have object data copied to. - * @return true - The batched objects data was successfully copied. - * @return false - The batched objects data was not successfully copied. + * @return std::future - A future that should be waited on before the passed in vector is used. + * Value will be true if frame was succesfully retrieved. * * @author clayjay3 (claytonraycowen@gmail.com) * @date 2023-08-30 ******************************************************************************/ -bool ZEDCam::GetBatchedObjects(std::vector& vBatchedObjectData) +std::future ZEDCam::RequestBatchedObjectsCopy(std::vector& vBatchedObjectData) { // Check if object detection and batching has been enabled. if (m_slCamera.isObjectDetectionEnabled() && m_slObjectDetectionBatchParams.enable) { // Assemble the data container. - containers::DataFetchContainer&> stContainer(vBatchedObjectData); + containers::DataFetchContainer> stContainer(vBatchedObjectData); // Acquire lock on frame copy queue. std::unique_lock lkSchedulers(m_muPoolScheduleMutex); @@ -1570,32 +1442,21 @@ bool ZEDCam::GetBatchedObjects(std::vector& vBatchedObjectData // Release lock on the frame schedule queue. lkSchedulers.unlock(); - // Create lock variable to be used by condition variable. CV unlocks this during wait(). - std::unique_lock lkConditionLock(stContainer.muConditionMutex); - // Wait up to 10 seconds for the condition variable to be notified. - std::cv_status cdStatus = stContainer.cdMatWriteSuccess.wait_for(lkConditionLock, std::chrono::seconds(10)); - // Release lock. - lkConditionLock.unlock(); - - // Check condition variable status and return. - if (cdStatus == std::cv_status::no_timeout) - { - // Image was successfully written to the given cv::Mat reference. - return true; - } - else - { - // Submit logger message. - LOG_WARNING(g_qSharedLogger, "Reached timeout while retrieving batched object data from threadpool queue."); - // Image was not written successfully. - return false; - } + // Return the future from the promise stored in the container. + return stContainer.pCopiedDataStatus->get_future(); } else { // Submit logger message. LOG_WARNING(g_qSharedLogger, "Attempted to get ZED batched object data but object detection/tracking is not enabled!"); + + // Create dummy promise to return the future. + std::promise pmDummyPromise; + std::future fuDummyFuture = pmDummyPromise.get_future(); + // Set future value. + pmDummyPromise.set_value(false); + // Return unsuccessful. - return false; + return fuDummyFuture; } } diff --git a/src/vision/cameras/ZEDCam.h b/src/vision/cameras/ZEDCam.h index 279641d7..3aac8a9a 100644 --- a/src/vision/cameras/ZEDCam.h +++ b/src/vision/cameras/ZEDCam.h @@ -13,14 +13,22 @@ #include #include -#include #include #include "../../AutonomyConstants.h" #include "../../interfaces/AutonomyThread.hpp" #include "../../interfaces/Camera.hpp" -#include "../../util/vision/FetchContainers.hpp" +/****************************************************************************** + * @brief This class implements and interfaces with the most common ZEDSDK cameras + * and features. It is designed in such a way that multiple other classes/threads + * can safely call any method of an object of this class withing resource corruption + * or slowdown of the camera. + * + * + * @author clayjay3 (claytonraycowen@gmail.com) + * @date 2023-09-21 + ******************************************************************************/ class ZEDCam : public Camera, public AutonomyThread { public: @@ -41,14 +49,15 @@ class ZEDCam : public Camera, public AutonomyThread const float fMaxSenseDistance = constants::ZED_DEFAULT_MAXIMUM_DISTANCE, const bool bMemTypeGPU = false, const bool bUseHalfDepthPrecision = false, + const int nNumFrameRetrievalThreads = 10, const unsigned int unCameraSerialNumber = 0); ~ZEDCam(); - bool GrabFrame(cv::Mat& cvFrame) override; - bool GrabFrame(cv::cuda::GpuMat& cvGPUFrame); - bool GrabDepth(cv::Mat& cvDepth, const bool bRetrieveMeasure = true); - bool GrabDepth(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure = true); - bool GrabPointCloud(cv::Mat& cvPointCloud); - bool GrabPointCloud(cv::cuda::GpuMat& cvGPUPointCloud); + std::future RequestFrameCopy(cv::Mat& cvFrame) override; + std::future RequestFrameCopy(cv::cuda::GpuMat& cvGPUFrame); + std::future RequestDepthCopy(cv::Mat& cvDepth, const bool bRetrieveMeasure = true); + std::future RequestDepthCopy(cv::cuda::GpuMat& cvGPUDepth, const bool bRetrieveMeasure = true); + std::future RequestPointCloudCopy(cv::Mat& cvPointCloud); + std::future RequestPointCloudCopy(cv::cuda::GpuMat& cvGPUPointCloud); sl::ERROR_CODE ResetPositionalTracking(); sl::ERROR_CODE TrackCustomBoxObjects(std::vector& vCustomObjects); sl::ERROR_CODE RebootCamera(); @@ -71,14 +80,14 @@ class ZEDCam : public Camera, public AutonomyThread bool GetUsingGPUMem() const; std::string GetCameraModel(); unsigned int GetCameraSerial(); - bool GetPositionalPose(sl::Pose& slPose); + std::future RequestPositionalPoseCopy(sl::Pose& slPose); bool GetPositionalTrackingEnabled(); - bool GetIMUData(std::vector& vIMUValues); + std::future RequestIMUDataCopy(std::vector& vIMUValues); sl::SPATIAL_MAPPING_STATE GetSpatialMappingState(); sl::SPATIAL_MAPPING_STATE ExtractSpatialMapAsync(std::future& fuMeshFuture); bool GetObjectDetectionEnabled(); - bool GetObjects(std::vector& vObjectData); - bool GetBatchedObjects(std::vector& vBatchedObjectData); + std::future RequestObjectsCopy(std::vector& vObjectData); + std::future RequestBatchedObjectsCopy(std::vector& vBatchedObjectData); private: ///////////////////////////////////////// @@ -99,6 +108,7 @@ class ZEDCam : public Camera, public AutonomyThread sl::Objects m_slDetectedObjects; std::vector m_slDetectedObjectsBatched; sl::MEM m_slMemoryType; + int m_nNumFrameRetrievalThreads; unsigned int m_unCameraSerialNumber; // Mats for storing frames and measures. @@ -108,15 +118,12 @@ class ZEDCam : public Camera, public AutonomyThread sl::Mat m_slPointCloud; // Queues and mutexes for scheduling and copying camera frames and data to other threads. - std::queue>> m_qFrameCopySchedule; - std::queue>> m_qGPUFrameCopySchedule; - std::queue&>>> m_qCustomBoxInjestSchedule; - std::queue>> m_qPoseCopySchedule; - std::queue&>>> m_qIMUDataCopySchedule; - std::queue&>>> m_qObjectDataCopySchedule; - std::queue&>>> m_qObjectBatchedDataCopySchedule; - std::shared_mutex m_muPoolScheduleMutex; - std::mutex m_muFrameCopyMutex; + std::queue> m_qGPUFrameCopySchedule; + std::queue>> m_qCustomBoxInjestSchedule; + std::queue> m_qPoseCopySchedule; + std::queue>> m_qIMUDataCopySchedule; + std::queue>> m_qObjectDataCopySchedule; + std::queue>> m_qObjectBatchedDataCopySchedule; std::mutex m_muCustomBoxInjestMutex; std::mutex m_muPoseCopyMutex; std::mutex m_muIMUDataCopyMutex; diff --git a/tools/valgrind/run_valgrind.sh b/tools/valgrind/run_valgrind.sh index 29934a96..4eb29821 100755 --- a/tools/valgrind/run_valgrind.sh +++ b/tools/valgrind/run_valgrind.sh @@ -23,7 +23,7 @@ cp /usr/local/share/opencv*/*.supp $script_dir supp_files=$(find "$script_dir" -maxdepth 1 -type f -name "*.supp") # Construct the Valgrind command -valgrind_cmd="valgrind -s --leak-check=yes --show-leak-kinds=all " +valgrind_cmd="valgrind -s --leak-check=yes --show-leak-kinds=all --track-origins=yes " if [ "$1" = "GitHub-Action" ]; then valgrind_cmd+=" --log-file=/opt/Autonomy_Software/tools/valgrind/valgrind.rpt" fi