GrabBag/Device/GlLineLaserDevice/Src/GlLineLaserDevice.cpp
2026-02-06 01:14:27 +08:00

614 lines
17 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#include "GlLineLaserDevice.h"
#include "VrError.h"
#include "VrLog.h"
#include <cstring>
#include <chrono>
#include <algorithm>
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(&ethConfig, 0, sizeof(ethConfig));
if (sIP && strlen(sIP) > 0) {
LOG_DEBUG("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(&ethConfig, &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, &ethConfig);
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;
// 启动数据采集线程(主动轮询模式)
m_bDetecting = true;
m_detectThread = std::thread(&CGlLineLaserDevice::DetectThreadFunc, this);
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);
}
// 等待线程结束
if (m_detectThread.joinable()) {
m_detectThread.join();
}
m_bDetecting = false;
// 通知状态变化
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;
}
// 数据采集线程函数(主动轮询模式,参考 test_deal_atch_datas
void CGlLineLaserDevice::DetectThreadFunc()
{
LOG_DEBUG("Detect thread started\n");
while (!m_bStopDetect) {
// 启动批处理(参考 test_deal_atch_datas
int ret = GLX8_2_StartMeasure(m_nDeviceId, 5000); // 5秒超时
if (ret != 0) {
LOG_ERROR("GLX8_2_StartMeasure failed: %d\n", ret);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
continue;
}
// 等待一小段时间让设备准备好
std::this_thread::sleep_for(std::chrono::milliseconds(50));
// 获取批处理数据(参考 test_batch_datas
GetBatchData();
// 批处理完成后等待一段时间再开始下一次
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
LOG_DEBUG("Detect thread stopped\n");
}
// 获取批处理数据(参考 test_batch_datas 的流程)
void CGlLineLaserDevice::GetBatchData()
{
GLX8_2_STR_CALLBACK_INFO info;
memset(&info, 0, sizeof(info));
const int maxBatchLines = m_nBatchLines;
// 确保缓存足够大
size_t totalPoints = static_cast<size_t>(m_nProfileWidth) * maxBatchLines;
if (m_profileBuffer.size() < totalPoints) {
m_profileBuffer.resize(totalPoints);
}
if (m_intensityBuffer.size() < totalPoints) {
m_intensityBuffer.resize(totalPoints);
}
std::vector<uint32_t> encoderBuffer(maxBatchLines);
// 使用 GLX8_2_ReceiveDataAuto 顺序获取批处理数据
int ret = GLX8_2_ReceiveDataAuto(m_nDeviceId, &info,
m_profileBuffer.data(),
m_intensityBuffer.data(),
encoderBuffer.data());
if (ret != 0) {
LOG_WARNING("GLX8_2_ReceiveDataAuto failed: %d\n", ret);
return;
}
int batchCount = info.BatchPoints;
int width = info.xPoints;
if (batchCount <= 0 || width <= 0) {
LOG_WARNING("Invalid batch data: batchCount=%d, width=%d\n", batchCount, width);
return;
}
LOG_DEBUG("Received batch: %d lines, width: %d, startEncoder: %d\n",
batchCount, width, info.startEncoder);
// 确保位置缓存足够大
if (m_positionBuffer.size() < static_cast<size_t>(width)) {
m_positionBuffer.resize(width);
}
// 逐行处理数据并回调转换为xyz点云数据
for (int lineIdx = 0; lineIdx < batchCount; lineIdx++) {
if (m_bStopDetect) {
break;
}
// 计算当前行在缓存中的偏移
const int32_t* lineProfile = m_profileBuffer.data() + static_cast<size_t>(lineIdx) * width;
// 转换为xyz坐标
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 = encoderBuffer[lineIdx]; // 使用实际的编码器值
laserLineData.fSwingAngle = 0.0f; // 线激光没有摆动角度
laserLineData.bEndOnceScan = (lineIdx == batchCount - 1) ? VzTrue : VzFalse;
// 回调给上层应用
if (m_pDetectCallback) {
m_pDetectCallback(m_eDataType, &laserLineData, m_pDetectCallbackParam);
}
m_ullFrameIndex++;
}
LOG_DEBUG("Processed %d lines, total frames: %llu\n", batchCount, m_ullFrameIndex);
}
// 将轮廓数据转换为位置数据
void CGlLineLaserDevice::ConvertProfileToPosition(const int32_t* profileData, int count, int lineIndex)
{
// lineIndex 参数保留用于未来扩展,当前使用 m_ullFrameIndex 计算全局偏移
(void)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;
}