GrabBag/VrEyeDevice/Src/VrEyeDevice.cpp

#include "VrEyeDevice.h"
#include "VrError.h"
#include "VrLog.h"

#include <functional>

CVrEyeDevice::CVrEyeDevice()
    : m_pHandle(nullptr)
{
}

CVrEyeDevice::~CVrEyeDevice()
{
    // 确保设备被正确关闭
    if (m_pHandle) {
        CloseDevice();
    }
}

int CVrEyeDevice::InitDevice()
{
    return CVrEyeCommon::GetInstance()->InitDevice();
}

int CVrEyeDevice::SetStatusCallback(VzNL_OnNotifyStatusCBEx fNotify, void *param)
{
    // 如果设备已经打开，立即设置回调
    if (m_pHandle) {
        VzNL_SetDeviceStatusNotifyEx(m_pHandle, fNotify, param);
        LOG_DEBUG("Status callback set for opened device\n");
    } else {
        LOG_DEBUG("Status callback stored, will be set when device opens\n");
    }
    
    return SUCCESS;
}

int CVrEyeDevice::OpenDevice(const char* sIP, bool bRGBD, bool bSwing, bool bFillLaser)
{
    int nErrCode = SUCCESS;
    // 搜索设备
    VzNL_ResearchDevice(keSearchDeviceFlag_EthLaserRobotEye);

    // 获取搜索到的设备
    SVzNLEyeCBInfo* pCBInfo = nullptr;
    int nCount = 0;
    nErrCode = VzNL_GetEyeCBDeviceInfo(nullptr, &nCount);
    if (0 != nErrCode)
    {
        return ERR_CODE(nErrCode);
    }

    if(0 == nCount)
    {
        return ERR_CODE(DEV_NOT_FIND);
    }

    pCBInfo = new SVzNLEyeCBInfo[nCount];
    if (!pCBInfo) {
        return ERR_CODE(DEV_OPEN_ERR);
    }
    
    nErrCode = VzNL_GetEyeCBDeviceInfo(pCBInfo, &nCount);
    if (0 != nErrCode) {
        delete[] pCBInfo;
        return ERR_CODE(nErrCode);
    }

    SVzNLOpenDeviceParam sVzNlOpenDeviceParam;
    sVzNlOpenDeviceParam.eEyeResolution = keVzNLEyeResolution_Unknown;
    sVzNlOpenDeviceParam.eSDKType = keSDKType_DetectLaser;
    sVzNlOpenDeviceParam.bStopStream = VzTrue;
    if(nullptr == sIP)
    {
        m_pHandle = VzNL_OpenDevice(&pCBInfo[0], &sVzNlOpenDeviceParam, &nErrCode);
        LOG_DEBUG("OpenDevice DEV %s result: %d\n", pCBInfo[0].byServerIP, nErrCode);
    }
    else
    {
        for(int i = 0; i < nCount; i++)
        {
            if(strcmp(pCBInfo[i].byServerIP, sIP) == 0)
            {
                m_pHandle = VzNL_OpenDevice(&pCBInfo[i], &sVzNlOpenDeviceParam, &nErrCode);
                break;
            }
        }
    }

    // 清理pCBInfo数组
    delete[] pCBInfo;
    pCBInfo = nullptr;


    if(nullptr == m_pHandle)
    {
        return ERR_CODE(DEV_OPEN_ERR);
    }

    if(0 != nErrCode)
    {
        return ERR_CODE(nErrCode);
    }

    m_sEeyCBDeviceInfo.sEyeCBInfo.nSize = sizeof(SVzNLEyeDeviceInfoEx);
    VzNL_GetDeviceInfo(m_pHandle, (SVzNLEyeCBInfo *)(&m_sEeyCBDeviceInfo));

    VzNL_BeginDetectLaser(m_pHandle);
    VzNL_EnableSwingMotor(m_pHandle, bSwing ? VzTrue : VzFalse);

    VzNL_EnableLaserLight(m_pHandle, VzTrue);
    VzBool bRet = VzNL_IsEnableLaserLight(m_pHandle);
    LOG_DEBUG("EnableLaserLight ret : %d\n", bRet);

    SVzNLVersionInfo sVersionInfo;
    VzNL_GetVersion(m_pHandle, &sVersionInfo);
    LOG_DEBUG("version : %s\n", sVersionInfo.szSDKVersion);

    // int nnnRet = VzNL_SetEthSendDataLength(m_pHandle, 1024);
    // LOG_DEBUG("SenddataLen ret : %d\n", nnnRet);

    //启用RGBD
    int nRGBRet = VzNL_EnableRGB(m_pHandle, bRGBD ? VzTrue : VzFalse);
    LOG_DEBUG("Enable RGB %s ret : %d\n", bRGBD ? "true" : "false", nRGBRet);

    // 填充
    VzNL_EnableFillLaserPoint(m_pHandle, bFillLaser ? VzTrue : VzFalse);
    
    return SUCCESS;
}

int CVrEyeDevice::GetVersion(SVzNLVersionInfo &sVersionInfo)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_GetVersion(m_pHandle, &sVersionInfo);
}

int CVrEyeDevice::GetDevInfo(SVzNLEyeDeviceInfoEx &sDeviceInfo)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    memcpy(&sDeviceInfo, &m_sEeyCBDeviceInfo, sizeof(SVzNLEyeDeviceInfoEx));
    return SUCCESS;
}

int CVrEyeDevice::CloseDevice()
{
    int nErrCode = SUCCESS;
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    VzNL_EndDetectLaser(m_pHandle);
    nErrCode = VzNL_CloseDevice(m_pHandle);
    if(0 == nErrCode)
    {
        m_pHandle = nullptr;
    }
    return nErrCode;
}

int CVrEyeDevice::StartDetect(VzNL_AutoOutputLaserLineExCB fCallFunc, EVzResultDataType eDataType, void *param)
{
    int nErrCode = SUCCESS;
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    VzNL_SetLaserLight(m_pHandle, VzTrue);
    LOG_DEBUG("StartDetect eDataType: %d\n", eDataType);
    nErrCode = VzNL_StartAutoDetectEx(m_pHandle, eDataType, keFlipType_None, fCallFunc, param);
    return nErrCode;
}

bool CVrEyeDevice::IsDetectIng()
{
    return false;
}

int CVrEyeDevice::StopDetect()
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_StopAutoDetect(m_pHandle);
}

int CVrEyeDevice::SetDetectROI(SVzNLRect &leftROI, SVzNLRect &rightROI)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    int nErrCode = SUCCESS;

    return nErrCode;
}

int CVrEyeDevice::GetDetectROI(SVzNLRect &leftROI, SVzNLRect &rightROI)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    int nErrCode = SUCCESS;

    return nErrCode;
}

int CVrEyeDevice::SetEyeExpose(unsigned int &exposeTime)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_ConfigEyeExpose(m_pHandle, keVzNLExposeMode_Fix, exposeTime);
}

int CVrEyeDevice::GetEyeExpose(unsigned int &exposeTime)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    EVzNLExposeMode peExposeMode;
    return VzNL_GetConfigEyeExpose(m_pHandle, &peExposeMode, &exposeTime);
}

int CVrEyeDevice::SetEyeGain(unsigned int &gain)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    int nRet = VzNL_SetCameraGain(m_pHandle, keEyeSensorType_Left, gain);
    nRet |= VzNL_SetCameraGain(m_pHandle, keEyeSensorType_Right, gain);
    return nRet;
}

int CVrEyeDevice::GetEyeGain(unsigned int &gain)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_GetCameraGain(m_pHandle, keEyeSensorType_Left, reinterpret_cast<unsigned short *>(&gain));
}

int CVrEyeDevice::SetFrame(int &frame)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_SetFrameRate(m_pHandle, frame);
}

int CVrEyeDevice::GetFrame(int &frame)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_GetFrameRate(m_pHandle, reinterpret_cast<int *>(&frame));
}

bool CVrEyeDevice::IsSupport()
{
    if(!m_pHandle) return false;

    int nRet = 0;
    VzBool bSupport = VzNL_IsSupportRGBCamera(m_pHandle, &nRet);
    return VzTrue == bSupport;
}

int CVrEyeDevice::SetRGBDExposeThres(float &value)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_SetRGBAutoExposeThres(m_pHandle, value);
}

int CVrEyeDevice::GetRGBDExposeThres(float &value)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    int nRet = 0;
    value = VzNL_GetRGBAutoExposeThres(m_pHandle, &nRet);
    return nRet;
}

int CVrEyeDevice::SetFilterHeight(double &dHeight)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_ConfigLaserLineFilterHeight(m_pHandle, dHeight);
}

int CVrEyeDevice::GetFilterHeight(double &dHeight)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_GetLaserLineFilterHeight(m_pHandle, &dHeight);
}

int CVrEyeDevice::GetSwingSpeed(float &fSpeed)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_GetSwingAngleSpeed(m_pHandle, &fSpeed);
}

int CVrEyeDevice::SetSwingSpeed(float &fSpeed)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_SetSwingAngleSpeed(m_pHandle, fSpeed);
}

int CVrEyeDevice::SetSwingAngle(float &fMin, float &fMax)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_SetSwingMotorAngle(m_pHandle, fMin, fMax);
}

int CVrEyeDevice::GetSwingAngle(float &fMin, float &fMax)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_GetSwingMotorAngle(m_pHandle, &fMin, &fMax);
}

int CVrEyeDevice::SetWorkRange(double &nMin, double &nMax)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_SetSwingMotorWorkRange(m_pHandle, nMin, nMax);
}

int CVrEyeDevice::GetWorkRange(double &dMin, double &dMax)
{
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_GetSwingMotorWorkRange(m_pHandle, &dMin, &dMax);
}

// 初始化对象
int IVrEyeDevice::CreateObject(IVrEyeDevice** ppEyeDevice)
{
    CVrEyeDevice* p = new CVrEyeDevice;
    *ppEyeDevice = p;
    return 0;
}