918 lines
33 KiB
C++
918 lines
33 KiB
C++
#include "GrabBagPresenter.h"
|
||
#include "VrError.h"
|
||
#include "VrLog.h"
|
||
#include <QtCore/QCoreApplication>
|
||
#include <QtCore/QFileInfo>
|
||
#include <QtCore/QDir>
|
||
#include <QtCore/QString>
|
||
#include <cmath>
|
||
#include <algorithm>
|
||
#include <QImage>
|
||
|
||
#include "VrTimeUtils.h"
|
||
#include "SG_bagPositioning_Export.h"
|
||
|
||
GrabBagPresenter::GrabBagPresenter()
|
||
: m_vrConfig(nullptr)
|
||
, m_pStatus(nullptr)
|
||
, m_pRobotProtocol(nullptr)
|
||
, m_bCameraConnected(false)
|
||
, m_bRobotConnected(false)
|
||
, m_currentWorkStatus(WorkStatus::Error)
|
||
{
|
||
|
||
}
|
||
|
||
GrabBagPresenter::~GrabBagPresenter()
|
||
{
|
||
// 停止算法检测线程
|
||
m_bAlgoDetectThreadRunning = false;
|
||
m_algoDetectCondition.notify_all();
|
||
|
||
// 释放缓存的检测数据
|
||
_ClearDetectionDataCache();
|
||
|
||
// 释放机械臂协议
|
||
if (m_pRobotProtocol) {
|
||
m_pRobotProtocol->Deinitialize();
|
||
delete m_pRobotProtocol;
|
||
m_pRobotProtocol = nullptr;
|
||
}
|
||
|
||
// 释放其他资源
|
||
for(auto it = m_vrEyeDeviceList.begin(); it != m_vrEyeDeviceList.end(); it++)
|
||
{
|
||
(*it)->CloseDevice();
|
||
delete (*it);
|
||
}
|
||
m_vrEyeDeviceList.clear();
|
||
|
||
}
|
||
|
||
int GrabBagPresenter::Init()
|
||
{
|
||
// 初始化连接状态
|
||
m_bCameraConnected = false;
|
||
m_bRobotConnected = false;
|
||
m_currentWorkStatus = WorkStatus::Error;
|
||
|
||
// 初始化VrConfig模块
|
||
IVrConfig::CreateInstance(&m_vrConfig);
|
||
|
||
// 获取可执行文件路径
|
||
QString exePath = QCoreApplication::applicationFilePath();
|
||
|
||
// 获取可执行文件所在目录
|
||
QString configPath = QFileInfo(exePath).absoluteDir().path() + "/config.xml";
|
||
|
||
// 读取IP列表
|
||
ConfigResult configResult = m_vrConfig->LoadConfig(configPath.toStdString());
|
||
|
||
// 初始化机械臂协议
|
||
int nRet = InitRobotProtocol();
|
||
if (nRet != 0) {
|
||
m_pStatus->OnStatusUpdate("机械臂服务初始化失败");
|
||
m_bRobotConnected = false;
|
||
} else {
|
||
m_pStatus->OnStatusUpdate("机械臂服务初始化成功");
|
||
}
|
||
|
||
// 初始化算法参数
|
||
nRet = InitAlgorithmParams();
|
||
if (nRet != 0) {
|
||
m_pStatus->OnStatusUpdate("算法参数初始化失败");
|
||
LOG_ERROR("Algorithm parameters initialization failed with error: %d\n", nRet);
|
||
} else {
|
||
m_pStatus->OnStatusUpdate("算法参数初始化成功");
|
||
LOG_INFO("Algorithm parameters initialization successful\n");
|
||
}
|
||
|
||
// 通知UI相机个数
|
||
int cameraCount = configResult.cameraList.size();
|
||
m_pStatus->OnCameraCountChanged(cameraCount);
|
||
|
||
if(cameraCount > 0){
|
||
if (cameraCount >= 1) {
|
||
// 尝试打开相机1
|
||
// 初始化VrEyeDevice模块
|
||
IVrEyeDevice* pDevice = nullptr;
|
||
IVrEyeDevice::CreateObject(&pDevice);
|
||
|
||
nRet = pDevice->InitDevice();
|
||
ERR_CODE_RETURN(nRet);
|
||
|
||
nRet = pDevice->OpenDevice(configResult.cameraList[0].ip.c_str(), &GrabBagPresenter::_StaticCameraNotify, this);
|
||
// 通过回调更新相机1状态
|
||
bool camera1Connected = (SUCCESS == nRet);
|
||
if(camera1Connected)
|
||
{
|
||
m_vrEyeDeviceList.push_back(pDevice);
|
||
}
|
||
m_pStatus->OnCamera1StatusChanged(camera1Connected);
|
||
m_pStatus->OnStatusUpdate(camera1Connected ? "相机一连接成功" : "相机一连接失败");
|
||
m_bCameraConnected = camera1Connected;
|
||
}
|
||
else {
|
||
m_pStatus->OnCamera1StatusChanged(false);
|
||
m_bCameraConnected = false;
|
||
}
|
||
ERR_CODE_RETURN(nRet);
|
||
|
||
if (cameraCount >= 2) {
|
||
IVrEyeDevice* pDevice = nullptr;
|
||
IVrEyeDevice::CreateObject(&pDevice);
|
||
nRet = pDevice->InitDevice();
|
||
ERR_CODE_RETURN(nRet);
|
||
// 尝试打开相机2
|
||
nRet = pDevice->OpenDevice(configResult.cameraList[1].ip.c_str(), &GrabBagPresenter::_StaticCameraNotify, this);
|
||
// 通过回调更新相机2状态
|
||
bool camera2Connected = (SUCCESS == nRet);
|
||
if(camera2Connected)
|
||
{
|
||
m_vrEyeDeviceList.push_back(pDevice);
|
||
}
|
||
m_pStatus->OnCamera2StatusChanged(camera2Connected);
|
||
m_pStatus->OnStatusUpdate(camera2Connected ? "相机二连接成功" : "相机二连接失败");
|
||
// 只要有一个相机连接成功就认为相机连接正常
|
||
if (camera2Connected) {
|
||
m_bCameraConnected = true;
|
||
}
|
||
}
|
||
else {
|
||
// 如果只有一个相机，则将相机2状态设为未连接
|
||
m_pStatus->OnCamera2StatusChanged(false);
|
||
}
|
||
ERR_CODE_RETURN(nRet);
|
||
} else {
|
||
IVrEyeDevice* pDevice = nullptr;
|
||
IVrEyeDevice::CreateObject(&pDevice);
|
||
nRet = pDevice->InitDevice();
|
||
ERR_CODE_RETURN(nRet);
|
||
// 尝试打开相机1
|
||
nRet = pDevice->OpenDevice(nullptr, &GrabBagPresenter::_StaticCameraNotify, this);
|
||
// 通过回调更新相机1状态
|
||
bool camera1Connected = (SUCCESS == nRet);
|
||
if(camera1Connected)
|
||
{
|
||
m_vrEyeDeviceList.push_back(pDevice);
|
||
}
|
||
m_pStatus->OnCamera1StatusChanged(camera1Connected);
|
||
m_pStatus->OnStatusUpdate(camera1Connected ? "相机一连接成功" : "相机一连接失败");
|
||
m_bCameraConnected = camera1Connected;
|
||
}
|
||
|
||
m_bAlgoDetectThreadRunning = true;
|
||
std::thread algoDetectThread(&GrabBagPresenter::_AlgoDetectThread, this);
|
||
algoDetectThread.detach();
|
||
|
||
m_pStatus->OnStatusUpdate("设备初始化完成");
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 初始化机械臂协议
|
||
int GrabBagPresenter::InitRobotProtocol()
|
||
{
|
||
LOG_DEBUG("Start initializing robot protocol\n");
|
||
|
||
// 创建RobotProtocol实例
|
||
if(nullptr == m_pRobotProtocol){
|
||
m_pRobotProtocol = new RobotProtocol();
|
||
}
|
||
|
||
// 设置连接状态回调
|
||
m_pRobotProtocol->SetConnectionCallback([this](bool connected) {
|
||
this->OnRobotConnectionChanged(connected);
|
||
});
|
||
|
||
// 设置工作信号回调
|
||
m_pRobotProtocol->SetWorkSignalCallback([this](bool startWork, int cameraIndex) {
|
||
this->OnRobotWorkSignal(startWork, cameraIndex);
|
||
});
|
||
|
||
// 初始化协议服务（使用端口503，避免与原有ModbusTCP冲突）
|
||
int nRet = m_pRobotProtocol->Initialize();
|
||
ERR_CODE_RETURN(nRet);
|
||
|
||
LOG_INFO("Robot protocol initialization successful\n");
|
||
return SUCCESS;
|
||
|
||
}
|
||
|
||
// 初始化算法参数
|
||
int GrabBagPresenter::InitAlgorithmParams()
|
||
{
|
||
LOG_DEBUG("Start initializing algorithm parameters\n");
|
||
|
||
// 获取可执行文件路径
|
||
QString exePath = QCoreApplication::applicationFilePath();
|
||
QString configPath = QFileInfo(exePath).absoluteDir().path() + "/config.xml";
|
||
|
||
// 读取配置文件
|
||
ConfigResult configResult = m_vrConfig->LoadConfig(configPath.toStdString());
|
||
const VrAlgorithmParams& xmlParams = configResult.algorithmParams;
|
||
|
||
// 初始化算法参数结构
|
||
memset(&m_algoParam, 0, sizeof(SG_bagPositionParam));
|
||
|
||
// 设置编织袋参数
|
||
m_algoParam.bagParam.bagL = xmlParams.bagParam.bagL;
|
||
m_algoParam.bagParam.bagW = xmlParams.bagParam.bagW;
|
||
m_algoParam.bagParam.bagH = xmlParams.bagParam.bagH;
|
||
|
||
// 设置滤波参数
|
||
m_algoParam.filterParam.continuityTh = xmlParams.filterParam.continuityTh;
|
||
m_algoParam.filterParam.outlierTh = xmlParams.filterParam.outlierTh;
|
||
|
||
#if BAG_ALGO_USE_CORNER_FEATURE
|
||
// 设置角点特征参数
|
||
m_algoParam.cornerParam.cornerTh = xmlParams.cornerParam.cornerTh;
|
||
m_algoParam.cornerParam.scale = xmlParams.cornerParam.scale;
|
||
m_algoParam.cornerParam.minEndingGap = xmlParams.cornerParam.minEndingGap;
|
||
m_algoParam.cornerParam.jumpCornerTh_1 = xmlParams.cornerParam.jumpCornerTh_1;
|
||
m_algoParam.cornerParam.jumpCornerTh_2 = xmlParams.cornerParam.jumpCornerTh_2;
|
||
#else
|
||
// 设置斜率参数
|
||
m_algoParam.slopeParam.LSlopeZWin = xmlParams.slopeParam.LSlopeZWin;
|
||
m_algoParam.slopeParam.validSlopeH = xmlParams.slopeParam.validSlopeH;
|
||
m_algoParam.slopeParam.minLJumpH = xmlParams.slopeParam.minLJumpH;
|
||
m_algoParam.slopeParam.minEndingGap = xmlParams.slopeParam.minEndingGap;
|
||
|
||
// 设置山谷参数
|
||
m_algoParam.valleyPara.valleyMinH = xmlParams.valleyParam.valleyMinH;
|
||
m_algoParam.valleyPara.valleyMaxW = xmlParams.valleyParam.valleyMaxW;
|
||
#endif
|
||
|
||
// 设置增长参数
|
||
m_algoParam.growParam.maxLineSkipNum = xmlParams.growParam.maxLineSkipNum;
|
||
m_algoParam.growParam.yDeviation_max = xmlParams.growParam.yDeviation_max;
|
||
m_algoParam.growParam.maxSkipDistance = xmlParams.growParam.maxSkipDistance;
|
||
m_algoParam.growParam.zDeviation_max = xmlParams.growParam.zDeviation_max;
|
||
m_algoParam.growParam.minLTypeTreeLen = xmlParams.growParam.minLTypeTreeLen;
|
||
m_algoParam.growParam.minVTypeTreeLen = xmlParams.growParam.minVTypeTreeLen;
|
||
|
||
// 初始化平面校准参数（单位矩阵，表示不进行额外的平面校准）
|
||
double initCalib[9] = {
|
||
1.0, 0.0, 0.0,
|
||
0.0, 1.0, 0.0,
|
||
0.0, 0.0, 1.0
|
||
};
|
||
for (int i = 0; i < 9; i++) {
|
||
m_planeCalibParam.planeCalib[i] = initCalib[i];
|
||
m_planeCalibParam.invRMatrix[i] = initCalib[i];
|
||
}
|
||
m_planeCalibParam.planeHeight = xmlParams.planeCalibParam.planeHeight;
|
||
|
||
LOG_INFO("Algorithm parameters initialized successfully:\n");
|
||
LOG_INFO(" Bag: L=%d, W=%d, H=%d\n", m_algoParam.bagParam.bagL, m_algoParam.bagParam.bagW, m_algoParam.bagParam.bagH);
|
||
LOG_INFO(" Filter: continuityTh=%.1f, outlierTh=%d\n", m_algoParam.filterParam.continuityTh, m_algoParam.filterParam.outlierTh);
|
||
LOG_INFO(" Plane calibration: height=%.1f\n", m_planeCalibParam.planeHeight);
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 机械臂协议回调函数实现
|
||
void GrabBagPresenter::OnRobotConnectionChanged(bool connected)
|
||
{
|
||
LOG_INFO("Robot connection status changed: %s\n", (connected ? "connected" : "disconnected"));
|
||
|
||
// 更新机械臂连接状态
|
||
m_bRobotConnected = connected;
|
||
|
||
if (m_pStatus) {
|
||
m_pStatus->OnRobotConnectionChanged(connected);
|
||
m_pStatus->OnStatusUpdate(connected ? "机械臂已连接" : "机械臂连接断开");
|
||
}
|
||
|
||
// 检查并更新工作状态
|
||
CheckAndUpdateWorkStatus();
|
||
}
|
||
|
||
void GrabBagPresenter::OnRobotWorkSignal(bool startWork, int cameraIndex)
|
||
{
|
||
LOG_INFO("Received robot work signal: %s for camera index: %d\n", (startWork ? "start work" : "stop work"), cameraIndex);
|
||
|
||
if (nullptr == m_pStatus) {
|
||
return;
|
||
}
|
||
|
||
if (startWork) {
|
||
QString message;
|
||
if (cameraIndex == -1) {
|
||
message = "收到开始工作信号，启动所有相机检测";
|
||
} else {
|
||
message = QString("收到开始工作信号，启动相机 %1 检测").arg(cameraIndex + 1);
|
||
}
|
||
m_pStatus->OnStatusUpdate(message.toStdString());
|
||
StartDetection(cameraIndex);
|
||
} else {
|
||
QString message;
|
||
if (cameraIndex == -1) {
|
||
message = "收到停止工作信号，停止所有相机检测";
|
||
} else {
|
||
message = QString("收到停止工作信号，停止相机 %1 检测").arg(cameraIndex + 1);
|
||
}
|
||
m_pStatus->OnStatusUpdate(message.toStdString());
|
||
StopDetection();
|
||
}
|
||
}
|
||
|
||
void GrabBagPresenter::SetStatusCallback(IYGrabBagStatus* status)
|
||
{
|
||
m_pStatus = status;
|
||
}
|
||
|
||
// 模拟检测函数，用于演示
|
||
int GrabBagPresenter::StartDetection(int cameraIndex)
|
||
{
|
||
LOG_INFO("***** Start detection with camera index: %d\n", cameraIndex);
|
||
|
||
// 检查设备状态是否准备就绪
|
||
if (m_currentWorkStatus != WorkStatus::Ready) {
|
||
LOG_INFO("Device not ready, cannot start detection\n");
|
||
if (m_pStatus) {
|
||
m_pStatus->OnStatusUpdate("设备未准备就绪，无法开始检测");
|
||
}
|
||
return ERR_CODE(DEV_BUSY);
|
||
}
|
||
|
||
// 通知UI工作状态变更为"正在工作"
|
||
if (m_pStatus) {
|
||
m_currentWorkStatus = WorkStatus::Working;
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Working);
|
||
}
|
||
|
||
if(m_vrEyeDeviceList.empty())
|
||
{
|
||
LOG_ERROR("No camera device found\n");
|
||
return ERR_CODE(DEV_NOT_FIND);
|
||
}
|
||
|
||
// 清空检测数据缓存（释放之前的内存）
|
||
_ClearDetectionDataCache();
|
||
|
||
int nRet = SUCCESS;
|
||
|
||
// 根据参数决定启动哪些相机
|
||
|
||
// 启动指定相机
|
||
if (cameraIndex >= 0 && cameraIndex < static_cast<int>(m_vrEyeDeviceList.size())) {
|
||
nRet = m_vrEyeDeviceList[cameraIndex]->StartDetect(&GrabBagPresenter::_StaticDetectionCallback, this);
|
||
if (nRet == SUCCESS) {
|
||
LOG_INFO("Camera %d start detection success\n", cameraIndex);
|
||
m_pStatus->OnStatusUpdate(QString("启动相机 %1 检测成功").arg(cameraIndex + 1).toStdString());
|
||
} else {
|
||
LOG_ERROR("Camera %d start detection failed with error: %d\n", cameraIndex, nRet);
|
||
m_pStatus->OnStatusUpdate(QString("启动相机 %1 检测失败").arg(cameraIndex + 1).toStdString());
|
||
}
|
||
} else {
|
||
LOG_ERROR("Invalid camera index: %d, available cameras: %zu\n", cameraIndex, m_vrEyeDeviceList.size());
|
||
m_pStatus->OnStatusUpdate(QString("无效的相机索引: %1，可用相机数量: %2").arg(cameraIndex).arg(m_vrEyeDeviceList.size()).toStdString());
|
||
nRet = ERR_CODE(DEV_NOT_FIND);
|
||
}
|
||
|
||
return nRet;
|
||
}
|
||
|
||
int GrabBagPresenter::StopDetection()
|
||
{
|
||
LOG_INFO("Stop detection\n");
|
||
|
||
// 停止所有相机的检测
|
||
for (size_t i = 0; i < m_vrEyeDeviceList.size(); ++i) {
|
||
IVrEyeDevice* pDevice = m_vrEyeDeviceList[i];
|
||
if (pDevice) {
|
||
int ret = pDevice->StopDetect();
|
||
if (ret == 0) {
|
||
LOG_INFO("Camera %zu stop detection successfully\n", i + 1);
|
||
} else {
|
||
LOG_WARNING("Camera %zu stop detection failed, error code: %d\n", i + 1, ret);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 通知UI工作状态变更为"就绪"（如果设备连接正常）
|
||
if (m_pStatus) {
|
||
// 检查设备连接状态，决定停止后的状态
|
||
if (m_bCameraConnected && m_bRobotConnected) {
|
||
m_currentWorkStatus = WorkStatus::Ready;
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Ready);
|
||
} else {
|
||
m_currentWorkStatus = WorkStatus::Error;
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Error);
|
||
}
|
||
m_pStatus->OnStatusUpdate("检测已停止");
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
// 加载调试数据进行检测
|
||
int GrabBagPresenter::LoadDebugDataAndDetect(const std::string& filePath)
|
||
{
|
||
LOG_INFO("Loading debug data from file: %s\n", filePath.c_str());
|
||
|
||
if (m_pStatus) {
|
||
m_pStatus->OnStatusUpdate("正在加载调试数据...");
|
||
}
|
||
|
||
int lineNum = 0;
|
||
float scanSpeed = 0.0f;
|
||
int maxTimeStamp = 0;
|
||
int clockPerSecond = 0;
|
||
|
||
int result = SUCCESS;
|
||
// 清空现有的检测数据缓存
|
||
_ClearDetectionDataCache();
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_detectionDataMutex);
|
||
result = m_dataLoader.LoadLaserScanData(filePath, m_detectionDataCache, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
|
||
}
|
||
if (result != SUCCESS) {
|
||
LOG_ERROR("Failed to load debug data: %s\n", m_dataLoader.GetLastError().c_str());
|
||
if (m_pStatus) {
|
||
m_pStatus->OnStatusUpdate("调试数据加载失败: " + m_dataLoader.GetLastError());
|
||
}
|
||
return result;
|
||
}
|
||
|
||
LOG_INFO("Successfully loaded %d lines of debug data\n", lineNum);
|
||
if (m_pStatus) {
|
||
m_pStatus->OnStatusUpdate(QString("成功加载 %1 行调试数据").arg(lineNum).toStdString());
|
||
m_currentWorkStatus = WorkStatus::Working;
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Working);
|
||
}
|
||
|
||
// 等待检测完成（这里可以考虑异步处理）
|
||
// 为了简化，这里直接调用检测任务
|
||
result = _DetectTask();
|
||
|
||
// 释放加载的数据
|
||
m_dataLoader.FreeLaserScanData(m_detectionDataCache);
|
||
|
||
return result;
|
||
}
|
||
|
||
IVrEyeDevice *GrabBagPresenter::GetEyeDevice(int index)
|
||
{
|
||
if(m_vrEyeDeviceList.size() <= index) return nullptr;
|
||
return m_vrEyeDeviceList[index];
|
||
}
|
||
|
||
// 静态回调函数实现
|
||
void GrabBagPresenter::_StaticCameraNotify(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
|
||
{
|
||
// 从pInfoParam获取this指针，转换回GrabBagPresenter*类型
|
||
GrabBagPresenter* pThis = reinterpret_cast<GrabBagPresenter*>(pInfoParam);
|
||
if (pThis)
|
||
{
|
||
// 调用实例的非静态成员函数
|
||
pThis->_CameraNotify(eStatus, pExtData, nDataLength, pInfoParam);
|
||
}
|
||
}
|
||
|
||
void GrabBagPresenter::_CameraNotify(EVzDeviceWorkStatus eStatus, void *pExtData, unsigned int nDataLength, void *pInfoParam)
|
||
{
|
||
LOG_DEBUG("----- Camera notify received: status=%d\n", (int)eStatus);
|
||
|
||
switch (eStatus) {
|
||
case EVzDeviceWorkStatus::keDeviceWorkStatus_Offline:
|
||
{
|
||
LOG_WARNING("Camera device offline/disconnected\n");
|
||
|
||
// 更新相机连接状态
|
||
m_bCameraConnected = false;
|
||
|
||
// 通知UI相机状态变更
|
||
if (m_pStatus) {
|
||
// 这里需要判断是哪个相机离线，暂时更新相机1状态
|
||
// 实际应用中可能需要通过pInfoParam或其他方式区分具体哪个相机
|
||
m_pStatus->OnCamera1StatusChanged(false);
|
||
m_pStatus->OnStatusUpdate("相机设备离线");
|
||
}
|
||
|
||
// 检查并更新工作状态
|
||
CheckAndUpdateWorkStatus();
|
||
break;
|
||
}
|
||
|
||
case EVzDeviceWorkStatus::keDeviceWorkStatus_Eye_Reconnect:
|
||
{
|
||
LOG_INFO("Camera device online/connected\n");
|
||
|
||
// 更新相机连接状态
|
||
m_bCameraConnected = true;
|
||
|
||
// 通知UI相机状态变更
|
||
if (m_pStatus) {
|
||
m_pStatus->OnCamera1StatusChanged(true);
|
||
m_pStatus->OnStatusUpdate("相机设备已连接");
|
||
}
|
||
|
||
// 检查并更新工作状态
|
||
CheckAndUpdateWorkStatus();
|
||
break;
|
||
}
|
||
|
||
case EVzDeviceWorkStatus::keDeviceWorkStatus_Device_Swing_Finish:
|
||
{
|
||
LOG_INFO("Received scan finish signal from camera\n");
|
||
|
||
// 通知检测线程开始处理
|
||
m_algoDetectCondition.notify_one();
|
||
break;
|
||
}
|
||
|
||
default:
|
||
LOG_DEBUG("Unhandled camera status: %d\n", (int)eStatus);
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 检测数据回调函数静态版本
|
||
void GrabBagPresenter::_StaticDetectionCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint, void* pUserData)
|
||
{
|
||
GrabBagPresenter* pThis = reinterpret_cast<GrabBagPresenter*>(pUserData);
|
||
if (pThis && pLaserLinePoint) {
|
||
pThis->_DetectionCallback(eDataType, pLaserLinePoint);
|
||
}
|
||
}
|
||
|
||
// 检测数据回调函数实例版本
|
||
void GrabBagPresenter::_DetectionCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint)
|
||
{
|
||
if (!pLaserLinePoint) {
|
||
return;
|
||
}
|
||
|
||
// 转换数据格式：从SVzLaserLineData转换为SVzNL3DLaserLine并存储到缓存
|
||
SVzNL3DLaserLine laser3DLine;
|
||
|
||
// 复制基本信息
|
||
laser3DLine.nTimeStamp = pLaserLinePoint->llTimeStamp;
|
||
laser3DLine.nPositionCnt = pLaserLinePoint->nPointCount;
|
||
|
||
// 分配和复制点云数据
|
||
laser3DLine.p3DPosition = new SVzNL3DPosition[pLaserLinePoint->nPointCount];
|
||
// 复制点云数据
|
||
memcpy(laser3DLine.p3DPosition, pLaserLinePoint->p3DPoint, sizeof(SVzNL3DPosition) * pLaserLinePoint->nPointCount);
|
||
|
||
// 将转换后的数据保存到缓存中
|
||
std::lock_guard<std::mutex> lock(m_detectionDataMutex);
|
||
m_detectionDataCache.push_back(laser3DLine);
|
||
}
|
||
|
||
void GrabBagPresenter::CheckAndUpdateWorkStatus()
|
||
{
|
||
if (m_bCameraConnected && m_bRobotConnected) {
|
||
m_currentWorkStatus = WorkStatus::Ready;
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Ready);
|
||
m_pStatus->OnStatusUpdate("设备已准备好");
|
||
} else {
|
||
m_currentWorkStatus = WorkStatus::Error;
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Error);
|
||
m_pStatus->OnStatusUpdate("设备未准备好");
|
||
}
|
||
}
|
||
|
||
void GrabBagPresenter::_AlgoDetectThread()
|
||
{
|
||
while(m_bAlgoDetectThreadRunning)
|
||
{
|
||
std::unique_lock<std::mutex> lock(m_algoDetectMutex);
|
||
m_algoDetectCondition.wait(lock, [this]() {
|
||
return m_currentWorkStatus == WorkStatus::Working;
|
||
});
|
||
|
||
// 检查设备状态是否准备就绪
|
||
_DetectTask();
|
||
}
|
||
}
|
||
|
||
int GrabBagPresenter::_DetectTask()
|
||
{
|
||
LOG_INFO("----- Start real detection task using algorithm\n");
|
||
|
||
// 1. 获取缓存的点云数据
|
||
if (m_detectionDataCache.empty()) {
|
||
LOG_WARNING("No cached detection data available\n");
|
||
if (m_pStatus) {
|
||
m_pStatus->OnStatusUpdate("无缓存的检测数据");
|
||
}
|
||
return ERR_CODE(DEV_DATA_INVALID);
|
||
}
|
||
|
||
// 2. 准备算法输入数据
|
||
if(m_pStatus){
|
||
m_pStatus->OnStatusUpdate("正在进行算法检测...");
|
||
}
|
||
|
||
// 3. 使用成员变量算法参数（已在初始化时从XML读取）
|
||
LOG_INFO("Using algorithm parameters from XML configuration\n");
|
||
LOG_INFO(" Bag: L=%d, W=%d, H=%d\n", m_algoParam.bagParam.bagL, m_algoParam.bagParam.bagW, m_algoParam.bagParam.bagH);
|
||
LOG_INFO(" Filter: continuityTh=%.1f, outlierTh=%d\n", m_algoParam.filterParam.continuityTh, m_algoParam.filterParam.outlierTh);
|
||
|
||
CVrTimeUtils oTimeUtils;
|
||
// 4. 数据预处理：调平和去除地面（使用成员变量平面校准参数）
|
||
for (size_t i = 0; i < m_detectionDataCache.size(); i++) {
|
||
sg_lineDataR(&m_detectionDataCache[i], m_planeCalibParam.planeCalib, m_planeCalibParam.planeHeight);
|
||
}
|
||
LOG_DEBUG("sg_lineDataR time: %.2f ms\n", oTimeUtils.GetElapsedTimeInMilliSec());
|
||
|
||
// 5. 调用算法检测接口（使用成员变量参数）
|
||
oTimeUtils.Update();
|
||
std::vector<SSG_peakRgnInfo> objOps;
|
||
sg_getBagPosition(static_cast<SVzNL3DLaserLine*>(m_detectionDataCache.data()),
|
||
m_detectionDataCache.size(), m_algoParam, m_planeCalibParam, objOps);
|
||
|
||
LOG_INFO("sg_getBagPosition detected %zu objects time : %.2f ms\n", objOps.size(), oTimeUtils.GetElapsedTimeInMilliSec());
|
||
|
||
// 6. 转换检测结果为UI显示格式
|
||
DetectionResult detectionResult;
|
||
// 从点云数据生成投影图像
|
||
detectionResult.image = _GeneratePointCloudImage(static_cast<SVzNL3DLaserLine*>(m_detectionDataCache.data()),
|
||
m_detectionDataCache.size(), objOps);
|
||
|
||
// 转换检测结果
|
||
for (size_t i = 0; i < objOps.size(); i++) {
|
||
const SSG_peakRgnInfo& obj = objOps[i];
|
||
|
||
// 创建层数据（简化处理，将所有目标放在同一层）
|
||
if (detectionResult.positionLayout.empty()) {
|
||
GrabBagPositionLayout layer;
|
||
layer.layerIndex = 1;
|
||
detectionResult.positionLayout.push_back(layer);
|
||
}
|
||
|
||
// 创建位置数据
|
||
GrabBagPosition pos;
|
||
pos.x = obj.centerPos.x;
|
||
pos.y = obj.centerPos.y;
|
||
pos.z = obj.centerPos.z;
|
||
pos.roll = obj.centerPos.x_roll;
|
||
pos.pitch = obj.centerPos.y_pitch;
|
||
pos.yaw = obj.centerPos.z_yaw;
|
||
|
||
detectionResult.positionLayout[0].position.push_back(pos);
|
||
|
||
LOG_INFO("Object %zu: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n",
|
||
i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
|
||
}
|
||
|
||
// 8. 返回检测结果
|
||
if (objOps.size() > 0) {
|
||
// 调用检测结果回调函数
|
||
m_pStatus->OnDetectionResult(detectionResult);
|
||
|
||
// 更新状态
|
||
QString statusMsg = QString("检测完成，发现%1个目标").arg(objOps.size());
|
||
m_pStatus->OnStatusUpdate(statusMsg.toStdString());
|
||
|
||
// 更新机械臂协议状态（发送第一个检测到的目标位置）
|
||
_SendDetectionResultToRobot(objOps);
|
||
} else {
|
||
// 没有检测到目标
|
||
m_pStatus->OnStatusUpdate("检测完成，未发现目标");
|
||
LOG_WARNING("No objects detected\n");
|
||
}
|
||
|
||
// 9. 检测完成后，将工作状态更新为"完成"
|
||
if (m_pStatus) {
|
||
m_currentWorkStatus = WorkStatus::Completed;
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Completed);
|
||
}
|
||
|
||
// 恢复到就绪状态
|
||
m_currentWorkStatus = WorkStatus::Ready;
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 释放缓存的检测数据
|
||
void GrabBagPresenter::_ClearDetectionDataCache()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_detectionDataMutex);
|
||
|
||
LOG_DEBUG("Clearing detection data cache, current size: %zu\n", m_detectionDataCache.size());
|
||
|
||
// 释放缓存的内存
|
||
for (auto& cachedLine : m_detectionDataCache) {
|
||
if (cachedLine.p3DPosition) {
|
||
delete[] cachedLine.p3DPosition;
|
||
cachedLine.p3DPosition = nullptr;
|
||
}
|
||
}
|
||
|
||
// 清空缓存容器
|
||
m_detectionDataCache.clear();
|
||
|
||
LOG_DEBUG("Detection data cache cleared successfully\n");
|
||
}
|
||
|
||
// 发送检测结果给机械臂
|
||
void GrabBagPresenter::_SendDetectionResultToRobot(const std::vector<SSG_peakRgnInfo>& objOps)
|
||
{
|
||
if (!m_pRobotProtocol) {
|
||
LOG_WARNING("Robot protocol not initialized, cannot send detection result\n");
|
||
return;
|
||
}
|
||
|
||
if (objOps.empty()) {
|
||
LOG_INFO("No objects detected, sending empty result to robot\n");
|
||
// 可以选择发送一个特殊的"无目标"坐标给机械臂
|
||
RobotProtocol::RobotCoordinate emptyCoord;
|
||
memset(&emptyCoord, 0, sizeof(RobotProtocol::RobotCoordinate));
|
||
emptyCoord.x = -9999.0; // 使用特殊值表示无目标
|
||
emptyCoord.y = -9999.0;
|
||
emptyCoord.z = -9999.0;
|
||
m_pRobotProtocol->SetRobotCoordinate(emptyCoord);
|
||
return;
|
||
}
|
||
|
||
// 选择第一个检测到的目标作为抓取目标
|
||
const SSG_peakRgnInfo& targetObj = objOps[0];
|
||
|
||
LOG_INFO("Processing detection result for robot: %zu objects detected\n", objOps.size());
|
||
LOG_INFO("Selected target object: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n",
|
||
targetObj.centerPos.x, targetObj.centerPos.y, targetObj.centerPos.z,
|
||
targetObj.centerPos.x_roll, targetObj.centerPos.y_pitch, targetObj.centerPos.z_yaw);
|
||
|
||
// 坐标转换：从算法坐标系转换到机械臂坐标系
|
||
RobotProtocol::RobotCoordinate robotCoord;
|
||
|
||
// 基本坐标转换（根据实际的坐标系关系进行调整）
|
||
// 这里假设需要进行一些坐标系转换，具体转换关系需要根据实际标定结果确定
|
||
|
||
// 位置坐标转换 (单位转换：mm -> mm，坐标系转换等)
|
||
robotCoord.x = targetObj.centerPos.x; // X轴坐标
|
||
robotCoord.y = targetObj.centerPos.y; // Y轴坐标
|
||
robotCoord.z = targetObj.centerPos.z; // Z轴坐标
|
||
|
||
// 姿态角度转换 (弧度转角度，坐标系转换等)
|
||
robotCoord.rx = targetObj.centerPos.x_roll * 180.0 / M_PI; // Roll角 (弧度转角度)
|
||
robotCoord.ry = targetObj.centerPos.y_pitch * 180.0 / M_PI; // Pitch角 (弧度转角度)
|
||
robotCoord.rz = targetObj.centerPos.z_yaw * 180.0 / M_PI; // Yaw角 (弧度转角度)
|
||
|
||
// 应用坐标系偏移和旋转变换（如果需要）
|
||
// 这部分可以根据实际的手眼标定结果进行调整
|
||
// 例如：
|
||
// - 相机坐标系到机械臂基坐标系的变换
|
||
// - 工具坐标系的偏移
|
||
// - 抓取点的偏移调整
|
||
|
||
// 示例：添加一些实际应用中可能需要的偏移
|
||
robotCoord.z += 50.0; // Z轴向上偏移50mm，避免碰撞
|
||
|
||
// 工作空间检查
|
||
// 检查目标位置是否在机械臂的工作空间内
|
||
double distanceFromOrigin = sqrt(robotCoord.x * robotCoord.x +
|
||
robotCoord.y * robotCoord.y +
|
||
robotCoord.z * robotCoord.z);
|
||
|
||
const double MAX_REACH = 1500.0; // 机械臂最大工作半径 (mm)
|
||
const double MIN_REACH = 200.0; // 机械臂最小工作半径 (mm)
|
||
|
||
if (distanceFromOrigin > MAX_REACH || distanceFromOrigin < MIN_REACH) {
|
||
LOG_WARNING("Target position out of robot workspace: distance=%.2f mm (valid range: %.2f-%.2f mm)\n",
|
||
distanceFromOrigin, MIN_REACH, MAX_REACH);
|
||
|
||
// 可以选择调整坐标到工作空间内，或者发送错误状态
|
||
if (distanceFromOrigin > MAX_REACH) {
|
||
// 按比例缩放到最大工作半径
|
||
double scale = MAX_REACH / distanceFromOrigin;
|
||
robotCoord.x *= scale;
|
||
robotCoord.y *= scale;
|
||
robotCoord.z *= scale;
|
||
LOG_INFO("Scaled target position to workspace boundary\n");
|
||
}
|
||
}
|
||
|
||
// 发送坐标给机械臂
|
||
int result = m_pRobotProtocol->SetRobotCoordinate(robotCoord);
|
||
if (result == 0) {
|
||
LOG_INFO("Successfully sent robot coordinates: X=%.2f, Y=%.2f, Z=%.2f, RX=%.2f, RY=%.2f, RZ=%.2f\n",
|
||
robotCoord.x, robotCoord.y, robotCoord.z,
|
||
robotCoord.rx, robotCoord.ry, robotCoord.rz);
|
||
|
||
// 更新状态信息
|
||
if (m_pStatus) {
|
||
QString statusMsg = QString("已发送目标坐标给机械臂: X=%.1f, Y=%.1f, Z=%.1f").arg(robotCoord.x).arg(robotCoord.y).arg(robotCoord.z);
|
||
m_pStatus->OnStatusUpdate(statusMsg.toStdString());
|
||
}
|
||
} else {
|
||
LOG_ERROR("Failed to send robot coordinates, error code: %d\n", result);
|
||
|
||
if (m_pStatus) {
|
||
m_pStatus->OnStatusUpdate("发送坐标给机械臂失败");
|
||
}
|
||
}
|
||
|
||
// 记录多个目标的信息（用于调试和分析）
|
||
if (objOps.size() > 1) {
|
||
LOG_INFO("Additional detected objects (%zu total):\n", objOps.size());
|
||
for (size_t i = 1; i < objOps.size() && i < 5; i++) { // 最多记录5个目标
|
||
const SSG_peakRgnInfo& obj = objOps[i];
|
||
LOG_INFO(" Object %zu: X=%.2f, Y=%.2f, Z=%.2f\n",i, obj.centerPos.x, obj.centerPos.y, obj.centerPos.z);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 点云转图像 - 简化版本
|
||
QImage GrabBagPresenter::_GeneratePointCloudImage(SVzNL3DLaserLine* scanData, int lineNum,
|
||
const std::vector<SSG_peakRgnInfo>& objOps)
|
||
{
|
||
if (!scanData || lineNum <= 0) {
|
||
return QImage(); // 返回空图像
|
||
}
|
||
|
||
// 统计X和Y的范围
|
||
SVzNLRangeD x_range = {0, -1};
|
||
SVzNLRangeD y_range = {0, -1};
|
||
|
||
for (int line = 0; line < lineNum; line++) {
|
||
for (int i = 0; i < scanData[line].nPositionCnt; i++) {
|
||
SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
|
||
if (pt3D->pt3D.z < 1e-4) continue;
|
||
|
||
if (x_range.max < x_range.min) {
|
||
x_range.min = x_range.max = pt3D->pt3D.x;
|
||
} else {
|
||
if (x_range.min > pt3D->pt3D.x) x_range.min = pt3D->pt3D.x;
|
||
if (x_range.max < pt3D->pt3D.x) x_range.max = pt3D->pt3D.x;
|
||
}
|
||
if (y_range.max < y_range.min) {
|
||
y_range.min = y_range.max = pt3D->pt3D.y;
|
||
} else {
|
||
if (y_range.min > pt3D->pt3D.y) y_range.min = pt3D->pt3D.y;
|
||
if (y_range.max < pt3D->pt3D.y) y_range.max = pt3D->pt3D.y;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 创建图像
|
||
int imgCols = 800;
|
||
int imgRows = 600;
|
||
double x_cols = 768.0;
|
||
double y_rows = 568.0;
|
||
int x_skip = 16;
|
||
int y_skip = 16;
|
||
|
||
// 计算投影比例
|
||
double x_scale = (x_range.max - x_range.min) / x_cols;
|
||
double y_scale = (y_range.max - y_range.min) / y_rows;
|
||
|
||
QImage image(imgCols, imgRows, QImage::Format_RGB888);
|
||
image.fill(Qt::black);
|
||
QPainter painter(&image);
|
||
|
||
// 定义颜色
|
||
QColor objColors[8] = {
|
||
QColor(245,222,179), QColor(210,105,30), QColor(240,230,140), QColor(135,206,235),
|
||
QColor(250,235,215), QColor(189,252,201), QColor(221,160,221), QColor(188,143,143)
|
||
};
|
||
|
||
// 绘制点云
|
||
for (int line = 0; line < lineNum; line++) {
|
||
for (int i = 0; i < scanData[line].nPositionCnt; i++) {
|
||
SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
|
||
if (pt3D->pt3D.z < 1e-4) continue;
|
||
|
||
int objId = (pt3D->nPointIdx >> 16) & 0xff;
|
||
QColor pointColor = (objId > 0) ? objColors[objId % 8] : QColor(60, 60, 60);
|
||
|
||
int px = (int)((pt3D->pt3D.x - x_range.min) / x_scale + x_skip);
|
||
int py = (int)((pt3D->pt3D.y - y_range.min) / y_scale + y_skip);
|
||
|
||
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
|
||
painter.setPen(QPen(pointColor, 1));
|
||
painter.drawPoint(px, py);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 绘制检测目标
|
||
for (size_t i = 0; i < objOps.size(); i++) {
|
||
QColor objColor = (i == 0) ? QColor(255, 0, 0) : QColor(255, 255, 0);
|
||
int size = (i == 0) ? 12 : 8;
|
||
|
||
int px = (int)((objOps[i].centerPos.x - x_range.min) / x_scale + x_skip);
|
||
int py = (int)((objOps[i].centerPos.y - y_range.min) / y_scale + y_skip);
|
||
|
||
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
|
||
painter.setPen(QPen(objColor, 2));
|
||
painter.setBrush(QBrush(objColor));
|
||
painter.drawEllipse(px - size/2, py - size/2, size, size);
|
||
}
|
||
}
|
||
|
||
return image;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|