574 lines
16 KiB
C++
574 lines
16 KiB
C++
#include "GlLineLaserDevice.h"
|
||
#include "VrError.h"
|
||
#include "VrLog.h"
|
||
|
||
#include <cstring>
|
||
#include <chrono>
|
||
#include <algorithm>
|
||
|
||
// 静态成员初始化
|
||
CGlLineLaserDevice* CGlLineLaserDevice::s_pInstance = nullptr;
|
||
|
||
CGlLineLaserDevice::CGlLineLaserDevice()
|
||
: m_nDeviceId(0)
|
||
, m_bDeviceOpen(false)
|
||
, m_nProfileWidth(4096)
|
||
, m_dXPitch(0.01)
|
||
, m_dYPitch(1.0)
|
||
, m_nBatchLines(200)
|
||
{
|
||
memset(&m_modelInfo, 0, sizeof(GLX8_2_ModelInfo));
|
||
}
|
||
|
||
CGlLineLaserDevice::~CGlLineLaserDevice()
|
||
{
|
||
if (m_bDeviceOpen) {
|
||
CloseDevice();
|
||
}
|
||
}
|
||
|
||
int CGlLineLaserDevice::InitDevice()
|
||
{
|
||
// 初始化 gl_linelaser_sdk
|
||
int ret = GLX8_2_Initialize();
|
||
if (ret != 0) {
|
||
LOG_ERROR("GLX8_2_Initialize failed: %d\n", ret);
|
||
return ERR_CODE(DEV_OPEN_ERR);
|
||
}
|
||
|
||
LOG_DEBUG("GLX8_2_Initialize success, SDK version: %s\n", GLX8_2_GetVersion());
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::SetStatusCallback(VzNL_OnNotifyStatusCBEx fNotify, void *param)
|
||
{
|
||
m_pStatusCallback = fNotify;
|
||
m_pStatusCallbackParam = param;
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::OpenDevice(const char* sIP, bool bRGBD, bool bSwing, bool bFillLaser)
|
||
{
|
||
// gl_linelaser_sdk 不支持 RGBD 和摆动模式，忽略这些参数
|
||
(void)bRGBD;
|
||
(void)bSwing;
|
||
(void)bFillLaser;
|
||
|
||
if (m_bDeviceOpen) {
|
||
LOG_WARNING("Device already open\n");
|
||
return SUCCESS;
|
||
}
|
||
|
||
|
||
// 解析IP地址
|
||
GLX8_2_ETHERNET_CONFIG ethConfig;
|
||
memset(ðConfig, 0, sizeof(ethConfig));
|
||
|
||
if (sIP && strlen(sIP) > 0) {
|
||
LOG_ERROR("open IP address format: %s\n", sIP);
|
||
// 解析IP字符串 "x.x.x.x"
|
||
int ip[4];
|
||
if (sscanf(sIP, "%d.%d.%d.%d", &ip[0], &ip[1], &ip[2], &ip[3]) == 4) {
|
||
ethConfig.abyIpAddress[0] = (unsigned char)ip[0];
|
||
ethConfig.abyIpAddress[1] = (unsigned char)ip[1];
|
||
ethConfig.abyIpAddress[2] = (unsigned char)ip[2];
|
||
ethConfig.abyIpAddress[3] = (unsigned char)ip[3];
|
||
m_strDeviceIP = sIP;
|
||
} else {
|
||
LOG_ERROR("Invalid IP address format: %s\n", sIP);
|
||
return ERR_CODE(DEV_ARG_INVAILD);
|
||
}
|
||
} else {
|
||
// 搜索在线设备
|
||
int count = 0;
|
||
GLX8_2_ETHERNET_CONFIG* pDevices = GLX8_2_SearchOnline(&count, 3000);
|
||
if (count == 0 || pDevices == nullptr) {
|
||
LOG_ERROR("No device found\n");
|
||
return ERR_CODE(DEV_NOT_FIND);
|
||
}
|
||
|
||
// 使用第一个找到的设备
|
||
memcpy(ðConfig, &pDevices[0], sizeof(GLX8_2_ETHERNET_CONFIG));
|
||
char ipStr[32];
|
||
sprintf(ipStr, "%d.%d.%d.%d", ethConfig.abyIpAddress[0], ethConfig.abyIpAddress[1], ethConfig.abyIpAddress[2], ethConfig.abyIpAddress[3]);
|
||
m_strDeviceIP = ipStr;
|
||
LOG_DEBUG("Found device: %s\n", m_strDeviceIP.c_str());
|
||
}
|
||
|
||
// 打开设备
|
||
int ret = GLX8_2_EthernetOpen(m_nDeviceId, ðConfig);
|
||
if (ret != 0) {
|
||
LOG_ERROR("GLX8_2_Ethernet Open failed: %d\n", ret);
|
||
return ERR_CODE(DEV_OPEN_ERR);
|
||
}
|
||
|
||
m_bDeviceOpen = true;
|
||
LOG_DEBUG("Device opened: %s\n", m_strDeviceIP.c_str());
|
||
|
||
// 获取设备信息
|
||
ret = GLX8_2_GetModelInfos(m_nDeviceId, &m_modelInfo);
|
||
if (ret == 0) {
|
||
m_nProfileWidth = m_modelInfo.ProfileDataWidth;
|
||
m_dXPitch = m_modelInfo.xPixth;
|
||
if (m_modelInfo.yPixth > 0) {
|
||
m_dYPitch = m_modelInfo.yPixth;
|
||
}
|
||
LOG_DEBUG("Device model: %s, width: %d, xPitch: %.4f, yPitch: %.4f\n",
|
||
m_modelInfo.Model, m_nProfileWidth, m_dXPitch, m_dYPitch);
|
||
}
|
||
|
||
// 分配数据缓存
|
||
m_profileBuffer.resize(static_cast<size_t>(m_nProfileWidth) * m_nBatchLines);
|
||
m_intensityBuffer.resize(static_cast<size_t>(m_nProfileWidth) * m_nBatchLines);
|
||
m_positionBuffer.resize(m_nProfileWidth);
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::GetVersion(SVzNLVersionInfo& sVersionInfo)
|
||
{
|
||
memset(&sVersionInfo, 0, sizeof(SVzNLVersionInfo));
|
||
|
||
const char* sdkVersion = GLX8_2_GetVersion();
|
||
if (sdkVersion) {
|
||
strncpy(sVersionInfo.szSDKVersion, sdkVersion, VZNL_VERSION_LENGTH - 1);
|
||
}
|
||
|
||
// 填充其他版本信息
|
||
strncpy(sVersionInfo.szAppVersion, "GlLineLaser", VZNL_VERSION_LENGTH - 1);
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::GetDevInfo(SVzNLEyeDeviceInfoEx& sDeviceInfo)
|
||
{
|
||
memset(&sDeviceInfo, 0, sizeof(SVzNLEyeDeviceInfoEx));
|
||
|
||
// 填充设备信息
|
||
strncpy(sDeviceInfo.sEyeCBInfo.byServerIP, m_strDeviceIP.c_str(), VZNL_SDK_NETWORK_IPv4_LENGTH - 1);
|
||
strncpy(sDeviceInfo.sEyeCBInfo.szDeviceName, m_modelInfo.Model, VZNL_DEVICE_NAME_LENGTH - 1);
|
||
strncpy(sDeviceInfo.sEyeCBInfo.szDeviceID, m_modelInfo.HeaderSerial, VZNL_GUID_LENGTH - 1);
|
||
|
||
// 设置分辨率
|
||
sDeviceInfo.sVideoRes.nFrameWidth = m_nProfileWidth;
|
||
sDeviceInfo.sVideoRes.nFrameHeight = m_nBatchLines;
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::CloseDevice()
|
||
{
|
||
if (!m_bDeviceOpen) {
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 先停止检测
|
||
if (m_bDetecting) {
|
||
StopDetect();
|
||
}
|
||
|
||
// 关闭设备
|
||
int ret = GLX8_2_CommClose(m_nDeviceId);
|
||
if (ret != 0) {
|
||
LOG_ERROR("GLX8_2_CommClose failed: %d\n", ret);
|
||
}
|
||
|
||
m_bDeviceOpen = false;
|
||
LOG_DEBUG("Device closed\n");
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::StartDetect(VzNL_AutoOutputLaserLineExCB fCallFunc, EVzResultDataType eDataType, void *param)
|
||
{
|
||
if (!m_bDeviceOpen) {
|
||
LOG_ERROR("Device not open\n");
|
||
return ERR_CODE(DEV_NO_OPEN);
|
||
}
|
||
|
||
if (m_bDetecting) {
|
||
LOG_WARNING("Already detecting\n");
|
||
return SUCCESS;
|
||
}
|
||
|
||
m_pDetectCallback = fCallFunc;
|
||
m_pDetectCallbackParam = param;
|
||
m_eDataType = eDataType;
|
||
m_bStopDetect = false;
|
||
m_ullFrameIndex = 0;
|
||
|
||
// 设置回调实例指针
|
||
s_pInstance = this;
|
||
|
||
// 设置批处理回调
|
||
int ret = GLX8_2_SetBatchOneTimeDataHandler(m_nDeviceId, BatchDataCallback);
|
||
if (ret != 0) {
|
||
LOG_ERROR("GLX8_2_SetBatchOneTimeDataHandler failed: %d\n", ret);
|
||
return ERR_CODE(DEV_CTRL_ERR);
|
||
}
|
||
|
||
// 开始批处理采集（立即开始）
|
||
ret = GLX8_2_StartMeasureWithCallback(m_nDeviceId, 0);
|
||
if (ret != 0) {
|
||
LOG_ERROR("GLX8_2_StartMeasureWithCallback failed: %d\n", ret);
|
||
return ERR_CODE(DEV_CTRL_ERR);
|
||
}
|
||
|
||
m_bDetecting = true;
|
||
LOG_DEBUG("Detection started\n");
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
bool CGlLineLaserDevice::IsDetectIng()
|
||
{
|
||
return m_bDetecting;
|
||
}
|
||
|
||
int CGlLineLaserDevice::StopDetect()
|
||
{
|
||
if (!m_bDetecting) {
|
||
return SUCCESS;
|
||
}
|
||
|
||
m_bStopDetect = true;
|
||
|
||
// 停止批处理
|
||
int ret = GLX8_2_StopMeasure(m_nDeviceId);
|
||
if (ret != 0) {
|
||
LOG_ERROR("GLX8_2_StopMeasure failed: %d\n", ret);
|
||
}
|
||
|
||
m_bDetecting = false;
|
||
s_pInstance = nullptr;
|
||
|
||
// 通知状态变化
|
||
if (m_pStatusCallback) {
|
||
m_pStatusCallback(keDeviceWorkStatus_Device_Swing_Finish, nullptr, 0, m_pStatusCallbackParam);
|
||
m_pStatusCallback(keDeviceWorkStatus_Device_Auto_Stop, nullptr, 0, m_pStatusCallbackParam);
|
||
}
|
||
|
||
LOG_DEBUG("Detection stopped\n");
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 批处理数据回调（静态函数）
|
||
void CGlLineLaserDevice::BatchDataCallback(const GLX8_2_STR_CALLBACK_INFO* info, const GLX8_2_Data DataObj)
|
||
{
|
||
if (s_pInstance) {
|
||
s_pInstance->ProcessBatchData(info, DataObj);
|
||
}
|
||
}
|
||
|
||
// 处理批处理数据
|
||
void CGlLineLaserDevice::ProcessBatchData(const GLX8_2_STR_CALLBACK_INFO* info, const GLX8_2_Data DataObj)
|
||
{
|
||
if (!m_pDetectCallback || m_bStopDetect) {
|
||
return;
|
||
}
|
||
|
||
// 获取批处理轮廓数据
|
||
int32_t* profileData = GLX8_2_GetBatchProfilePoint(DataObj, 0);
|
||
uint8_t* intensityData = GLX8_2_GetBatchIntensityPoint(DataObj, 0);
|
||
|
||
if (!profileData) {
|
||
LOG_WARNING("No profile data in batch\n");
|
||
return;
|
||
}
|
||
|
||
int width = info->xPoints;
|
||
int batchCount = info->BatchPoints;
|
||
LOG_DEBUG("BatchPoints: %d, xPoints: %d\n", batchCount, width);
|
||
|
||
// 逐行处理数据并回调
|
||
for (int lineIdx = 0; lineIdx < batchCount; lineIdx++) {
|
||
// 转换当前行的数据
|
||
const int32_t* lineProfile = profileData + static_cast<size_t>(lineIdx) * width;
|
||
ConvertProfileToPosition(lineProfile, width, lineIdx);
|
||
|
||
// 填充 SVzLaserLineData 结构
|
||
SVzLaserLineData laserLineData;
|
||
memset(&laserLineData, 0, sizeof(SVzLaserLineData));
|
||
|
||
laserLineData.p3DPoint = m_positionBuffer.data();
|
||
laserLineData.p2DPoint = nullptr; // 线激光没有2D数据
|
||
laserLineData.nPointCount = width;
|
||
laserLineData.dTotleOffset = static_cast<double>(m_ullFrameIndex) * m_dYPitch;
|
||
laserLineData.dStep = m_dYPitch;
|
||
laserLineData.llFrameIdx = m_ullFrameIndex;
|
||
laserLineData.llTimeStamp = std::chrono::duration_cast<std::chrono::microseconds>(
|
||
std::chrono::steady_clock::now().time_since_epoch()).count();
|
||
laserLineData.nEncodeNo = info->startEncoder + lineIdx;
|
||
laserLineData.fSwingAngle = 0.0f; // 线激光没有摆动角度
|
||
laserLineData.bEndOnceScan = (lineIdx == batchCount - 1) ? VzTrue : VzFalse;
|
||
|
||
// 回调
|
||
m_pDetectCallback(m_eDataType, &laserLineData, m_pDetectCallbackParam);
|
||
|
||
m_ullFrameIndex++;
|
||
}
|
||
|
||
// 如果是最后一个批处理，通知完成
|
||
if (info->returnStatus != 0) {
|
||
StopDetect();
|
||
}
|
||
}
|
||
|
||
// 将轮廓数据转换为位置数据
|
||
void CGlLineLaserDevice::ConvertProfileToPosition(const int32_t* profileData, int count, int lineIndex)
|
||
{
|
||
// 确保缓存足够大
|
||
if (m_positionBuffer.size() < static_cast<size_t>(count)) {
|
||
m_positionBuffer.resize(count);
|
||
}
|
||
|
||
// 计算Y偏移（基于行索引）
|
||
double yOffset = static_cast<double>(m_ullFrameIndex) * m_dYPitch;
|
||
|
||
// 转换每个点
|
||
for (int i = 0; i < count; i++) {
|
||
SVzNL3DPosition& pos = m_positionBuffer[i];
|
||
|
||
pos.nPointIdx = i;
|
||
|
||
// X 坐标：根据点索引和X间距计算
|
||
// X范围居中：从 -width/2 * xPitch 到 width/2 * xPitch
|
||
pos.pt3D.x = (static_cast<double>(i) - static_cast<double>(count) / 2.0) * m_dXPitch;
|
||
|
||
// Y 坐标：扫描方向偏移
|
||
pos.pt3D.y = yOffset;
|
||
|
||
// Z 坐标：高度值，gl_linelaser_sdk 单位是 0.01um，转换为 mm
|
||
// 无效值通常是最大值或特定值，这里假设 0x7FFFFFFF 或负值为无效
|
||
int32_t rawZ = profileData[i];
|
||
if (rawZ == 0x7FFFFFFF || rawZ < -100000000) {
|
||
pos.pt3D.z = 0.0; // 无效值设为0
|
||
} else {
|
||
pos.pt3D.z = static_cast<double>(rawZ) * 0.00001; // 0.01um -> mm
|
||
}
|
||
}
|
||
}
|
||
|
||
// ============ ROI相关（线激光不支持）============
|
||
|
||
int CGlLineLaserDevice::SetDetectROI(SVzNLRect& leftROI, SVzNLRect& rightROI)
|
||
{
|
||
(void)leftROI;
|
||
(void)rightROI;
|
||
// 线激光不支持ROI设置
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::GetDetectROI(SVzNLRect& leftROI, SVzNLRect& rightROI)
|
||
{
|
||
memset(&leftROI, 0, sizeof(SVzNLRect));
|
||
memset(&rightROI, 0, sizeof(SVzNLRect));
|
||
return SUCCESS;
|
||
}
|
||
|
||
// ============ 曝光/增益相关（线激光通过SDK设置）============
|
||
|
||
int CGlLineLaserDevice::SetEyeExpose(unsigned int& exposeTime)
|
||
{
|
||
(void)exposeTime;
|
||
// gl_linelaser_sdk 通过 GLX8_2_SetSetting 设置曝光
|
||
// 暂不实现详细接口
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::GetEyeExpose(unsigned int& exposeTime)
|
||
{
|
||
exposeTime = 1000; // 默认值
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::SetEyeGain(unsigned int& gain)
|
||
{
|
||
(void)gain;
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::GetEyeGain(unsigned int& gain)
|
||
{
|
||
gain = 100; // 默认值
|
||
return SUCCESS;
|
||
}
|
||
|
||
// ============ 帧率相关 ============
|
||
|
||
int CGlLineLaserDevice::SetFrame(int& frame)
|
||
{
|
||
(void)frame;
|
||
// 线激光帧率由硬件决定
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::GetFrame(int& frame)
|
||
{
|
||
frame = 100; // 默认帧率
|
||
return SUCCESS;
|
||
}
|
||
|
||
// ============ RGBD相关（线激光不支持）============
|
||
|
||
bool CGlLineLaserDevice::IsSupport()
|
||
{
|
||
return false; // 不支持RGBD
|
||
}
|
||
|
||
int CGlLineLaserDevice::SetRGBDExposeThres(float& value)
|
||
{
|
||
(void)value;
|
||
return ERR_CODE(DEV_UNSUPPORT);
|
||
}
|
||
|
||
int CGlLineLaserDevice::GetRGBDExposeThres(float& value)
|
||
{
|
||
value = 0.0f;
|
||
return ERR_CODE(DEV_UNSUPPORT);
|
||
}
|
||
|
||
// ============ 过滤高度 ============
|
||
|
||
int CGlLineLaserDevice::SetFilterHeight(double& dHeight)
|
||
{
|
||
(void)dHeight;
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::GetFilterHeight(double& dHeight)
|
||
{
|
||
dHeight = 0.0;
|
||
return SUCCESS;
|
||
}
|
||
|
||
// ============ 摆动机构相关（线激光不支持）============
|
||
|
||
int CGlLineLaserDevice::GetSwingSpeed(float& fSpeed)
|
||
{
|
||
fSpeed = 0.0f;
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::SetSwingSpeed(float& fSpeed)
|
||
{
|
||
(void)fSpeed;
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::SetSwingAngle(float& fMin, float& fMax)
|
||
{
|
||
(void)fMin;
|
||
(void)fMax;
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::GetSwingAngle(float& fMin, float& fMax)
|
||
{
|
||
fMin = 0.0f;
|
||
fMax = 0.0f;
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::SetWorkRange(double& dMin, double& dMax)
|
||
{
|
||
(void)dMin;
|
||
(void)dMax;
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::GetWorkRange(double& dMin, double& dMax)
|
||
{
|
||
// 从设备信息获取工作范围
|
||
dMin = m_modelInfo.zRangmin;
|
||
dMax = m_modelInfo.zRangmax;
|
||
return SUCCESS;
|
||
}
|
||
|
||
// ============ GL线激光专用接口实现 ============
|
||
|
||
int CGlLineLaserDevice::GetProfileDataWidth()
|
||
{
|
||
return m_nProfileWidth;
|
||
}
|
||
|
||
double CGlLineLaserDevice::GetXPitch()
|
||
{
|
||
return m_dXPitch;
|
||
}
|
||
|
||
double CGlLineLaserDevice::GetYPitch()
|
||
{
|
||
return m_dYPitch;
|
||
}
|
||
|
||
int CGlLineLaserDevice::SetYPitch(double pitch)
|
||
{
|
||
if (pitch <= 0) {
|
||
return ERR_CODE(DEV_ARG_INVAILD);
|
||
}
|
||
m_dYPitch = pitch;
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::SetBatchLines(unsigned int batchLines)
|
||
{
|
||
if (batchLines == 0) {
|
||
return ERR_CODE(DEV_ARG_INVAILD);
|
||
}
|
||
m_nBatchLines = batchLines;
|
||
|
||
// 重新分配缓存
|
||
m_profileBuffer.resize(static_cast<size_t>(m_nProfileWidth) * m_nBatchLines);
|
||
m_intensityBuffer.resize(static_cast<size_t>(m_nProfileWidth) * m_nBatchLines);
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
unsigned int CGlLineLaserDevice::GetBatchLines()
|
||
{
|
||
return m_nBatchLines;
|
||
}
|
||
|
||
int CGlLineLaserDevice::SwitchProgram(int programNo)
|
||
{
|
||
int ret = GLX8_2_SwitchProgram(m_nDeviceId, programNo);
|
||
if (ret != 0) {
|
||
LOG_ERROR("GLX8_2_SwitchProgram failed: %d\n", ret);
|
||
return ERR_CODE(DEV_CTRL_ERR);
|
||
}
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::GetModelInfo(char* model, char* serialNumber)
|
||
{
|
||
if (model) {
|
||
strcpy(model, m_modelInfo.Model);
|
||
}
|
||
if (serialNumber) {
|
||
strcpy(serialNumber, m_modelInfo.HeaderSerial);
|
||
}
|
||
return SUCCESS;
|
||
}
|
||
|
||
int CGlLineLaserDevice::GetMeasureRange(double& xMin, double& xMax, double& zMin, double& zMax)
|
||
{
|
||
xMin = m_modelInfo.xRangmin;
|
||
xMax = m_modelInfo.xRangmax;
|
||
zMin = m_modelInfo.zRangmin;
|
||
zMax = m_modelInfo.zRangmax;
|
||
return SUCCESS;
|
||
}
|
||
|
||
// ============ 工厂方法实现 ============
|
||
// 注意：IVrEyeDevice::CreateObject 在 VrEyeDevice 中实现，创建 VzNLSDK 设备
|
||
// GlLineLaserDevice 只提供 IGlLineLaserDevice::CreateGlLineLaserObject 工厂方法
|
||
|
||
int IGlLineLaserDevice::CreateGlLineLaserObject(IGlLineLaserDevice** ppDevice)
|
||
{
|
||
CGlLineLaserDevice* p = new CGlLineLaserDevice();
|
||
*ppDevice = p;
|
||
return SUCCESS;
|
||
}
|