GrabBag/Device/VrEyeDevice/Src/VrEyeDevice.cpp

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

#include <functional>

#ifdef VR_EYE_DEBUG_MODE
#include "LaserDataLoader.h"
#include <chrono>
#endif

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)
{
#ifdef VR_EYE_DEBUG_MODE
    m_debugStatusCallback = fNotify;
    m_debugStatusCallbackParam = param;
    LOG_DEBUG("VR_EYE_DEBUG_MODE: Status callback stored\n");
    return SUCCESS;
#endif
    // 如果设备已经打开，立即设置回调
    if (m_pHandle) {
        VzNL_SetDeviceStatusNotifyEx(m_pHandle, fNotify, param);
    } 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)
{
#ifdef VR_EYE_DEBUG_MODE
    LOG_DEBUG("VR_EYE_DEBUG_MODE: OpenDevice success\n");
    return SUCCESS;
#endif
    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);
    }

    LOG_DEBUG("VzNL_ResearchDevice count : %d\n", nCount);

    pCBInfo = new SVzNLEyeCBInfo[nCount];
    if (!pCBInfo) {
        return ERR_CODE(DATA_ERR_MEM);
    }
    
    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 || 0 == strlen(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);

    if(!bSwing)
    {
        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)
{
#ifdef VR_EYE_DEBUG_MODE
    strcpy(sVersionInfo.szSDKVersion, "DEBUG_MODE_1.0.0");
    return SUCCESS;
#endif
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_GetVersion(m_pHandle, &sVersionInfo);
}

int CVrEyeDevice::GetDevInfo(SVzNLEyeDeviceInfoEx &sDeviceInfo)
{
#ifdef VR_EYE_DEBUG_MODE
    memset(&sDeviceInfo, 0, sizeof(SVzNLEyeDeviceInfoEx));
    strcpy(sDeviceInfo.sEyeCBInfo.byServerIP, "192.168.1.100");
    return SUCCESS;
#endif
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    memcpy(&sDeviceInfo, &m_sEeyCBDeviceInfo, sizeof(SVzNLEyeDeviceInfoEx));
    return SUCCESS;
}

int CVrEyeDevice::CloseDevice()
{
#ifdef VR_EYE_DEBUG_MODE
    LOG_DEBUG("VR_EYE_DEBUG_MODE: CloseDevice success\n");
    return SUCCESS;
#endif
    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)
{
#ifdef VR_EYE_DEBUG_MODE
    LOG_DEBUG("VR_EYE_DEBUG_MODE: StartDetect\n");

    // 等待旧线程结束
    if (m_debugThread.joinable()) {
        m_debugThread.join();
    }

    m_debugCallback = fCallFunc;
    m_debugCallbackParam = param;
    m_debugDataType = eDataType;
    m_debugThread = std::thread(&CVrEyeDevice::DebugThreadFunc, this);
    return SUCCESS;
#endif
    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()
{
#ifdef VR_EYE_DEBUG_MODE
    LOG_DEBUG("VR_EYE_DEBUG_MODE: StopDetect\n");
    return SUCCESS;
#endif
    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)
{
#ifdef VR_EYE_DEBUG_MODE
    return SUCCESS;
#endif
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_ConfigEyeExpose(m_pHandle, keVzNLExposeMode_Fix, exposeTime);
}

int CVrEyeDevice::GetEyeExpose(unsigned int &exposeTime)
{
#ifdef VR_EYE_DEBUG_MODE
    exposeTime = 1000;
    return SUCCESS;
#endif
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    EVzNLExposeMode peExposeMode;
    return VzNL_GetConfigEyeExpose(m_pHandle, &peExposeMode, &exposeTime);
}

int CVrEyeDevice::SetEyeGain(unsigned int &gain)
{
#ifdef VR_EYE_DEBUG_MODE
    return SUCCESS;
#endif
    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)
{
#ifdef VR_EYE_DEBUG_MODE
    gain = 100;
    return SUCCESS;
#endif
    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)
{
#ifdef VR_EYE_DEBUG_MODE
    return SUCCESS;
#endif
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_SetFrameRate(m_pHandle, frame);
}

int CVrEyeDevice::GetFrame(int &frame)
{
#ifdef VR_EYE_DEBUG_MODE
    frame = 30;
    return SUCCESS;
#endif
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_GetFrameRate(m_pHandle, reinterpret_cast<int *>(&frame));
}

bool CVrEyeDevice::IsSupport()
{
#ifdef VR_EYE_DEBUG_MODE
    return true;
#endif
    if(!m_pHandle) return false;

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

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

int CVrEyeDevice::GetRGBDExposeThres(float &value)
{
#ifdef VR_EYE_DEBUG_MODE
    value = 128.0f;
    return SUCCESS;
#endif
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    int nRet = 0;
    value = VzNL_GetRGBAutoExposeThres(m_pHandle, &nRet);
    return nRet;
}

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

int CVrEyeDevice::GetFilterHeight(double &dHeight)
{
#ifdef VR_EYE_DEBUG_MODE
    dHeight = 100.0;
    return SUCCESS;
#endif
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_GetLaserLineFilterHeight(m_pHandle, &dHeight);
}

int CVrEyeDevice::GetSwingSpeed(float &fSpeed)
{
#ifdef VR_EYE_DEBUG_MODE
    fSpeed = 10.0f;
    return SUCCESS;
#endif
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_GetSwingAngleSpeed(m_pHandle, &fSpeed);
}

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

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

int CVrEyeDevice::GetSwingAngle(float &fMin, float &fMax)
{
#ifdef VR_EYE_DEBUG_MODE
    fMin = -30.0f;
    fMax = 30.0f;
    return SUCCESS;
#endif
    if(!m_pHandle) return ERRCODE(DEV_NO_OPEN);
    return VzNL_GetSwingMotorAngle(m_pHandle, &fMin, &fMax);
}

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

int CVrEyeDevice::GetWorkRange(double &dMin, double &dMax)
{
#ifdef VR_EYE_DEBUG_MODE
    dMin = 500.0;
    dMax = 1500.0;
    return SUCCESS;
#endif
    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;
}

#ifdef VR_EYE_DEBUG_MODE
void CVrEyeDevice::DebugThreadFunc()
{
    // 固定的点云文件路径
    // const std::string debugFilePath = "C:\\project\\QT\\GrabBag\\TestData\\workpiece\\new\\test\\9_LazerData_Hi229229.txt";
    // const std::string debugFilePath = "C:\\project\\QT\\GrabBag\\TestData\\bag\\LaserLine1_grid.txt";
    const std::string debugFilePath = "C:\\project\\QT\\GrabBag\\TestData\\workpiece\\new_1\\3.txt";

    LaserDataLoader loader;
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>> laserLines;
    int lineNum = 0;
    float scanSpeed = 0.0f;
    int maxTimeStamp = 0;
    int clockPerSecond = 0;

    // 加载点云文件
    int ret = loader.LoadLaserScanData(debugFilePath, laserLines, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
    if (ret != SUCCESS) {
        LOG_DEBUG("VR_EYE_DEBUG_MODE: Failed to load laser data from %s, error: %s\n",debugFilePath.c_str(), loader.GetLastError().c_str());
        return;
    }

        // 循环读取并回调
    auto iter = laserLines.begin();

    while (iter != laserLines.end()) {
        if (m_debugCallback) {
            auto& dataPair = *iter;
            m_debugCallback(dataPair.first, &dataPair.second, m_debugCallbackParam);
        }

        iter++;
        // 模拟采集间隔，约30ms
        std::this_thread::sleep_for(std::chrono::milliseconds(2));
    }
    if (m_debugStatusCallback) {
        m_debugStatusCallback(keDeviceWorkStatus_Device_Swing_Finish, nullptr, 0, m_debugStatusCallbackParam);
        m_debugStatusCallback(keDeviceWorkStatus_Device_Auto_Stop, nullptr, 0, m_debugStatusCallbackParam);
    }

    // 释放数据
    loader.FreeLaserScanData(laserLines);
    LOG_DEBUG("VR_EYE_DEBUG_MODE: Debug thread stopped\n");
}
#endif