This class implements and interfaces with the SIM cameras and data. 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. More...

#include <SIMZEDCam.h>

Inheritance diagram for SIMZEDCam:

[legend]

Collaboration diagram for SIMZEDCam:

[legend]

Public Member Functions
	SIMZEDCam (const std::string szCameraPath, const int nPropResolutionX, const int nPropResolutionY, const int nPropFramesPerSecond, const double dPropHorizontalFOV, const double dPropVerticalFOV, const bool bEnableRecordingFlag, const int nNumFrameRetrievalThreads=10, const unsigned int unCameraSerialNumber=0)
	Construct a new SIM Cam:: SIM Cam object.

	~SIMZEDCam ()
	Destroy the SIM Cam:: SIM Cam object.

std::future< bool >	RequestFrameCopy (cv::Mat &cvFrame) override
	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.

std::future< bool >	RequestDepthCopy (cv::Mat &cvDepth, const bool bRetrieveMeasure=true)
	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.

std::future< bool >	RequestPointCloudCopy (cv::Mat &cvPointCloud)
	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.

std::future< bool >	RequestPositionalPoseCopy (Pose &stPose) override
	Puts a sl::GeoPose pointer into a queue so a copy of a GeoPose from the camera can be written to it.

std::future< bool >	RequestFusionGeoPoseCopy (sl::GeoPose &slGeoPose) override
	Puts a sl::GeoPose pointer into a queue so a copy of a GeoPose from the camera can be written to it.

std::future< bool >	RequestSensorsCopy (sl::SensorsData &slSensorsData) override
	Requests a copy of the sensors data from the camera.

sl::ERROR_CODE	ResetPositionalTracking () override
	This method is used to reset the positional tracking of the camera. Because this is a simulation camera, and then ZEDSDK is not available, this method will just reset the offsets to zero.

sl::ERROR_CODE	RebootCamera () override
	This method is used to reboot the camera. This method will stop the camera thread, join the camera thread, destroy the camera stream objects, reconstruct the camera stream objects, set the frame callbacks, and restart the camera thread. This simulates a camera reboot since the ZED SDK is not available.

sl::FUSION_ERROR_CODE	SubscribeFusionToCameraUUID (sl::CameraIdentifier &slCameraUUID) override
	This method is used to subscribe the sl::Fusion object to the camera with the given UUID. Given that this is a simulation camera, and the ZED SDK is not available, this method will do nothing.

sl::CameraIdentifier	PublishCameraToFusion () override
	This method is used to publish the camera to the sl::Fusion object. Since this is a simulation camera this will do nothing.

sl::ERROR_CODE	EnablePositionalTracking (const float fExpectedCameraHeightFromFloorTolerance=constants::ZED_DEFAULT_FLOOR_PLANE_ERROR) override
	This method is used to enable positional tracking on the camera. Since this is a simulation camera, this method will just set the member variable and then use the NavBoard to simulate the camera's position.

void	DisablePositionalTracking () override
	This method is used to disable positional tracking on the camera. Since this is a simulation camera, this method will just set the member variable.

void	SetPositionalPose (const double dX, const double dY, const double dZ, const double dXO, const double dYO, const double dZO) override
	This method is used to set the positional pose of the camera. Since this is a simulation camera, this method will just set the offset member variables.

bool	GetCameraIsOpen () override
	Accessor for the camera open status.

bool	GetUsingGPUMem () const override
	Returns if the camera is using GPU memory. This is a simulation camera, so this method will always return false.

std::string	GetCameraModel () override
	Accessor for the name of this model of camera.

bool	GetPositionalTrackingEnabled () override
	Accessor for the if the camera's positional tracking is enabled. Since this is a simulation camera, this method will just return the member variable.

Public Member Functions inherited from ZEDCamera
	ZEDCamera (const int nPropResolutionX, const int nPropResolutionY, const int nPropFramesPerSecond, const double dPropHorizontalFOV, const double dPropVerticalFOV, const bool bEnableRecordingFlag, const bool bMemTypeGPU, const bool bUseHalfDepthPrecision, const bool bEnableFusionMaster, const int nNumFrameRetrievalThreads, const unsigned int unCameraSerialNumber)
	Construct a new ZEDCamera object.

virtual	~ZEDCamera ()=default
	Destroy the ZEDCamera object.

virtual std::future< bool >	RequestFrameCopy (cv::cuda::GpuMat &cvGPUFrame)
	Puts a frame pointer into a queue so a copy of a frame from the camera can be written to it.

virtual std::future< bool >	RequestDepthCopy (cv::cuda::GpuMat &cvGPUDepth, const bool bRetrieveMeasure=true)
	Puts a frame pointer into a queue so a copy of a depth frame from the camera can be written to it.

virtual std::future< bool >	RequestPointCloudCopy (cv::cuda::GpuMat &cvGPUPointCloud)
	Puts a frame pointer into a queue so a copy of a point cloud from the camera can be written to it.

virtual std::future< bool >	RequestFloorPlaneCopy (sl::Plane &slFloorPlane)
	Puts a FloorPlane pointer into a queue so a copy of a FloorPlane from the camera can be written to it.

virtual std::future< bool >	RequestObjectsCopy (std::vector< sl::ObjectData > &vObjectData)
	Puts a vector of ObjectData pointers into a queue so a copy of a vector of ObjectData from the camera can be written to it.

virtual std::future< bool >	RequestBatchedObjectsCopy (std::vector< sl::ObjectsBatch > &vBatchedObjectData)
	Puts a vector of ObjectsBatch pointers into a queue so a copy of a vector of ObjectsBatch from the camera can be written to it.

virtual sl::ERROR_CODE	TrackCustomBoxObjects (std::vector< ZedObjectData > &vCustomObjects)
	Tracks custom bounding boxes in the camera's field of view.

virtual sl::FUSION_ERROR_CODE	IngestGPSDataToFusion (geoops::GPSCoordinate stNewGPSLocation)
	Ingests GPS data into the fusion object.

virtual sl::ERROR_CODE	EnableSpatialMapping ()
	Enables spatial mapping.

virtual void	DisableSpatialMapping ()
	Disables spatial mapping.

virtual sl::ERROR_CODE	EnableObjectDetection (const bool bEnableBatching=false)
	Enables object detection.

virtual void	DisableObjectDetection ()
	Disables object detection.

virtual bool	GetIsFusionMaster () const
	Accessor for if this ZED is running a fusion instance.

virtual unsigned int	GetCameraSerial ()
	Accessor for the Camera Serial private member.

virtual sl::PositionalTrackingStatus	GetPositionalTrackingState ()
	Accessor for the Positional Tracking State private member.

virtual sl::FusedPositionalTrackingStatus	GetFusedPositionalTrackingState ()
	Accessor for the Fused Positional Tracking State private member.

virtual sl::SPATIAL_MAPPING_STATE	GetSpatialMappingState ()
	Accessor for the Spatial Mapping State private member.

virtual sl::SPATIAL_MAPPING_STATE	ExtractSpatialMapAsync (std::future< sl::Mesh > &fuMeshFuture)
	Puts a Mesh pointer into a queue so a copy of a spatial mapping mesh from the camera can be written to it.

virtual bool	GetObjectDetectionEnabled ()
	Accessor for the Object Detection Enabled private member.

Public Member Functions inherited from Camera< cv::Mat >
	Camera (const int nPropResolutionX, const int nPropResolutionY, const int nPropFramesPerSecond, const PIXEL_FORMATS ePropPixelFormat, const double dPropHorizontalFOV, const double dPropVerticalFOV, const bool bEnableRecordingFlag, const int nNumFrameRetrievalThreads=5)
	Construct a new Camera object.

virtual	~Camera ()
	Destroy the Camera object.

cv::Size	GetPropResolution () const
	Accessor for the Prop Resolution private member.

int	GetPropFramesPerSecond () const
	Accessor for the Prop Frames Per Second private member.

PIXEL_FORMATS	GetPropPixelFormat () const
	Accessor for the Prop Pixel Format private member.

double	GetPropHorizontalFOV () const
	Accessor for the Prop Horizontal F O V private member.

double	GetPropVerticalFOV () const
	Accessor for the Prop Vertical F O V private member.

bool	GetEnableRecordingFlag () const
	Accessor for the Enable Recording Flag private member.

void	SetEnableRecordingFlag (const bool bEnableRecordingFlag)
	Mutator for the Enable Recording Flag private member.

Public Member Functions inherited from AutonomyThread< void >
	AutonomyThread ()
	Construct a new Autonomy Thread object.

virtual	~AutonomyThread ()
	Destroy the Autonomy Thread object. If the parent object or main thread is destroyed or exited while this thread is still running, a race condition will occur. Stopping and joining the thread here insures that the main program can't exit if the user forgot to stop and join the thread.

void	Start ()
	When this method is called, it starts a new thread that runs the code within the ThreadedContinuousCode method. This is the users main code that will run the important and continuous code for the class.

void	RequestStop ()
	Signals threads to stop executing user code, terminate. DOES NOT JOIN. This method will not force the thread to exit, if the user code is not written properly and contains WHILE statement or any other long-executing or blocking code, then the thread will not exit until the next iteration.

void	Join ()
	Waits for thread to finish executing and then closes thread. This method will block the calling code until thread is finished.

bool	Joinable () const
	Check if the code within the thread and all pools created by it are finished executing and the thread is ready to be closed.

AutonomyThreadState	GetThreadState () const
	Accessor for the Threads State private member.

IPS &	GetIPS ()
	Accessor for the Frame I P S private member.

Private Member Functions
void	ThreadedContinuousCode () override
	The code inside this private method runs in a separate thread, but still has access to this*. This method continuously get new frames from the OpenCV VideoCapture object and stores it in a member variable. Then a thread pool is started and joined once per iteration to mass copy the frames and/or measure to any other thread waiting in the queues.

void	PooledLinearCode () override
	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 Grab methods.

void	SetCallbacks ()
	This method sets the callbacks for the WebRTC connections.

void	EstimateDepthMeasure (const cv::Mat &cvDepthImage, cv::Mat &cvDepthMeasure)
	This method estimates the depth measure from the depth image.

void	CalculatePointCloud (const cv::Mat &cvDepthMeasure, cv::Mat &cvPointCloud)
	This method calculates a point cloud from the decoded depth measure use some simple trig and the camera FOV.

Private Attributes
const std::function< void(const rovecomm::RoveCommPacket< double > &, const sockaddr_in &)>	ProcessIMUData
	Callback function to process incoming IMU data from RoveComm for the SIM ZED Camera. Normally, this data would come from the physical ZED camera's IMU over USB,.

std::string	m_szCameraPath

std::atomic< bool >	m_bCameraPositionalTrackingEnabled

sl::SensorsData	m_stIMUData

std::unique_ptr< WebRTC >	m_pRGBStream

std::unique_ptr< WebRTC >	m_pDepthImageStream

double	m_dPoseOffsetX

double	m_dPoseOffsetY

double	m_dPoseOffsetZ

double	m_dPoseOffsetXO

double	m_dPoseOffsetYO

double	m_dPoseOffsetZO

geoops::RoverPose	m_stCurrentRoverPose

std::shared_mutex	m_muCurrentRoverPoseMutex

cv::Mat	m_cvFrame

cv::Mat	m_cvDepthImageBuffer

cv::Mat	m_cvDepthImage

cv::Mat	m_cvDepthMeasure

cv::Mat	m_cvPointCloud

std::queue< containers::DataFetchContainer< Pose > >	m_qPoseCopySchedule

std::queue< containers::DataFetchContainer< sl::GeoPose > >	m_qGeoPoseCopySchedule

std::queue< containers::DataFetchContainer< sl::SensorsData > >	m_qSensorsCopySchedule

std::shared_mutex	m_muWebRTCRGBImageCopyMutex

std::shared_mutex	m_muWebRTCDepthImageCopyMutex

std::shared_mutex	m_muPoseCopyMutex

std::shared_mutex	m_muGeoPoseCopyMutex

std::shared_mutex	m_muSensorsCopyMutex

bool	m_bQueueTogglesAlreadyReset

std::atomic< bool >	m_bPosesQueued

std::atomic< bool >	m_bGeoPosesQueued

std::atomic< bool >	m_bSensorsQueued

Additional Inherited Members
Public Types inherited from AutonomyThread< void >
enum	AutonomyThreadState

Protected Member Functions inherited from AutonomyThread< void >
void	RunPool (const unsigned int nNumTasksToQueue, const unsigned int nNumThreads=2, const bool bForceStopCurrentThreads=false)
	When this method is called, it starts/adds tasks to a thread pool that runs nNumTasksToQueue copies of the code within the PooledLinearCode() method using nNumThreads number of threads. This is meant to be used as an internal utility of the child class to further improve parallelization. Default value for nNumThreads is 2.

void	RunDetachedPool (const unsigned int nNumTasksToQueue, const unsigned int nNumThreads=2, const bool bForceStopCurrentThreads=false)
	When this method is called, it starts a thread pool full of threads that don't return std::futures (like a placeholder for the thread return type). This means the thread will not have a return type and there is no way to determine if the thread has finished other than calling the Join() method. Only use this if you want to 'set and forget'. It will be faster as it doesn't return futures. Runs PooledLinearCode() method code. This is meant to be used as an internal utility of the child class to further improve parallelization.

void	ParallelizeLoop (const int nNumThreads, const N tTotalIterations, F &&tLoopFunction)
	Given a ref-qualified looping function and an arbitrary number of iterations, this method will divide up the loop and run each section in a thread pool. This function must not return anything. This method will block until the loop has completed.

void	ClearPoolQueue ()
	Clears any tasks waiting to be ran in the queue, tasks currently running will remain running.

void	JoinPool ()
	Waits for pool to finish executing tasks. This method will block the calling code until thread is finished.

bool	PoolJoinable () const
	Check if the internal pool threads are done executing code and the queue is empty.

void	SetMainThreadIPSLimit (int nMaxIterationsPerSecond=0)
	Mutator for the Main Thread Max I P S private member.

int	GetPoolNumOfThreads ()
	Accessor for the Pool Num Of Threads private member.

int	GetPoolQueueLength ()
	Accessor for the Pool Queue Size private member.

std::vector< void >	GetPoolResults ()
	Accessor for the Pool Results private member. The action of getting results will destroy and remove them from this object. This method blocks if the thread is not finished, so no need to call JoinPool() before getting results.

int	GetMainThreadMaxIPS () const
	Accessor for the Main Thread Max I P S private member.

Protected Attributes inherited from ZEDCamera
const std::memory_order	ATOMIC_MEMORY_ORDER_METHOD = std::memory_order_relaxed

unsigned int	m_unCameraSerialNumber

bool	m_bCameraIsFusionMaster

Protected Attributes inherited from Camera< cv::Mat >
int	m_nPropResolutionX

int	m_nPropResolutionY

int	m_nPropFramesPerSecond

int	m_nNumFrameRetrievalThreads

PIXEL_FORMATS	m_ePropPixelFormat

double	m_dPropHorizontalFOV

double	m_dPropVerticalFOV

std::atomic_bool	m_bEnableRecordingFlag

std::queue< containers::FrameFetchContainer< cv::Mat > >	m_qFrameCopySchedule

std::shared_mutex	m_muPoolScheduleMutex

std::shared_mutex	m_muFrameCopyMutex

Protected Attributes inherited from AutonomyThread< void >
IPS	m_IPS

Detailed Description

This class implements and interfaces with the SIM cameras and data. 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 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2023-09-30

Constructor & Destructor Documentation

◆ SIMZEDCam()

SIMZEDCam::SIMZEDCam	(	const std::string	szCameraPath,
		const int	nPropResolutionX,
		const int	nPropResolutionY,
		const int	nPropFramesPerSecond,
		const double	dPropHorizontalFOV,
		const double	dPropVerticalFOV,
		const bool	bEnableRecordingFlag,
		const int	nNumFrameRetrievalThreads = `10`,
		const unsigned int	unCameraSerialNumber = `0`
	)

Construct a new SIM Cam:: SIM Cam object.

Parameters

szCameraPath	- The file path to the camera hardware.
nPropResolutionX	- X res of camera.
nPropResolutionY	- Y res of camera.
nPropFramesPerSecond	- FPS camera is running at.
ePropPixelFormat	- The pixel layout/format of the image.
dPropHorizontalFOV	- The horizontal field of view.
dPropVerticalFOV	- The vertical field of view.
bEnableRecordingFlag	- Whether or not this camera should be recorded.
nNumFrameRetrievalThreads	- The number of threads to use for frame queueing and copying.
unCameraSerialNumber	- The serial number of the camera.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2023-09-30

                                                              :
    ZEDCamera(nPropResolutionX,
              nPropResolutionY,
              nPropFramesPerSecond,
              dPropHorizontalFOV,
              dPropVerticalFOV,
              bEnableRecordingFlag,
              false,
              false,
              false,
              nNumFrameRetrievalThreads,
              unCameraSerialNumber)
{
    // Assign member variables.
    m_szCameraPath              = szCameraPath;
    m_nNumFrameRetrievalThreads = nNumFrameRetrievalThreads;
    m_bQueueTogglesAlreadyReset = false;
 
    // Initialize OpenCV mats to a black/empty image the size of the camera resolution.
    m_cvFrame        = cv::Mat::zeros(nPropResolutionY, nPropResolutionX, CV_8UC4);
    m_cvDepthImage   = cv::Mat::zeros(nPropResolutionY, nPropResolutionX, CV_8UC1);
    m_cvDepthMeasure = cv::Mat::zeros(nPropResolutionY, nPropResolutionX, CV_32FC1);
    m_cvPointCloud   = cv::Mat::zeros(nPropResolutionY, nPropResolutionX, CV_32FC4);
 
    // Construct camera stream objects. Append proper camera path arguments to each URL camera path.
    m_pRGBStream        = std::make_unique<WebRTC>(szCameraPath, "ZEDFrontRGB");
    m_pDepthImageStream = std::make_unique<WebRTC>(szCameraPath, "ZEDFrontDepthImage");
 
    // Set callbacks for the WebRTC connections.
    this->SetCallbacks();
 
    // Subscribe to RoveSoSimulator packets.
    rovecomm::RoveCommPacket<u_int8_t> stSubscribePacket;
    stSubscribePacket.unDataId    = manifest::System::SUBSCRIBE_DATA_ID;
    stSubscribePacket.unDataCount = 0;
    stSubscribePacket.eDataType   = manifest::DataTypes::UINT8_T;
    stSubscribePacket.vData       = std::vector<uint8_t>{};
    // Set RoveComm callbacks for the data from the sim.
    if (network::g_pRoveCommUDPNode)
    {
        // Determine the IP address to send the subscribe packet to.
        const char* cIPAddress = constants::MODE_SIM ? constants::SIM_IP_ADDRESS.c_str() : manifest::RoveSoSimulator::IP_ADDRESS.IP_STR.c_str();
 
        // Send subscribe packet to RoveSoSimulator.
        network::g_pRoveCommUDPNode->SendUDPPacket(stSubscribePacket, cIPAddress, constants::ROVECOMM_OUTGOING_UDP_PORT);
 
        // Set RoveComm callbacks.
        network::g_pRoveCommUDPNode->AddUDPCallback<double>(ProcessIMUData, manifest::RoveSoSimulator::TELEMETRY.find("IMU")->second.DATA_ID);
    }
 
    // Set max FPS of the ThreadedContinuousCode method.
    this->SetMainThreadIPSLimit(nPropFramesPerSecond);
}

Here is the call graph for this function:

◆ ~SIMZEDCam()

SIMZEDCam::~SIMZEDCam ( )

Destroy the SIM Cam:: SIM Cam object.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2023-09-30

{
    // Close the WebRTC connections.
    if (m_pRGBStream)
    {
        m_pRGBStream->CloseConnection();
    }
    if (m_pDepthImageStream)
    {
        m_pDepthImageStream->CloseConnection();
    }
 
    // Stop threaded code.
    this->RequestStop();
    this->Join();
}

Here is the call graph for this function:

Member Function Documentation

◆ RequestFrameCopy()

std::future< bool > SIMZEDCam::RequestFrameCopy ( cv::Mat & cvFrame )

overridevirtual

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.

Parameters

cvFrame - A reference to the cv::Mat to store the frame in.

Returns: std::future<bool> - A future that should be waited on before the passed in frame is used. Value will be true if frame was successfully retrieved.

Author: ClayJay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2023-09-30

Implements ZEDCamera.

{
    // Assemble the FrameFetchContainer.
    containers::FrameFetchContainer<cv::Mat> stContainer(cvFrame, m_ePropPixelFormat);
 
    // Acquire lock on frame copy queue.
    std::unique_lock<std::shared_mutex> lkScheduler(m_muPoolScheduleMutex);
    // Append frame fetch container to the schedule queue.
    m_qFrameCopySchedule.push(stContainer);
    // Release lock on the frame schedule queue.
    lkScheduler.unlock();
 
    // Return the future from the promise stored in the container.
    return stContainer.pCopiedFrameStatus->get_future();
}

◆ RequestDepthCopy()

std::future< bool > SIMZEDCam::RequestDepthCopy	(	cv::Mat &	cvDepth,
		const bool	bRetrieveMeasure = `true`
	)

virtual

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.

Parameters

cvDepth	- A reference to the cv::Mat to copy the depth frame to.
bRetrieveMeasure	- False to get depth IMAGE instead of MEASURE. Do not use the 8-bit grayscale depth image purposes other than displaying depth.

Returns: std::future<bool> - A future that should be waited on before the passed in frame is used. Value will be true if frame was successfully retrieved.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2023-08-26

Implements ZEDCamera.

{
    // Create instance variables.
    PIXEL_FORMATS eFrameType;
 
    // Check if the container should be set to retrieve an image or a measure.
    bRetrieveMeasure ? eFrameType = PIXEL_FORMATS::eDepthMeasure : eFrameType = PIXEL_FORMATS::eDepthImage;
    // Assemble container.
    containers::FrameFetchContainer<cv::Mat> stContainer(cvDepth, eFrameType);
 
    // Acquire lock on frame copy queue.
    std::unique_lock<std::shared_mutex> lkSchedulers(m_muPoolScheduleMutex);
    // Append frame fetch container to the schedule queue.
    m_qFrameCopySchedule.push(stContainer);
    // Release lock on the frame schedule queue.
    lkSchedulers.unlock();
 
    // Return the future from the promise stored in the container.
    return stContainer.pCopiedFrameStatus->get_future();
}

◆ RequestPointCloudCopy()

std::future< bool > SIMZEDCam::RequestPointCloudCopy ( cv::Mat & cvPointCloud )

virtual

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.

Parameters

cvPointCloud - A reference to the cv::Mat to copy the point cloud frame to.

Returns: std::future<bool> - A future that should be waited on before the passed in frame is used. Value will be true if frame was successfully retrieved.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2023-08-26

Implements ZEDCamera.

{
    // Assemble the FrameFetchContainer.
    containers::FrameFetchContainer<cv::Mat> stContainer(cvPointCloud, PIXEL_FORMATS::eXYZ);
 
    // Acquire lock on frame copy queue.
    std::unique_lock<std::shared_mutex> lkSchedulers(m_muPoolScheduleMutex);
    // Append frame fetch container to the schedule queue.
    m_qFrameCopySchedule.push(stContainer);
    // Release lock on the frame schedule queue.
    lkSchedulers.unlock();
 
    // Return the future from the promise stored in the container.
    return stContainer.pCopiedFrameStatus->get_future();
}

◆ RequestPositionalPoseCopy()

std::future< bool > SIMZEDCam::RequestPositionalPoseCopy ( ZEDCamera::Pose & stPose )

overridevirtual

Puts a sl::GeoPose pointer into a queue so a copy of a GeoPose from the camera can be written to it.

Parameters

stPose - A reference to the sl::GeoPose to store the GeoPose in.

Returns: std::future<bool> - A future that should be waited on before the passed in GeoPose is used.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

Implements ZEDCamera.

{
    // Check if positional tracking is enabled.
    if (m_bCameraPositionalTrackingEnabled)
    {
        // Assemble the data container.
        containers::DataFetchContainer<Pose> stContainer(stPose);
 
        // Acquire lock on pose copy queue.
        std::unique_lock<std::shared_mutex> lkSchedulers(m_muPoolScheduleMutex);
        // Append pose fetch container to the schedule queue.
        m_qPoseCopySchedule.push(stContainer);
        // Release lock on the pose schedule queue.
        lkSchedulers.unlock();
 
        // Check if pose queue toggle has already been set.
        if (!m_bPosesQueued.load(ATOMIC_MEMORY_ORDER_METHOD))
        {
            // Signify that the pose queue is not empty.
            m_bPosesQueued.store(true, ATOMIC_MEMORY_ORDER_METHOD);
        }
 
        // Return the future from the promise stored in the container.
        return stContainer.pCopiedDataStatus->get_future();
    }
    else
    {
        // Submit logger message.
        LOG_WARNING(logging::g_qSharedLogger, "Attempted to get ZED positional pose but positional tracking is not enabled or is still initializing!");
 
        // Create dummy promise to return the future.
        std::promise<bool> pmDummyPromise;
        std::future<bool> fuDummyFuture = pmDummyPromise.get_future();
        // Set future value.
        pmDummyPromise.set_value(false);
 
        // Return unsuccessful.
        return fuDummyFuture;
    }
}

◆ RequestFusionGeoPoseCopy()

std::future< bool > SIMZEDCam::RequestFusionGeoPoseCopy ( sl::GeoPose & slGeoPose )

overridevirtual

Puts a sl::GeoPose pointer into a queue so a copy of a GeoPose from the camera can be written to it.

Parameters

slGeoPose - A reference to the sl::GeoPose to store the GeoPose in.

Returns: std::future<bool> - A future that should be waited on before the passed in GeoPose is used.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

Implements ZEDCamera.

{
    // Check if positional tracking has been enabled.
    if (m_bCameraIsFusionMaster && m_bCameraPositionalTrackingEnabled)
    {
        // Assemble the data container.
        containers::DataFetchContainer<sl::GeoPose> stContainer(slGeoPose);
 
        // Acquire lock on frame copy queue.
        std::unique_lock<std::shared_mutex> lkSchedulers(m_muPoolScheduleMutex);
        // Append frame fetch container to the schedule queue.
        m_qGeoPoseCopySchedule.push(stContainer);
        // Release lock on the frame schedule queue.
        lkSchedulers.unlock();
 
        // Check if pose queue toggle has already been set.
        if (!m_bGeoPosesQueued.load(ATOMIC_MEMORY_ORDER_METHOD))
        {
            // Signify that the pose queue is not empty.
            m_bGeoPosesQueued.store(true, ATOMIC_MEMORY_ORDER_METHOD);
        }
 
        // Return the future from the promise stored in the container.
        return stContainer.pCopiedDataStatus->get_future();
    }
    else
    {
        // Submit logger message.
        LOG_WARNING(logging::g_qSharedLogger,
                    "Attempted to get ZED FUSION geo pose but positional tracking is not enabled and/or this camera was not initialized as a Fusion Master!");
 
        // Create dummy promise to return the future.
        std::promise<bool> pmDummyPromise;
        std::future<bool> fuDummyFuture = pmDummyPromise.get_future();
        // Set future value.
        pmDummyPromise.set_value(false);
 
        // Return unsuccessful.
        return fuDummyFuture;
    }
}

◆ RequestSensorsCopy()

std::future< bool > SIMZEDCam::RequestSensorsCopy ( sl::SensorsData & slSensorsData )

overridevirtual

Requests a copy of the sensors data from the camera.

Parameters

slSensorsData - A reference to the sl::SensorsData to store the sensors data in.

Returns: std::future<bool> - A future that should be waited on before the passed in sensors data is used.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2025-08-26

Reimplemented from ZEDCamera.

{
    // Assemble the DataFetchContainer.
    containers::DataFetchContainer<sl::SensorsData> stContainer(slSensorsData);
 
    // Acquire lock on data copy queue.
    std::unique_lock<std::shared_mutex> lkSchedulers(m_muPoolScheduleMutex);
    // Append data fetch container to the schedule queue.
    m_qSensorsCopySchedule.push(stContainer);
    // Release lock on the data schedule queue.
    lkSchedulers.unlock();
 
    // Check if pose queue toggle has already been set.
    if (!m_bSensorsQueued.load(ATOMIC_MEMORY_ORDER_METHOD))
    {
        // Signify that the pose queue is not empty.
        m_bSensorsQueued.store(true, ATOMIC_MEMORY_ORDER_METHOD);
    }
 
    // Return the future from the promise stored in the container.
    return stContainer.pCopiedDataStatus->get_future();
}

◆ ResetPositionalTracking()

sl::ERROR_CODE SIMZEDCam::ResetPositionalTracking ( )

overridevirtual

This method is used to reset the positional tracking of the camera. Because this is a simulation camera, and then ZEDSDK is not available, this method will just reset the offsets to zero.

Returns: sl::ERROR_CODE - The error code returned by the ZED SDK. In this case, it will always be SUCCESS.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

Implements ZEDCamera.

{
    // Reset offsets back to zero.
    m_dPoseOffsetX  = 0.0;
    m_dPoseOffsetY  = 0.0;
    m_dPoseOffsetZ  = 0.0;
    m_dPoseOffsetXO = 0.0;
    m_dPoseOffsetYO = 0.0;
    m_dPoseOffsetZO = 0.0;
 
    return sl::ERROR_CODE::SUCCESS;
}

Here is the caller graph for this function:

◆ RebootCamera()

sl::ERROR_CODE SIMZEDCam::RebootCamera ( )

overridevirtual

This method is used to reboot the camera. This method will stop the camera thread, join the camera thread, destroy the camera stream objects, reconstruct the camera stream objects, set the frame callbacks, and restart the camera thread. This simulates a camera reboot since the ZED SDK is not available.

Returns: sl::ERROR_CODE - The error code returned by the ZED SDK. In this case, it will always be SUCCESS. Even if the streams are not successfully reconnected, the camera will still be considered open.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

Implements ZEDCamera.

{
    // Stop the camera thread.
    this->RequestStop();
    // Join the camera thread.
    this->Join();
 
    // Destroy the camera stream objects.
    m_pRGBStream.reset();
    m_pDepthImageStream.reset();
    // Reconstruct camera stream objects. Append proper camera path arguments to each URL camera path.
    m_pRGBStream        = std::make_unique<WebRTC>(m_szCameraPath, "ZEDFrontRGB");
    m_pDepthImageStream = std::make_unique<WebRTC>(m_szCameraPath, "ZEDFrontDepthImage");
 
    // Set the frame callbacks.
    this->SetCallbacks();
 
    // Restart the camera thread.
    this->Start();
 
    return sl::ERROR_CODE::SUCCESS;
}

Here is the call graph for this function:

Here is the caller graph for this function:

◆ SubscribeFusionToCameraUUID()

sl::FUSION_ERROR_CODE SIMZEDCam::SubscribeFusionToCameraUUID ( sl::CameraIdentifier & slCameraUUID )

overridevirtual

This method is used to subscribe the sl::Fusion object to the camera with the given UUID. Given that this is a simulation camera, and the ZED SDK is not available, this method will do nothing.

Parameters

slCameraUUID - The UUID of the camera to subscribe to.

Returns: sl::FUSION_ERROR_CODE - The error code returned by the ZED SDK. In this case, it will always be SUCCESS.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

Implements ZEDCamera.

{
    // Unused parameter.
    (void) slCameraUUID;
 
    return sl::FUSION_ERROR_CODE::SUCCESS;
}

◆ PublishCameraToFusion()

sl::CameraIdentifier SIMZEDCam::PublishCameraToFusion ( )

overridevirtual

This method is used to publish the camera to the sl::Fusion object. Since this is a simulation camera this will do nothing.

Returns: sl::CameraIdentifier - The identifier of the camera.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

Implements ZEDCamera.

{
    return sl::CameraIdentifier(m_unCameraSerialNumber);
}

◆ EnablePositionalTracking()

sl::ERROR_CODE SIMZEDCam::EnablePositionalTracking ( const float fExpectedCameraHeightFromFloorTolerance = constants::ZED_DEFAULT_FLOOR_PLANE_ERROR )

overridevirtual

This method is used to enable positional tracking on the camera. Since this is a simulation camera, this method will just set the member variable and then use the NavBoard to simulate the camera's position.

Parameters

fExpectedCameraHeightFromFloorTolerance - The expected camera height from the floor tolerance.

Returns: sl::ERROR_CODE - The error code returned by the ZED SDK. In this case, it will always be SUCCESS.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

Implements ZEDCamera.

{
    // Unused parameter.
    (void) fExpectedCameraHeightFromFloorTolerance;
    // Update member variables.
    m_bCameraPositionalTrackingEnabled = true;
 
    return sl::ERROR_CODE::SUCCESS;
}

Here is the caller graph for this function:

◆ DisablePositionalTracking()

void SIMZEDCam::DisablePositionalTracking ( )

overridevirtual

This method is used to disable positional tracking on the camera. Since this is a simulation camera, this method will just set the member variable.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

Implements ZEDCamera.

{
    // Update member variables.
    m_bCameraPositionalTrackingEnabled = false;
}

Here is the caller graph for this function:

◆ SetPositionalPose()

void SIMZEDCam::SetPositionalPose	(	const double	dX,
		const double	dY,
		const double	dZ,
		const double	dXO,
		const double	dYO,
		const double	dZO
	)

overridevirtual

This method is used to set the positional pose of the camera. Since this is a simulation camera, this method will just set the offset member variables.

Parameters

dX	- The new X position of the camera in ZED_MEASURE_UNITS.
dY	- The new Y position of the camera in ZED_MEASURE_UNITS.
dZ	- The new Z position of the camera in ZED_MEASURE_UNITS.
dXO	- The new tilt of the camera around the X axis in degrees.
dYO	- The new tilt of the camera around the Y axis in degrees.
dZO	- The new tilt of the camera around the Z axis in degrees.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

Implements ZEDCamera.

{
    // Acquire lock on the current rover pose mutex.
    std::unique_lock<std::shared_mutex> lkPose(m_muCurrentRoverPoseMutex);
 
    // Update offset member variables.
    m_dPoseOffsetX  = dX - m_stCurrentRoverPose.GetUTMCoordinate().dEasting;
    m_dPoseOffsetY  = dY - m_stCurrentRoverPose.GetUTMCoordinate().dAltitude;
    m_dPoseOffsetZ  = dZ - m_stCurrentRoverPose.GetUTMCoordinate().dNorthing;
    m_dPoseOffsetXO = dXO;
    m_dPoseOffsetYO = dYO - m_stCurrentRoverPose.GetCompassHeading();
    m_dPoseOffsetZO = dZO;
}

Here is the call graph for this function:

◆ GetCameraIsOpen()

bool SIMZEDCam::GetCameraIsOpen ( )

overridevirtual

Accessor for the camera open status.

Returns: true - The camera has been successfully opened.; false - The camera has not been successfully opened.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2023-09-30

Implements Camera< cv::Mat >.

{
    return m_pRGBStream->GetIsConnected() && m_pDepthImageStream->GetIsConnected() && this->GetThreadState() == AutonomyThreadState::eRunning;
}

Here is the call graph for this function:

Here is the caller graph for this function:

◆ GetUsingGPUMem()

bool SIMZEDCam::GetUsingGPUMem ( ) const

overridevirtual

Returns if the camera is using GPU memory. This is a simulation camera, so this method will always return false.

Returns: true - We are using GPU memory.; false - We are not using GPU memory.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

Reimplemented from ZEDCamera.

{
    return false;
}

◆ GetCameraModel()

std::string SIMZEDCam::GetCameraModel ( )

overridevirtual

Accessor for the name of this model of camera.

Returns: std::string - The model of the camera.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

Implements ZEDCamera.

{
    return "SIMZED2i";
}

◆ GetPositionalTrackingEnabled()

bool SIMZEDCam::GetPositionalTrackingEnabled ( )

overridevirtual

Accessor for the if the camera's positional tracking is enabled. Since this is a simulation camera, this method will just return the member variable.

Returns: true - Positional tracking is enabled.; false - Positional tracking is disabled.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

Implements ZEDCamera.

{
    return m_bCameraPositionalTrackingEnabled && this->GetCameraIsOpen();
}

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ThreadedContinuousCode()

void SIMZEDCam::ThreadedContinuousCode ( )

overrideprivatevirtual

The code inside this private method runs in a separate thread, but still has access to this*. This method continuously get new frames from the OpenCV VideoCapture object and stores it in a member variable. Then a thread pool is started and joined once per iteration to mass copy the frames and/or measure to any other thread waiting in the queues.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2023-09-30

Implements AutonomyThread< void >.

{
    // Acquire a lock on the rover pose mutex.
    std::unique_lock<std::shared_mutex> lkRoverPoseLock(m_muCurrentRoverPoseMutex);
    // Check if the NavBoard pointer is valid.
    if (globals::g_pNavigationBoard != nullptr)
    {
        // Get the current rover pose from the NavBoard.
        m_stCurrentRoverPose = geoops::RoverPose(globals::g_pNavigationBoard->GetGPSData(), globals::g_pNavigationBoard->GetHeading());
    }
    // Release lock.
    lkRoverPoseLock.unlock();
 
    // Check if the depth image WebRTC connection is open.
    if (m_pDepthImageStream != nullptr && m_pDepthImageStream->GetIsConnected())
    {
        // Acquire a lock on the WebRTC mutex.
        std::shared_lock<std::shared_mutex> lkWebRTC2(m_muWebRTCDepthImageCopyMutex);
        // Estimate the depth measure from the depth image.
        this->EstimateDepthMeasure(m_cvDepthImage, m_cvDepthMeasure);
        // Check if the depth image is empty.
        if (m_cvDepthImage.empty())
        {
            // Release lock.
            lkWebRTC2.unlock();
            return;
        }
        // Release lock.
        lkWebRTC2.unlock();
 
        // Calculate the point cloud from the estimated depth measure.
        this->CalculatePointCloud(m_cvDepthMeasure, m_cvPointCloud);
    }
 
    // Acquire a shared_lock on the frame copy queue.
    std::shared_lock<std::shared_mutex> lkSchedulers(m_muPoolScheduleMutex);
    // Check if the frame copy queue is empty.
    if (!m_qFrameCopySchedule.empty() || !m_qPoseCopySchedule.empty() || !m_qGeoPoseCopySchedule.empty() || !m_qSensorsCopySchedule.empty())
    {
        // Add the length of all queues together to determine the number of tasks to create.
        size_t siTotalQueueLength = m_qFrameCopySchedule.size() + m_qPoseCopySchedule.size() + m_qGeoPoseCopySchedule.size() + m_qSensorsCopySchedule.size();
 
        // Acquire shared lock on the WebRTC mutex, so that the WebRTC connection doesn't try to write to the Mats while they are being copied in the thread pool.
        std::shared_lock<std::shared_mutex> lkWebRTC(m_muWebRTCRGBImageCopyMutex);
        std::shared_lock<std::shared_mutex> lkWebRTC2(m_muWebRTCDepthImageCopyMutex);
 
        // Start the thread pool to store multiple copies of the sl::Mat into the given cv::Mats.
        this->RunDetachedPool(siTotalQueueLength, m_nNumFrameRetrievalThreads);
 
        // Get current time.
        std::chrono::_V2::system_clock::duration tmCurrentTime = std::chrono::high_resolution_clock::now().time_since_epoch();
        // Only reset once every couple seconds.
        if (std::chrono::duration_cast<std::chrono::seconds>(tmCurrentTime).count() % 31 == 0 && !m_bQueueTogglesAlreadyReset)
        {
            // Reset queue counters.
            m_bPosesQueued.store(false, ATOMIC_MEMORY_ORDER_METHOD);
            m_bGeoPosesQueued.store(false, ATOMIC_MEMORY_ORDER_METHOD);
            m_bSensorsQueued.store(false, ATOMIC_MEMORY_ORDER_METHOD);
 
            // Set reset toggle.
            m_bQueueTogglesAlreadyReset = true;
        }
        // Crucial for toggle action. If time is not evenly devisable and toggles have previously been set, reset queue reset boolean.
        else if (m_bQueueTogglesAlreadyReset)
        {
            // Reset reset toggle.
            m_bQueueTogglesAlreadyReset = false;
        }
 
        // Wait for thread pool to finish.
        this->JoinPool();
 
        // Release lock on WebRTC mutex.
        lkWebRTC.unlock();
        lkWebRTC2.unlock();
    }
 
    // Release lock on frame copy queue.
    lkSchedulers.unlock();
}

Here is the call graph for this function:

◆ PooledLinearCode()

void SIMZEDCam::PooledLinearCode ( )

overrideprivatevirtual

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 Grab methods.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2023-09-30

Implements AutonomyThread< void >.

{
    //  Frame queue.
 
    // Acquire mutex for getting frames out of the queue.
    std::unique_lock<std::shared_mutex> lkFrameQueue(m_muFrameCopyMutex);
    // Check if the queue is empty.
    if (!m_qFrameCopySchedule.empty())
    {
        // Get frame container out of queue.
        containers::FrameFetchContainer<cv::Mat> stContainer = m_qFrameCopySchedule.front();
        // Pop out of queue.
        m_qFrameCopySchedule.pop();
        // Release lock.
        lkFrameQueue.unlock();
 
        // Determine which frame should be copied.
        switch (stContainer.eFrameType)
        {
            case PIXEL_FORMATS::eBGRA: *(stContainer.pFrame) = m_cvFrame.clone(); break;
            case PIXEL_FORMATS::eDepthImage: *(stContainer.pFrame) = m_cvDepthImage.clone(); break;
            case PIXEL_FORMATS::eDepthMeasure: *(stContainer.pFrame) = m_cvDepthMeasure.clone(); break;
            case PIXEL_FORMATS::eXYZ: *(stContainer.pFrame) = m_cvPointCloud.clone(); break;
            default: *(stContainer.pFrame) = m_cvFrame.clone(); break;
        }
 
        // Signal future that the frame has been successfully retrieved.
        stContainer.pCopiedFrameStatus->set_value(true);
    }
 
    //  Pose queue.
    // Acquire mutex for getting data out of the pose queue.
    std::unique_lock<std::shared_mutex> lkPoseQueue(m_muPoseCopyMutex);
    // Check if the queue is empty.
    if (!m_qPoseCopySchedule.empty())
    {
        // Get pose container out of queue.
        containers::DataFetchContainer<Pose> stContainer = m_qPoseCopySchedule.front();
        // Pop out of queue.
        m_qPoseCopySchedule.pop();
        // Release lock.
        lkPoseQueue.unlock();
 
        // Get angle realignments.
        double dNewYO = numops::InputAngleModulus<double>(m_stCurrentRoverPose.GetCompassHeading() + m_dPoseOffsetYO, 0.0, 360.0);
        // Repack values into pose.
        Pose stPose(m_stCurrentRoverPose.GetUTMCoordinate().dEasting + m_dPoseOffsetX,
                    m_stCurrentRoverPose.GetUTMCoordinate().dAltitude + m_dPoseOffsetY,
                    m_stCurrentRoverPose.GetUTMCoordinate().dNorthing + m_dPoseOffsetZ,
                    m_dPoseOffsetXO,
                    dNewYO,
                    m_dPoseOffsetZO);
 
        // ISSUE NOTE: Might be in the future if we ever change our coordinate system on the ZED. This can be used to fix the directions of the Pose's coordinate system.
        // // Check ZED coordinate system.
        // switch (m_slCameraParams.coordinate_system)
        // {
        //     case sl::COORDINATE_SYSTEM::LEFT_HANDED_Y_UP:
        //     {
        //         // Realign based in the signedness of this coordinate system. Z is backwards.
        //         stPose.stTranslation.dZ *= -1;
        //         break;
        //     }
        //     default:
        //     {
        //         // No need to flip signs for other coordinate systems.
        //         break;
        //     }
        // }
 
        // Copy pose.
        *(stContainer.pData) = stPose;
 
        // Signal future that the data has been successfully retrieved.
        stContainer.pCopiedDataStatus->set_value(true);
    }
    else
    {
        // Release lock.
        lkPoseQueue.unlock();
    }
 
    //  GeoPose queue.
    // Acquire mutex for getting data out of the pose queue.
    std::unique_lock<std::shared_mutex> lkGeoPoseQueue(m_muGeoPoseCopyMutex);
    // Check if the queue is empty.
    if (!m_qGeoPoseCopySchedule.empty())
    {
        // Get pose container out of queue.
        containers::DataFetchContainer<sl::GeoPose> stContainer = m_qGeoPoseCopySchedule.front();
        // Pop out of queue.
        m_qGeoPoseCopySchedule.pop();
        // Release lock.
        lkGeoPoseQueue.unlock();
 
        // Get rover's current UTM position.
        geoops::UTMCoordinate stCurrentUTM = m_stCurrentRoverPose.GetUTMCoordinate();
        // Add offsets to the current UTM position.
        stCurrentUTM.dEasting += m_dPoseOffsetX;
        stCurrentUTM.dAltitude += m_dPoseOffsetY;
        stCurrentUTM.dNorthing += m_dPoseOffsetZ;
        // Create a GPS coordinate from the UTM position.
        geoops::GPSCoordinate stCurrentGPS = geoops::ConvertUTMToGPS(stCurrentUTM);
 
        // Create new GeoPose.
        sl::GeoPose slGeoPose;
        slGeoPose.heading = numops::InputAngleModulus<double>(m_stCurrentRoverPose.GetCompassHeading() + m_dPoseOffsetYO, 0.0, 360.0);
        slGeoPose.latlng_coordinates.setCoordinates(stCurrentGPS.dLatitude, stCurrentGPS.dLongitude, stCurrentGPS.dAltitude, false);
 
        // Copy pose.
        *(stContainer.pData) = slGeoPose;
 
        // Signal future that the data has been successfully retrieved.
        stContainer.pCopiedDataStatus->set_value(true);
    }
    else
    {
        // Release lock.
        lkGeoPoseQueue.unlock();
    }
 
    //  Sensors queue.
    // Acquire mutex for getting data out of the sensors queue.
    std::unique_lock<std::shared_mutex> lkSensorsQueue(m_muSensorsCopyMutex);
    // Check if the queue is empty.
    if (!m_qSensorsCopySchedule.empty())
    {
        // Get pose container out of queue.
        containers::DataFetchContainer<sl::SensorsData> stContainer = m_qSensorsCopySchedule.front();
        // Pop out of queue.
        m_qSensorsCopySchedule.pop();
        // Release lock.
        lkSensorsQueue.unlock();
 
        // Copy pose.
        *(stContainer.pData) = m_stIMUData;
 
        // Signal future that the data has been successfully retrieved.
        stContainer.pCopiedDataStatus->set_value(true);
    }
    else
    {
        // Release lock.
        lkSensorsQueue.unlock();
    }
}

Here is the call graph for this function:

◆ SetCallbacks()

void SIMZEDCam::SetCallbacks ( )

private

This method sets the callbacks for the WebRTC connections.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2024-12-26

{
    // Set the frame callbacks.
    m_pRGBStream->SetOnFrameReceivedCallback(
        [this](cv::Mat& cvFrame)
        {
            // Check if the frame is empty.
            if (!cvFrame.empty())
            {
                // Acquire a lock on the webRTC copy mutex.
                std::unique_lock<std::shared_mutex> lkWebRTC(m_muWebRTCRGBImageCopyMutex);
                // Deep copy the frame.
                m_cvFrame = cvFrame.clone();
            }
        });
    m_pDepthImageStream->SetOnFrameReceivedCallback(
        [this](cv::Mat& cvFrame)
        {
            // Check if the frame is empty.
            if (!cvFrame.empty())
            {
                // Acquire a lock on the webRTC copy mutex.
                std::unique_lock<std::shared_mutex> lkWebRTC(m_muWebRTCDepthImageCopyMutex);
                // Deep copy the frame to the depth image buffer.
                m_cvDepthImageBuffer = cvFrame.clone();
                // Convert the depth image buffer to grayscale.
                cv::cvtColor(m_cvDepthImageBuffer, m_cvDepthImage, cv::COLOR_BGR2GRAY);
            }
        });
}

Here is the call graph for this function:

Here is the caller graph for this function:

◆ EstimateDepthMeasure()

void SIMZEDCam::EstimateDepthMeasure	(	const cv::Mat &	cvDepthImage,
		cv::Mat &	cvDepthMeasure
	)

private

This method estimates the depth measure from the depth image.

Parameters

cvDepthImage	- The depth image to estimate the depth measure from.
cvDepthMeasure	- The estimated depth measure that will be written to.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2025-04-29

{
    // Declare instance variables.
    const float fMaxDepth = 2001.0f;    // Maximum depth in cm.
 
    // Check if the depth image is empty.
    if (cvDepthImage.empty())
    {
        // If the depth image is empty, fill the depth measure with zeros.
        cvDepthMeasure = cv::Mat::zeros(cvDepthMeasure.size(), CV_32FC1);
        return;
    }
 
// TEST: Even though this speeds up the code, it might be too much CPU work as the codebase grows. Use a GpuMat instead.
#pragma omp parallel for collapse(2)
 
    // Iterate over each pixel in the cvDepthImage image.
    for (int nY = 0; nY < cvDepthImage.rows; ++nY)
    {
        for (int nX = 0; nX < cvDepthImage.cols; ++nX)
        {
            // For this, we are just using the depth image to estimate the depth measure. We will treat 255 as 0 cm and 0 as fMaxDepth - 1 cm.
            // Get the depth value from the depth image.
            uchar ucDepthValue = cvDepthImage.at<uchar>(nY, nX);
 
            // Calculate the depth in cm.
            float fDepth = (1.0f - (ucDepthValue / 255.0f)) * fMaxDepth;
            // Check if nY and nX are within the bounds of the depth measure image.
            if (nY < cvDepthMeasure.rows && nX < cvDepthMeasure.cols)
            {
                // Store the estimated depth in the new cv::Mat. Convert cm to m.
                cvDepthMeasure.at<float>(nY, nX) = fDepth / 100.0f;    // Convert cm to m.
            }
        }
    }
}

Here is the call graph for this function:

Here is the caller graph for this function:

◆ CalculatePointCloud()

void SIMZEDCam::CalculatePointCloud	(	const cv::Mat &	cvDepthMeasure,
		cv::Mat &	cvPointCloud
	)

private

This method calculates a point cloud from the decoded depth measure use some simple trig and the camera FOV.

Parameters

cvDepthMeasure	- The decoded depth measure.
cvPointCloud	- The point cloud that will be written to.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2025-01-04

{
    // Calculate focal lengths from FOV.
    const double dRadPerDeg = M_PI / 180.0;
    const double dFx        = (cvDepthMeasure.cols / 2.0) / tan(m_dPropHorizontalFOV * dRadPerDeg / 2.0);
    const double dFy        = (cvDepthMeasure.rows / 2.0) / tan(m_dPropVerticalFOV * dRadPerDeg / 2.0);
    // Image center.
    const double dCx = cvDepthMeasure.cols / 2.0;
    const double dCy = cvDepthMeasure.rows / 2.0;
 
// TEST: Even though this speeds up the code, it might be too much CPU work as the codebase grows. Use a GpuMat instead.
// This is a parallel for loop that calculates the point cloud from the decoded depth measure.
#pragma omp parallel for collapse(2)
 
    // Iterate over each pixel in the cvDepthMeasure image.
    for (int nY = 0; nY < cvDepthMeasure.rows; ++nY)
    {
        for (int nX = 0; nX < cvDepthMeasure.cols; ++nX)
        {
            // Get depth value.
            float fDepth = cvDepthMeasure.at<float>(nY, nX);
 
            // Skip invalid depth values.
            if (fDepth <= 0)
            {
                cvPointCloud.at<cv::Vec4f>(nY, nX) = cv::Vec4f(0, 0, 0, 0);
                continue;
            }
 
            // Convert from pixel coordinates to 3D coordinates.
            float fX = static_cast<float>((nX - dCx) * fDepth / dFx);
            float fY = static_cast<float>((dCy - nY) * fDepth / dFy);
            float fZ = fDepth;
 
            // Store point. (XYZ + intensity, using Y channel for intensity)
            cvPointCloud.at<cv::Vec4f>(nY, nX) = cv::Vec4f(fX, fY, fZ, 255);
        }
    }
}

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ ProcessIMUData

const std::function<void(const rovecomm::RoveCommPacket<double>&, const sockaddr_in&)> SIMZEDCam::ProcessIMUData

private

Callback function to process incoming IMU data from RoveComm for the SIM ZED Camera. Normally, this data would come from the physical ZED camera's IMU over USB,.

Author: clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date: 2025-11-19

        {
            // Not using this.
            (void) stdAddr;
 
            // Acquire a write lock on the sensors mutex.
            std::unique_lock<std::shared_mutex> lkSensorsProcessLock(m_muSensorsCopyMutex);
            // Update IMU data.
            m_stIMUData.imu.linear_acceleration.x = static_cast<float>(stPacket.vData[0]);
            m_stIMUData.imu.linear_acceleration.y = static_cast<float>(stPacket.vData[1]);
            m_stIMUData.imu.linear_acceleration.z = static_cast<float>(stPacket.vData[2]);
            m_stIMUData.imu.angular_velocity.x    = static_cast<float>(stPacket.vData[3]);
            m_stIMUData.imu.angular_velocity.y    = static_cast<float>(stPacket.vData[4]);
            m_stIMUData.imu.angular_velocity.z    = static_cast<float>(stPacket.vData[5]);
 
            // Manually calculate the Gyro pose using the Tait-Bryan angles (ZYX convention) and the quaternion representation.
            // This is because the SIM does not provide orientation data from the IMU, only angular velocity.
            double dQx    = stPacket.vData[6];
            double dQy    = stPacket.vData[7];
            double dQz    = stPacket.vData[8];
            double dQw    = stPacket.vData[9];
            double dRoll  = std::atan2(2.0 * (dQw * dQx + dQy * dQz), 1.0 - 2.0 * (dQx * dQx + dQy * dQy));
            double dPitch = std::asin(2.0 * (dQw * dQy - dQz * dQx));
            double dYaw   = std::atan2(2.0 * (dQw * dQz + dQx * dQy), 1.0 - 2.0 * (dQy * dQy + dQz * dQz));
            // Pack the gyro values into a sl::Transform.
            sl::float3 slEulerAngles(static_cast<float>(dRoll), static_cast<float>(dPitch), static_cast<float>(dYaw));
            sl::Transform slIMUTransform;
            slIMUTransform.setEulerAngles(slEulerAngles);
            m_stIMUData.imu.pose = slIMUTransform;
 
            // Unlock mutex.
            lkSensorsProcessLock.unlock();
 
            // Submit logger message.
            LOG_DEBUG(logging::g_qSharedLogger,
                      "Incoming IMU data processed from RoveComm for SIM ZED Camera: (AccelX {}, AccelY {}, AccelZ {}, GyroX {}, GyroY {}, GyroZ {})",
                      stPacket.vData[0],
                      stPacket.vData[1],
                      stPacket.vData[2],
                      stPacket.vData[3],
                      stPacket.vData[4],
                      stPacket.vData[5]);
        };

The documentation for this class was generated from the following files:

src/vision/cameras/sim/SIMZEDCam.h
src/vision/cameras/sim/SIMZEDCam.cpp

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ SIMZEDCam()

◆ ~SIMZEDCam()

Member Function Documentation

◆ RequestFrameCopy()

◆ RequestDepthCopy()

◆ RequestPointCloudCopy()

◆ RequestPositionalPoseCopy()

◆ RequestFusionGeoPoseCopy()

◆ RequestSensorsCopy()

◆ ResetPositionalTracking()

◆ RebootCamera()

◆ SubscribeFusionToCameraUUID()

◆ PublishCameraToFusion()

◆ EnablePositionalTracking()

◆ DisablePositionalTracking()

◆ SetPositionalPose()

◆ GetCameraIsOpen()

◆ GetUsingGPUMem()

◆ GetCameraModel()

◆ GetPositionalTrackingEnabled()

◆ ThreadedContinuousCode()

◆ PooledLinearCode()

◆ SetCallbacks()

◆ EstimateDepthMeasure()

◆ CalculatePointCloud()

Member Data Documentation

◆ ProcessIMUData