初始化TOF设备并开始流式传输深度、点云和RGBD数据，以进行实时处理。
作者：yangsy
2024/11/08 19:08:22
警告
- 本文为示例文章，如开发需要请根据实际应用进行调整！
定义全局变量&常用函数
// record data frame types
std::map<unsigned int, Synexens::SYStreamType> g_mapStreamType;
// record the device's open flow status
std::map<unsigned int, bool> g_mapDeviceOpen;
// device info
Synexens::SYDeviceInfo* pDeviceInfo = nullptr;
// device count
int nDeviceCount = 0;
// error code
Synexens::SYErrorCode errorCode;
// error code print
void PrintCodeInfo(std::string strFunc, Synexens::SYErrorCode errorCode)
{
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
printf("\033[0m\033[1;32m%s code:%d\n\033[0m", strFunc.c_str(), errorCode);
}
else
{
printf("\033[0m\033[1;31m%s code:%d\n\033[0m", strFunc.c_str(), errorCode);
}
}pDeviceInfo - 设备信息，用于储存全局设备信息
nDeviceCount - 设备数量，当前能够搜索到的可用设备数量
errorCode - 错误码
PrintCodeInfo - 打印调试信息
g_mapStreamType - 记录数据帧类型
g_mapDeviceOpen - 记录设备开流状态
初始化SDK
// Initialize SDK
errorCode = Synexens::InitSDK();
if (errorCode != Synexens::SYERRORCODE_SUCCESS)
{
PrintCodeInfo("InitSDK", errorCode);
return 0;
}获取SDK版本信息
// get SDK version info
void GetSDKVersion()
{
int nSDKVersionLength = 0;
Synexens::SYErrorCode errorCodeGetSDKVersion = Synexens::GetSDKVersion(nSDKVersionLength, nullptr);
if (errorCodeGetSDKVersion == Synexens::SYERRORCODE_SUCCESS)
{
if (nSDKVersionLength > 0)
{
char* pStringSDKVersion = new char[nSDKVersionLength];
errorCodeGetSDKVersion = Synexens::GetSDKVersion(nSDKVersionLength, pStringSDKVersion);
if (errorCodeGetSDKVersion == Synexens::SYERRORCODE_SUCCESS)
{
printf("SDKVersion:%s\n", pStringSDKVersion);
}
else
{
PrintCodeInfo("GetSDKVersion2", errorCodeGetSDKVersion);
}
delete[] pStringSDKVersion;
}
}
else
{
PrintCodeInfo("GetSDKVersion", errorCodeGetSDKVersion);
}
}查找设备获取设备信息
// query the number of devices and obtain device information
bool GetDevicesInfo(int& nDeviceCount, Synexens::SYDeviceInfo** pDeviceInfo)
{
Synexens::SYErrorCode errorCode = Synexens::FindDevice(nDeviceCount);
if (errorCode == Synexens::SYERRORCODE_SUCCESS && nDeviceCount > 0)
{
*pDeviceInfo = new Synexens::SYDeviceInfo[nDeviceCount];
errorCode = Synexens::FindDevice(nDeviceCount, *pDeviceInfo);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
PrintCodeInfo("GetDeviceInfo SUCCESS", errorCode);
return true;
}
else
{
PrintCodeInfo("FindDevice devices Failed", errorCode);
return false;
}
}
else
{
PrintCodeInfo("FindDevice Count Failed", errorCode);
return false;
}
}打开设备
// open devices
bool OpenDevices(int nDeviceCount, Synexens::SYDeviceInfo* pDeviceInfo)
{
for (int nDeviceIndex = 0; nDeviceIndex < nDeviceCount; nDeviceIndex++)
{
Synexens::SYErrorCode errorCodeOpenDevice = Synexens::OpenDevice(pDeviceInfo[nDeviceIndex]);
if (errorCodeOpenDevice == Synexens::SYERRORCODE_SUCCESS)
{
PrintCodeInfo("OpenDevice SUCCESS", errorCodeOpenDevice);
return true;
}
else
{
PrintCodeInfo("OpenDevice Failed", errorCodeOpenDevice);
return false;
}
}
}获取设备详细信息（非必需）
void GetDevicesDetails(int nDeviceCount, Synexens::SYDeviceInfo* pDeviceInfo)
{
for (int nDeviceIndex = 0; nDeviceIndex < nDeviceCount; nDeviceIndex++)
{
// get device sn
int nStringLength = 0;
Synexens::SYErrorCode errorCodeGetSN = Synexens::GetDeviceSN(pDeviceInfo[nDeviceIndex].m_nDeviceID, nStringLength, nullptr);
if (errorCodeGetSN == Synexens::SYERRORCODE_SUCCESS)
{
if (nStringLength > 0)
{
char* pStringSN = new char[nStringLength];
errorCodeGetSN = Synexens::GetDeviceSN(pDeviceInfo[nDeviceIndex].m_nDeviceID, nStringLength, pStringSN);
if (errorCodeGetSN == Synexens::SYERRORCODE_SUCCESS)
{
printf("SN%d:%s\n", nDeviceIndex, pStringSN);
}
else
{
PrintCodeInfo("GetDeviceSN", errorCodeGetSN);
}
delete[] pStringSN;
}
}
else
{
PrintCodeInfo("GetDeviceSN", errorCodeGetSN);
}
// get HWVersion info
nStringLength = 0;
Synexens::SYErrorCode errorCodeGetHWVersion = Synexens::GetDeviceHWVersion(pDeviceInfo[nDeviceIndex].m_nDeviceID, nStringLength, nullptr);
if (errorCodeGetHWVersion == Synexens::SYERRORCODE_SUCCESS)
{
if (nStringLength > 0)
{
char* pStringFWVersion = new char[nStringLength];
errorCodeGetHWVersion = Synexens::GetDeviceHWVersion(pDeviceInfo[nDeviceIndex].m_nDeviceID, nStringLength, pStringFWVersion);
if (errorCodeGetHWVersion == Synexens::SYERRORCODE_SUCCESS)
{
printf("HWVersion%d:%s\n", nDeviceIndex, pStringFWVersion);
}
else
{
PrintCodeInfo("GetDeviceHWVersion2", errorCodeGetHWVersion);
}
delete[] pStringFWVersion;
}
}
else
{
PrintCodeInfo("GetDeviceHWVersion", errorCodeGetHWVersion);
}
// Query data frame support types and supported resolutions
int nSupportTypeCount = 0;
Synexens::SYErrorCode errorCodeQueryFrameType = Synexens::QueryDeviceSupportFrameType(pDeviceInfo[nDeviceIndex].m_nDeviceID, nSupportTypeCount);
if (errorCodeQueryFrameType == Synexens::SYERRORCODE_SUCCESS && nSupportTypeCount > 0)
{
Synexens::SYSupportType* pSupportType = new Synexens::SYSupportType[nSupportTypeCount];
errorCodeQueryFrameType = Synexens::QueryDeviceSupportFrameType(pDeviceInfo[nDeviceIndex].m_nDeviceID, nSupportTypeCount, pSupportType);
if (errorCodeQueryFrameType == Synexens::SYERRORCODE_SUCCESS && nSupportTypeCount > 0)
{
for (int j = 0; j < nSupportTypeCount; j++)
{
std::string sQueryFrameType = "";
switch (pSupportType[j])
{
case Synexens::SYSUPPORTTYPE_DEPTH:
{
sQueryFrameType = "depth";
break;
}
case Synexens::SYSUPPORTTYPE_RGB:
{
sQueryFrameType = "RGB";
break;
}
case Synexens::SYSUPPORTTYPE_RGBD:
{
sQueryFrameType = "RGBD";
break;
}
case Synexens::SYSUPPORTTYPE_RGBDAI:
{
sQueryFrameType = "RGBDAI";
break;
}
default:
{
sQueryFrameType = "invalid";
break;
}
}
int nResolutionCount = 0;
Synexens::SYErrorCode errorCodeQueryResolution = Synexens::QueryDeviceSupportResolution(pDeviceInfo[nDeviceIndex].m_nDeviceID, pSupportType[j], nResolutionCount);
if (errorCodeQueryResolution == Synexens::SYERRORCODE_SUCCESS && nResolutionCount > 0)
{
Synexens::SYResolution* pResolution = new Synexens::SYResolution[nResolutionCount];
errorCodeQueryResolution = Synexens::QueryDeviceSupportResolution(pDeviceInfo[nDeviceIndex].m_nDeviceID, pSupportType[j], nResolutionCount, pResolution);
if (errorCodeQueryResolution == Synexens::SYERRORCODE_SUCCESS && nResolutionCount > 0)
{
for (int k = 0; k < nResolutionCount; k++)
{
std::string sResolution = "";
switch (pResolution[k])
{
case Synexens::SYRESOLUTION_320_240:
{
sResolution = "320*240";
break;
}
case Synexens::SYRESOLUTION_640_480:
{
sResolution = "640*480";
break;
}
case Synexens::SYRESOLUTION_960_540:
{
sResolution = "960*540";
break;
}
case Synexens::SYRESOLUTION_1920_1080:
{
sResolution = "1920*1080";
break;
}
case Synexens::SYRESOLUTION_1600_1200:
{
sResolution = "1600*1200";
break;
}
default:
{
sResolution = "invalid";
break;
}
}
printf("FrameType:%s,Resolution:%s\n", sQueryFrameType.c_str(), sResolution.c_str());
}
}
else
{
PrintCodeInfo("QueryDeviceSupportResolution Failed", errorCodeQueryResolution);
}
delete[] pResolution;
}
else
{
PrintCodeInfo("QueryDeviceSupportResolution count Failed", errorCodeQueryResolution);
}
}
}
else
{
PrintCodeInfo("QueryDeviceSupportFrameType Failed", errorCodeQueryFrameType);
}
delete[] pSupportType;
}
else
{
PrintCodeInfo("QueryDeviceSupportFrameType count Failed", errorCodeQueryFrameType);
}
}
}判断设备类型并设置分辨率帧类型进行开流
void SetFrameTypeResolution(int nDeviceCount, Synexens::SYDeviceInfo* pDeviceInfo)
{
for (int nDeviceIndex = 0; nDeviceIndex < nDeviceCount; nDeviceIndex++)
{
g_mapDeviceOpen.insert(std::pair<unsigned int, bool>(pDeviceInfo[nDeviceIndex].m_nDeviceID, false));
Synexens::SYErrorCode errorCode;
switch (pDeviceInfo[nDeviceIndex].m_deviceType)
{
case Synexens::SYDEVICETYPE_CS30_SINGLE:
case Synexens::SYDEVICETYPE_CS30_DUAL:
{
errorCode = Synexens::SetFrameResolution(pDeviceInfo[nDeviceIndex].m_nDeviceID, Synexens::SYFRAMETYPE_DEPTH, Synexens::SYRESOLUTION_640_480);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
errorCode = Synexens::SetFrameResolution(pDeviceInfo[nDeviceIndex].m_nDeviceID, Synexens::SYFRAMETYPE_RGB, Synexens::SYRESOLUTION_960_540);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
Synexens::SYStreamType streamType = Synexens::SYSTREAMTYPE_RGBD;
errorCode = Synexens::StartStreaming(pDeviceInfo[nDeviceIndex].m_nDeviceID, streamType);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
auto itStreamFind = g_mapStreamType.find(pDeviceInfo[pDeviceInfo[nDeviceIndex].m_nDeviceID].m_nDeviceID);
if (itStreamFind != g_mapStreamType.end())
{
itStreamFind->second = streamType;
}
else
{
g_mapStreamType.insert(std::pair<unsigned int, Synexens::SYStreamType>(pDeviceInfo[nDeviceIndex].m_nDeviceID, streamType));
}
g_mapDeviceOpen[pDeviceInfo[nDeviceIndex].m_nDeviceID] = true;
PrintCodeInfo("StartStreaming Success", errorCode);
}
else
{
PrintCodeInfo("StartStreaming Failed", errorCode);
}
}
else
{
PrintCodeInfo("SetFrameResolution RGB Failed", errorCode);
}
}
else
{
PrintCodeInfo("SetFrameResolution Depth Failed", errorCode);
}
break;
}
case Synexens::SYDEVICETYPE_CS20_SINGLE:
{
errorCode = Synexens::SetFrameResolution(pDeviceInfo[nDeviceIndex].m_nDeviceID, Synexens::SYFRAMETYPE_DEPTH, Synexens::SYRESOLUTION_320_240);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
Synexens::SYStreamType streamType = Synexens::SYSTREAMTYPE_DEPTHIR;
errorCode = Synexens::StartStreaming(pDeviceInfo[nDeviceIndex].m_nDeviceID, streamType);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
auto itStreamFind = g_mapStreamType.find(pDeviceInfo[pDeviceInfo[nDeviceIndex].m_nDeviceID].m_nDeviceID);
if (itStreamFind != g_mapStreamType.end())
{
itStreamFind->second = streamType;
}
else
{
g_mapStreamType.insert(std::pair<unsigned int, Synexens::SYStreamType>(pDeviceInfo[nDeviceIndex].m_nDeviceID, streamType));
}
g_mapDeviceOpen[pDeviceInfo[nDeviceIndex].m_nDeviceID] = true;
PrintCodeInfo("StartStreaming Success", errorCode);
}
else
{
PrintCodeInfo("StartStreaming Failed", errorCode);
}
}
else
{
PrintCodeInfo("SetFrameResolution Depth", errorCode);
}
break;
}
case Synexens::SYDEVICETYPE_CS20_DUAL:
{
errorCode = Synexens::SetFrameResolution(pDeviceInfo[nDeviceIndex].m_nDeviceID, Synexens::SYFRAMETYPE_DEPTH, Synexens::SYRESOLUTION_320_240);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
Synexens::SYStreamType streamType = Synexens::SYSTREAMTYPE_RAW;
errorCode = Synexens::StartStreaming(pDeviceInfo[nDeviceIndex].m_nDeviceID, streamType);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
auto itStreamFind = g_mapStreamType.find(pDeviceInfo[pDeviceInfo[nDeviceIndex].m_nDeviceID].m_nDeviceID);
if (itStreamFind != g_mapStreamType.end())
{
itStreamFind->second = streamType;
}
else
{
g_mapStreamType.insert(std::pair<unsigned int, Synexens::SYStreamType>(pDeviceInfo[nDeviceIndex].m_nDeviceID, streamType));
}
g_mapDeviceOpen[pDeviceInfo[nDeviceIndex].m_nDeviceID] = true;
PrintCodeInfo("StartStreaming Success", errorCode);
}
else
{
PrintCodeInfo("StartStreaming Failed", errorCode);
}
}
else
{
PrintCodeInfo("SetFrameResolution Depth", errorCode);
}
break;
}
case Synexens::SYDEVICETYPE_CS20_P:
{
errorCode = Synexens::SetFrameResolution(pDeviceInfo[nDeviceIndex].m_nDeviceID, Synexens::SYFRAMETYPE_DEPTH, Synexens::SYRESOLUTION_320_240);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
Synexens::SYStreamType streamType = Synexens::SYSTREAMTYPE_DEPTH;
errorCode = Synexens::StartStreaming(pDeviceInfo[nDeviceIndex].m_nDeviceID, streamType);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
auto itStreamFind = g_mapStreamType.find(pDeviceInfo[pDeviceInfo[nDeviceIndex].m_nDeviceID].m_nDeviceID);
if (itStreamFind != g_mapStreamType.end())
{
itStreamFind->second = streamType;
}
else
{
g_mapStreamType.insert(std::pair<unsigned int, Synexens::SYStreamType>(pDeviceInfo[nDeviceIndex].m_nDeviceID, streamType));
}
g_mapDeviceOpen[pDeviceInfo[nDeviceIndex].m_nDeviceID] = true;
PrintCodeInfo("StartStreaming Success", errorCode);
}
else
{
PrintCodeInfo("StartStreaming Failed", errorCode);
}
}
else
{
PrintCodeInfo("SetFrameResolution Depth", errorCode);
}
break;
}
case Synexens::SYDEVICETYPE_CS40:
{
errorCode = Synexens::SetFrameResolution(pDeviceInfo[nDeviceIndex].m_nDeviceID, Synexens::SYFRAMETYPE_DEPTH, Synexens::SYRESOLUTION_640_480);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
Synexens::SYStreamType streamType = Synexens::SYSTREAMTYPE_DEPTH;
errorCode = Synexens::StartStreaming(pDeviceInfo[nDeviceIndex].m_nDeviceID, streamType);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
auto itStreamFind = g_mapStreamType.find(pDeviceInfo[pDeviceInfo[nDeviceIndex].m_nDeviceID].m_nDeviceID);
if (itStreamFind != g_mapStreamType.end())
{
itStreamFind->second = streamType;
}
else
{
g_mapStreamType.insert(std::pair<unsigned int, Synexens::SYStreamType>(pDeviceInfo[nDeviceIndex].m_nDeviceID, streamType));
}
g_mapDeviceOpen[pDeviceInfo[nDeviceIndex].m_nDeviceID] = true;
PrintCodeInfo("StartStreaming Success", errorCode);
}
else
{
PrintCodeInfo("StartStreaming Failed", errorCode);
}
}
else
{
PrintCodeInfo("SetFrameResolution Depth", errorCode);
}
break;
}
case Synexens::SYDEVICETYPE_CS40PRO:
{
errorCode = Synexens::SetFrameResolution(pDeviceInfo[nDeviceIndex].m_nDeviceID, Synexens::SYFRAMETYPE_DEPTH, Synexens::SYRESOLUTION_640_480);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
errorCode = Synexens::SetFrameResolution(pDeviceInfo[nDeviceIndex].m_nDeviceID, Synexens::SYFRAMETYPE_RGB, Synexens::SYRESOLUTION_1600_1200);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
Synexens::SYStreamType streamType = Synexens::SYSTREAMTYPE_DEPTH;
errorCode = Synexens::StartStreaming(pDeviceInfo[nDeviceIndex].m_nDeviceID, streamType);
if (errorCode == Synexens::SYERRORCODE_SUCCESS)
{
auto itStreamFind = g_mapStreamType.find(pDeviceInfo[pDeviceInfo[nDeviceIndex].m_nDeviceID].m_nDeviceID);
if (itStreamFind != g_mapStreamType.end())
{
itStreamFind->second = streamType;
}
else
{
g_mapStreamType.insert(std::pair<unsigned int, Synexens::SYStreamType>(pDeviceInfo[nDeviceIndex].m_nDeviceID, streamType));
}
g_mapDeviceOpen[pDeviceInfo[nDeviceIndex].m_nDeviceID] = true;
PrintCodeInfo("StartStreaming Success", errorCode);
}
else
{
PrintCodeInfo("StartStreaming Failed", errorCode);
}
}
else
{
PrintCodeInfo("SetFrameResolution RGB Failed", errorCode);
}
}
else
{
PrintCodeInfo("SetFrameResolution Depth Failed", errorCode);
}
break;
}
}
}
}