301 lines
8.2 KiB
C++
301 lines
8.2 KiB
C++
#include <iostream>
|
|
#include <vector>
|
|
#include <thread>
|
|
#include <chrono>
|
|
#include <atomic>
|
|
#include <mutex>
|
|
|
|
// VzNLSDK includes
|
|
#include "../../SDK/VzNLSDK/Inc/VZNL_Common.h"
|
|
#include "../../SDK/VzNLSDK/Inc/VZNL_Types.h"
|
|
#include "../../SDK/VzNLSDK/Inc/VZNL_DetectLaser.h"
|
|
#include "../../SDK/VzNLSDK/Inc/VZNL_RGBConfig.h"
|
|
#include "../../SDK/VzNLSDK/Inc/VZNL_SwingMotor.h"
|
|
|
|
class CameraDemoTest
|
|
{
|
|
private:
|
|
VZNLHANDLE m_hDevice;
|
|
std::atomic<bool> m_bScanning;
|
|
std::atomic<int> m_nScanDataCount;
|
|
bool m_isRGBD;
|
|
|
|
public:
|
|
CameraDemoTest()
|
|
: m_hDevice(nullptr)
|
|
, m_bScanning(false)
|
|
, m_nScanDataCount(0)
|
|
, m_isRGBD(false)
|
|
{
|
|
}
|
|
|
|
~CameraDemoTest()
|
|
{
|
|
Cleanup();
|
|
}
|
|
|
|
// Initialize camera
|
|
bool InitializeCamera(bool isRGBD)
|
|
{
|
|
m_isRGBD = isRGBD;
|
|
int nErrCode = 0;
|
|
|
|
// 初始化SDK
|
|
SVzNLConfigParam sVzNlConfigParam;
|
|
sVzNlConfigParam.nDeviceTimeOut = 5000; // 5 seconds timeout
|
|
nErrCode = VzNL_Init(&sVzNlConfigParam);
|
|
|
|
// 搜索设备
|
|
VzNL_ResearchDevice(keSearchDeviceFlag_EthLaserRobotEye);
|
|
|
|
// 获取搜索到的设备
|
|
SVzNLEyeCBInfo* pCBInfo = nullptr;
|
|
int nCount = 0;
|
|
nErrCode = VzNL_GetEyeCBDeviceInfo(nullptr, &nCount);
|
|
if (0 != nErrCode)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if(0 == nCount)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
pCBInfo = new SVzNLEyeCBInfo[nCount];
|
|
if (!pCBInfo) {
|
|
return false;
|
|
}
|
|
|
|
nErrCode = VzNL_GetEyeCBDeviceInfo(pCBInfo, &nCount);
|
|
if (0 != nErrCode) {
|
|
delete[] pCBInfo;
|
|
return false;
|
|
}
|
|
|
|
SVzNLOpenDeviceParam sVzNlOpenDeviceParam;
|
|
sVzNlOpenDeviceParam.eEyeResolution = keVzNLEyeResolution_Unknown;
|
|
sVzNlOpenDeviceParam.eSDKType = keSDKType_DetectLaser;
|
|
sVzNlOpenDeviceParam.bStopStream = VzTrue;
|
|
|
|
m_hDevice = VzNL_OpenDevice(&pCBInfo[0], &sVzNlOpenDeviceParam, &nErrCode);
|
|
|
|
|
|
// 清理pCBInfo数组
|
|
delete[] pCBInfo;
|
|
pCBInfo = nullptr;
|
|
|
|
|
|
if(nullptr == m_hDevice)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if(0 != nErrCode)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
SVzNLEyeDeviceInfoEx m_sEeyCBDeviceInfo;
|
|
m_sEeyCBDeviceInfo.sEyeCBInfo.nSize = sizeof(SVzNLEyeDeviceInfoEx);
|
|
VzNL_GetDeviceInfo(m_hDevice, (SVzNLEyeCBInfo *)(&m_sEeyCBDeviceInfo));
|
|
|
|
VzNL_BeginDetectLaser(m_hDevice);
|
|
VzNL_EnableSwingMotor(m_hDevice, VzTrue);
|
|
|
|
SVzNLVersionInfo sVersionInfo;
|
|
VzNL_GetVersion(m_hDevice, &sVersionInfo);
|
|
std::cout << "version : " << sVersionInfo.szSDKVersion << std::endl;
|
|
|
|
// int nnnRet = VzNL_SetEthSendDataLength(m_pHandle, 1024);
|
|
// LOG_DEBUG("SenddataLen ret : %d\n", nnnRet);
|
|
|
|
//启用RGBD
|
|
int nRGBRet = VzNL_EnableRGB(m_hDevice, isRGBD ? VzTrue : VzFalse);
|
|
std::cout << "Enable RGB " << (isRGBD ? "true" : "false") << " ret : " << nRGBRet << std::endl;
|
|
|
|
// 填充
|
|
VzNL_EnableFillLaserPoint(m_hDevice, VzTrue);
|
|
|
|
VzNL_SetDeviceStatusNotifyEx(m_hDevice, StatusCallback, this);
|
|
|
|
return true;
|
|
}
|
|
|
|
// Start scanning
|
|
bool StartScanning()
|
|
{
|
|
std::cout << "\n--- Starting Scan ---" << std::endl;
|
|
|
|
if (!m_hDevice)
|
|
{
|
|
std::cout << "Error: Device not opened" << std::endl;
|
|
return false;
|
|
}
|
|
|
|
// Clear previous data
|
|
m_nScanDataCount = 0;
|
|
|
|
// Select data type based on scan mode
|
|
EVzResultDataType dataType;
|
|
dataType = m_isRGBD ? keResultDataType_PointXYZRGBA : keResultDataType_Position;
|
|
|
|
m_bScanning = true;
|
|
|
|
// Start automatic detection
|
|
int ret = VzNL_StartAutoDetectEx(m_hDevice, dataType, keFlipType_None, ScanDataCallback, this);
|
|
if (ret != 0)
|
|
{
|
|
m_bScanning = false;
|
|
std::cout << "Error: Failed to start scanning, error code: " << ret << std::endl;
|
|
return false;
|
|
}
|
|
|
|
std::cout << "✓ Scanning started, data type: " << (m_isRGBD ? "RGBD" : "Position") << std::endl;
|
|
|
|
return true;
|
|
}
|
|
|
|
// Stop scanning
|
|
bool StopScanning()
|
|
{
|
|
std::cout << "\n--- Stopping Scan ---" << std::endl;
|
|
|
|
if (!m_hDevice)
|
|
{
|
|
std::cout << "Error: Device not opened" << std::endl;
|
|
return false;
|
|
}
|
|
|
|
m_bScanning = false;
|
|
|
|
int ret = VzNL_StopAutoDetect(m_hDevice);
|
|
if (ret != 0)
|
|
{
|
|
std::cout << "Error: Failed to stop scanning, error code: " << ret << std::endl;
|
|
return false;
|
|
}
|
|
|
|
VzNL_EndDetectLaser(m_hDevice);
|
|
|
|
std::cout << "✓ Scanning stopped" << std::endl;
|
|
return true;
|
|
}
|
|
|
|
bool IsScanning()
|
|
{
|
|
return m_bScanning.load();
|
|
}
|
|
|
|
// Get scan data count
|
|
int GetScanDataCount()
|
|
{
|
|
return m_nScanDataCount.load();
|
|
}
|
|
|
|
// Cleanup resources
|
|
void Cleanup()
|
|
{
|
|
if (m_hDevice)
|
|
{
|
|
if (m_bScanning)
|
|
{
|
|
VzNL_StopAutoDetect(m_hDevice);
|
|
}
|
|
VzNL_CloseDevice(m_hDevice);
|
|
m_hDevice = nullptr;
|
|
}
|
|
|
|
// Destroy SDK
|
|
VzNL_Destroy();
|
|
}
|
|
|
|
private:
|
|
// Status callback function
|
|
static void StatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pParam)
|
|
{
|
|
CameraDemoTest* pThis = static_cast<CameraDemoTest*>(pParam);
|
|
if (!pThis) return;
|
|
|
|
std::cout << "Device status change: ";
|
|
switch (eStatus)
|
|
{
|
|
case keDeviceWorkStatus_Eye_Comming:
|
|
std::cout << "Eye connected" << std::endl;
|
|
break;
|
|
case keDeviceWorkStatus_Eye_Reconnect:
|
|
std::cout << "Eye reconnected" << std::endl;
|
|
break;
|
|
case keDeviceWorkStatus_Working:
|
|
std::cout << "Working" << std::endl;
|
|
break;
|
|
case keDeviceWorkStatus_Offline:
|
|
std::cout << "Offline" << std::endl;
|
|
break;
|
|
case keDeviceWorkStatus_Device_Swing_Finish:
|
|
std::cout << "Swing module completed" << std::endl;
|
|
break;
|
|
case keDeviceWorkStatus_Device_Auto_Stop:
|
|
std::cout << "Device auto stopped, total scan data: " << pThis->GetScanDataCount() << std::endl;
|
|
pThis->m_bScanning = false;
|
|
break;
|
|
default:
|
|
std::cout << "Unknown status: " << eStatus << std::endl;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Scan data callback function
|
|
static void ScanDataCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint, void* pParam)
|
|
{
|
|
CameraDemoTest* pThis = static_cast<CameraDemoTest*>(pParam);
|
|
if (!pThis || !pLaserLinePoint) return;
|
|
|
|
// Increment data count
|
|
pThis->m_nScanDataCount++;
|
|
}
|
|
};
|
|
|
|
int main(int argc, char* argv[])
|
|
{
|
|
std::cout << "Camera Demo Test Program (Direct VzNLSDK)" << std::endl;
|
|
std::cout << "=========================================" << std::endl;
|
|
|
|
CameraDemoTest cameraTest;
|
|
|
|
// Check command line argument for RGBD mode
|
|
bool isRGBD = (argc > 1 && std::string(argv[1]) == "rgbd");
|
|
std::cout << "Scan mode: " << (isRGBD ? "RGBD" : "Position") << std::endl;
|
|
|
|
// Initialize camera
|
|
if (!cameraTest.InitializeCamera(isRGBD))
|
|
{
|
|
std::cout << "Failed to initialize camera" << std::endl;
|
|
return -1;
|
|
}
|
|
|
|
// Start scanning
|
|
if (!cameraTest.StartScanning())
|
|
{
|
|
std::cout << "Failed to start scanning" << std::endl;
|
|
return -1;
|
|
}
|
|
|
|
// Wait for scanning to complete
|
|
std::cout << "Scanning in progress..." << std::endl;
|
|
while (cameraTest.IsScanning())
|
|
{
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
}
|
|
|
|
// Stop scanning
|
|
cameraTest.StopScanning();
|
|
|
|
// Display final results
|
|
std::cout << "\n=== Final Results ===" << std::endl;
|
|
std::cout << "Total scan data received: " << cameraTest.GetScanDataCount() << std::endl;
|
|
|
|
std::cout << "\nAll tests completed!" << std::endl;
|
|
return 0;
|
|
}
|