eUi/subModule/Hd_3DCameraLMI_module/Hd_3DCameraLMI_module.cpp

#include "Hd_3DCameraLMI_module.h"
#include <QDebug>
#include <QMutexLocker>
QQueue<std::vector<cv::Mat>> queuePic;
QMutex* m_mutex;

//0106 断连提示
void _stdcall ExceptionCB(unsigned int nMesgType, void* pUser) {
    mv_Status = "disconnect";
    qDebug() << " LMI camera disconnects!";
}

#pragma region LMI

// Data callback function
// This function is called from a separate thread spawned by the GoSDK library.
// Processing within this function should be minimal.
kStatus kCall onData(void* ctx, void* sys, void* dataset)
{
    double time_Start = (double)clock();
    unsigned int i;
    Mat heightMat1, heightMat2;//高度图
    Mat luminanceMat1, luminanceMat2;//灰度图
    //DataContext* context = (DataContext*)ctx;

    std::vector<cv::Mat> picVec;

    for (i = 0; i < GoDataSet_Count(dataset); ++i)
    {
        GoDataMsg dataObj = GoDataSet_At(dataset, i);

        switch (GoDataMsg_Type(dataObj))
        {
        case GO_DATA_MESSAGE_TYPE_UNIFORM_SURFACE:
        {
            MyTiff temp;
            GoUniformSurfaceMsg surfaceMsg = dataObj;
            double XResolution = NM_TO_MM(GoSurfaceMsg_XResolution(surfaceMsg));
            double YResolution = NM_TO_MM(GoSurfaceMsg_YResolution(surfaceMsg));
            double ZResolution = NM_TO_MM(GoSurfaceMsg_ZResolution(surfaceMsg));
            double XOffset = UM_TO_MM(GoSurfaceMsg_XOffset(surfaceMsg));
            double YOffset = UM_TO_MM(GoSurfaceMsg_YOffset(surfaceMsg));
            double ZOffset = UM_TO_MM(GoSurfaceMsg_ZOffset(surfaceMsg));

            int Width = GoSurfaceMsg_Width(surfaceMsg);
            int Height = GoSurfaceMsg_Length(surfaceMsg);

            temp.Tiff = new unsigned short[Width * Height * sizeof(unsigned short)];
            temp.w = Width;
            temp.h = Height;
            for (int rowIdx = 0; rowIdx < Height; rowIdx++)
            {
                k16s* data = GoSurfaceMsg_RowAt(surfaceMsg, rowIdx);
                for (int colIdx = 0; colIdx < Width; colIdx++)
                {
                    unsigned short val = data[colIdx];
                    //unsigned short val = ZOffset + ZResolution * data[colIdx];
                    temp.Tiff[rowIdx * Width + colIdx] = val;
                }
            }

            heightMat1 = cv::Mat(temp.h, temp.w, CV_16U, temp.Tiff);
            heightMat1.copyTo(heightMat2);

            m_lmiheightFlag = true;
            delete[]temp.Tiff;
            temp.Tiff = nullptr;
            /*cv::Mat dst;
            SYSTEMTIME t;
            std::vector<int> params{ (int)cv::IMWRITE_TIFF_RESUNIT,2, (int)cv::IMWRITE_TIFF_COMPRESSION,1,(int)cv::IMWRITE_TIFF_XDPI,300,(int)cv::IMWRITE_TIFF_YDPI,300 };
            GetLocalTime(&t);
            std::string filename = std::to_string(t.wHour) + "-" + std::to_string(t.wMinute) + "-" + std::to_string(t.wSecond) + "-" + std::to_string(t.wMilliseconds) + ".tiff";
            cv::imwrite(filename, img, params);*/
        }
            break;
        case GO_DATA_MESSAGE_TYPE_SURFACE_INTENSITY:
        {
            MyBmp temp;
            GoSurfaceIntensityMsg surfaceIntMsg = dataObj;
            //double ZResolution = NM_TO_MM(GoSectionMsg_ZResolution(surfaceIntMsg));
            double XResolution = NM_TO_MM(GoSurfaceIntensityMsg_XResolution(surfaceIntMsg));
            double YResolution = NM_TO_MM(GoSurfaceIntensityMsg_YResolution(surfaceIntMsg));
            double XOffset = UM_TO_MM(GoSurfaceIntensityMsg_XOffset(surfaceIntMsg));
            double YOffset = UM_TO_MM(GoSurfaceIntensityMsg_YOffset(surfaceIntMsg));

            int Width = GoSurfaceIntensityMsg_Width(surfaceIntMsg);
            int Height = GoSurfaceIntensityMsg_Length(surfaceIntMsg);

            temp.Bmp = new unsigned char[Width * Height * sizeof(unsigned char)];
            temp.w = Width;
            temp.h = Height;

            for (int rowIdx = 0; rowIdx < Height; rowIdx++)
            {
                k8u* data = GoSurfaceIntensityMsg_RowAt(surfaceIntMsg, rowIdx);
                for (int colIdx = 0; colIdx < Width; colIdx++)
                {
                    temp.Bmp[rowIdx * Width + colIdx] = data[colIdx];
                }
            }

            luminanceMat1 = cv::Mat(temp.h, temp.w, CV_8UC1, temp.Bmp);
            luminanceMat1.copyTo(luminanceMat2);
            m_lmiluminanceFlag = true;
            delete[]temp.Bmp;
            temp.Bmp = nullptr;
            //SYSTEMTIME t;
            //GetLocalTime(&t);
            //std::string filename = std::to_string(t.wHour) + "-" + std::to_string(t.wMinute) + "-" + std::to_string(t.wSecond) + "-" + std::to_string(t.wMilliseconds) + ".bmp";
            //cv::imwrite(filename, img);
        }
            break;
        }
        //DataSetCollection.push_back(dataObj);
    }

    if(!luminanceMat2.empty() && !heightMat2.empty())
    {
        //imwrite("D://CCD//3d//bmp3D_" + to_string(clock()) + ".bmp", luminanceMat2);
        //imwrite("D://CCD//3d//tiff3D_" + to_string(clock()) + ".tiff", heightMat2);
        Mat luminanceMatShow;
        //resize(luminanceMat2, luminanceMatShow, Size(luminanceMat2.cols / 2, luminanceMat2.rows / 2));
        //picVec.push_back(luminanceMatShow);

        picVec.push_back(luminanceMat2);
        picVec.push_back(heightMat2);
    }
    else
    {
        luminanceMat2 = imread("C://empty.png");
        heightMat2 = imread("C://empty2.png");
        //resize(luminanceMat2, luminanceMat2, Size(luminanceMat2.cols / 2, luminanceMat2.rows / 2));
        picVec.push_back(luminanceMat2);
        picVec.push_back(heightMat2);

    }
    {
        QMutexLocker locker(m_mutex);
        if(picVec.size() > 0)
            queuePic.push_front(picVec);
    }
    heightMat1.release();
    heightMat2.release();
    luminanceMat1.release();
    luminanceMat2.release();
    picVec.clear();
    GoDestroy(dataset);
    double time_End = (double)clock();
    qDebug() <<  "image pic:" << picIndex++ << " srcImage Exist,Time:" << (time_End - time_Start) << "ms" << "--CameraName:" << id;

    return kOK;
}


bool InitLmi(k32u ID)
{

    //return true;//edit
    kStatus status;
    if ((status = GoSdk_Construct(&api)) != kOK)
    {
        //printf("Error: GoSdk_Construct:%d\n", status);
        qDebug() << "Error: GoSdk_Construct:%d\n", status;
        return false;
    }

    if ((status = GoSystem_Construct(&system1, kNULL)) != kOK)
    {
        //printf("Error: GoSystem_Construct:%d\n", status);
        qDebug() << "Error: GoSystem_Construct:%d\n", status;
        return false;
    }

    GoState state = GoSensor_IsResponsive(sensor);
    if ((status = GoSystem_FindSensorById(system1, ID, &sensor)) != kOK)
    {
        //printf("Error: GoSystem_FindSensor:%d\n", status);
        qDebug() << "Error: GoSystem_FindSensor:%d\n", status;
        return false;
    }

    /*GoSystem_Disconnect(system1);*/
    // create connection to GoSystem object
    if ((status = GoSensor_Connect(sensor)) != kOK)
    {
        //ExceptionCB(status, NULL);
        qDebug() << "Error: GoSensor_Connect:%d\n", status;
        return false;
    }

    // enable sensor data channel
    if ((status = GoSystem_EnableData(system1, kTRUE)) != kOK)
    {
        //printf("Error: GoSensor_EnableData:%d\n", status);
        qDebug() << "Error: GoSensor_EnableData:%d\n", status;
        return false;
    }

    // set data handler to receive data asynchronously
    if ((status = GoSystem_SetDataHandler(system1, onData, contextPointer)) != kOK)
    {
        //printf("Error: GoSystem_SetDataHandler:%d\n", status);
        qDebug() << "Error: GoSystem_SetDataHandler:%d\n", status;
        return false;
    }

    if ((status = GoSystem_SetDataCapacity(system1, kSIZE_MAX)) != kOK)
    {
        qDebug() << "Error: GoSystem_SetDataCapacity:%d\n", status;
    }
    return true;
}

bool StartLmi()
{

    kStatus status;
    if ((status = GoSystem_Start(system1)) != kOK)
    {
        //printf("Error: GoSystem_Stop:%d\n", status);
        qDebug() << "Failed to start LMI ";
        return false;
    }
    return true;
}

bool StopLmi()
{
    if (system1 == kNULL)
    {
        return true;
    }

    kStatus status;
    if ((status = GoSystem_Stop(system1)) != kOK)
    {
        qDebug() << "Failed to stop LMI ";
        return false;
    }
    return true;
}

void CloseLmi()
{
    // destroy handles
    if (system1 != kNULL)
    {
        GoDestroy(system1);
        system1 = kNULL;
    }
    if (api != kNULL)
    {
        GoDestroy(api);
        api = kNULL;
    }
}

#pragma endregion


Hd_3DCameraLMI_module::Hd_3DCameraLMI_module()
{
    m_mutex = new QMutex();
    init_flag = false;
    status = true;
    moduleName = "Hd_3DCameraLMI_module";
}

Hd_3DCameraLMI_module::~Hd_3DCameraLMI_module()
{
    StopLmi();
    CloseLmi();
    if (m_mutex)
    {
        delete m_mutex;
        m_mutex = nullptr;
    }
}

bool Hd_3DCameraLMI_module::readData(std::vector<cv::Mat>& vecPic, QByteArray &data )
{

    if (data == "connect")
    {
        data = mv_Status;
        return true;
    }


    {
        QMutexLocker locker(m_mutex);
        if (queuePic.empty())
        {
            qDebug() << " srcImage is null";
            return false;
        }
        vector<cv::Mat> dstvec = queuePic.dequeue();
        queuePic.clear();
        vecPic.assign(dstvec.begin(), dstvec.end());
    }
    return true;

}

bool Hd_3DCameraLMI_module::writeData(std::vector<cv::Mat> &, QByteArray &qbyte)
{
    string reciveid = qbyte.toStdString();

    id = atoi(reciveid.c_str());

    return true;
}

bool Hd_3DCameraLMI_module::initParas(QByteArray &)
{
    bool flag = InitLmi(id); //3D初始化
    m_status = flag ? OK : UnInit;
    init_flag = flag;
    if (!flag)
    {
        qDebug() << "Failed to Init LMI,id:"<< id;
        return false;
    }

    flag = StartLmi();
    m_status = flag ? Grab : OK;
    status = flag;

    if (!flag)
    {
        qDebug() << " Failed to start LMI";
        return false;
    }

    qDebug() << " success to start LMI,id:" << id;

    return true;
}

bool Hd_3DCameraLMI_module::run()
{

    bool result = false;
    //qDebug() << "[info]" << __FUNCTION__ << " line:" <<  __LINE__ << " run start" << id;
    try
    {
        int timeOut = 0;
        {
            QMutexLocker locker(m_mutex);
            queuePic.clear();
        }
        while (timeOut < 10000)
        {
            {
                QMutexLocker locker(m_mutex);
                if (queuePic.size() > 0)
                {
                    result = true;
                    return true;
                }
            }
            timeOut += 10;
            QThread::msleep(10);
        }
    }
    catch (QString ev)
    {
        qDebug() << " ev:" << ev ;
    }
    //qDebug() << "[info]"<< __FUNCTION   _ << " line:" <<  __LINE__ << " run end" ;
    return result;
}

bool Hd_3DCameraLMI_module::checkStatus()
{
    status = (mv_Status == "connected");
    return status;
}

Hd_3DCameraLMI_module *create()
{
    qDebug() << "Hd_3DCameraLMI_module create success!" ;
    return new Hd_3DCameraLMI_module;
}

void destory(Hd_3DCameraLMI_module *ptr)
{
    if (ptr)
    {
        delete  ptr;
        ptr = nullptr;
    }
    qDebug() << "Hd_3DCameraLMI_module  destory success!";
}