1032 lines
37 KiB
C++
1032 lines
37 KiB
C++
#include "GrabBagPresenter.h"
|
||
#include "VrError.h"
|
||
#include "VrLog.h"
|
||
#include <QtCore/QCoreApplication>
|
||
#include <QtCore/QFileInfo>
|
||
#include <QtCore/QDir>
|
||
#include <QtCore/QString>
|
||
#include <QtCore/QStandardPaths>
|
||
#include <QtCore/QFile>
|
||
#include <cmath>
|
||
#include <algorithm>
|
||
#include <QImage>
|
||
#include <QThread>
|
||
|
||
#include "VrTimeUtils.h"
|
||
#include "SG_bagPositioning_Export.h"
|
||
#include "VrConvert.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_currentWorkStatus = WorkStatus::InitIng;
|
||
|
||
|
||
m_pStatus->OnWorkStatusChanged(m_currentWorkStatus);
|
||
|
||
// 初始化VrConfig模块
|
||
IVrConfig::CreateInstance(&m_vrConfig);
|
||
|
||
// 设置配置改变通知回调
|
||
if (m_vrConfig) {
|
||
m_vrConfig->SetConfigChangeNotify(this);
|
||
}
|
||
|
||
// 获取配置文件路径
|
||
QString configPath = PathManager::GetConfigFilePath();
|
||
|
||
// 读取IP列表
|
||
ConfigResult configResult = m_vrConfig->LoadConfig(configPath.toStdString());
|
||
int nRet = SUCCESS;
|
||
|
||
// 初始化算法参数
|
||
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");
|
||
}
|
||
|
||
// 初始化机械臂协议
|
||
nRet = InitRobotProtocol();
|
||
if (nRet != 0) {
|
||
m_pStatus->OnStatusUpdate("机械臂服务初始化失败");
|
||
m_bRobotConnected = false;
|
||
} else {
|
||
m_pStatus->OnStatusUpdate("机械臂服务初始化成功");
|
||
}
|
||
|
||
|
||
// 通知UI相机个数
|
||
int cameraCount = configResult.cameraList.size();
|
||
m_pStatus->OnCameraCountChanged(cameraCount);
|
||
|
||
if(cameraCount > 0){
|
||
if (cameraCount >= 1) {
|
||
// 尝试打开相机1
|
||
LOG_INFO("Attempting to connect to camera 1 with IP: %s\n", configResult.cameraList[0].ip.c_str());
|
||
|
||
// 发送页面提示信息
|
||
m_pStatus->OnStatusUpdate(QString("正在连接相机一，IP: %1").arg(configResult.cameraList[0].ip.c_str()).toStdString());
|
||
|
||
// 初始化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);
|
||
LOG_INFO("Camera 1 connection successful, IP: %s\n", configResult.cameraList[0].ip.c_str());
|
||
m_pStatus->OnStatusUpdate(QString("相机一连接成功，IP: %1").arg(configResult.cameraList[0].ip.c_str()).toStdString());
|
||
} else {
|
||
LOG_ERROR("Camera 1 connection failed, IP: %s, error code: %d\n", configResult.cameraList[0].ip.c_str(), nRet);
|
||
m_pStatus->OnStatusUpdate(QString("相机一连接失败，IP: %1，错误码: %2").arg(configResult.cameraList[0].ip.c_str()).arg(nRet).toStdString());
|
||
}
|
||
m_pStatus->OnCamera1StatusChanged(camera1Connected);
|
||
m_pStatus->OnStatusUpdate(camera1Connected ? "相机一连接成功" : "相机一连接失败");
|
||
m_bCameraConnected = camera1Connected;
|
||
}
|
||
else {
|
||
LOG_WARNING("Camera count is 0, setting camera 1 as disconnected\n");
|
||
m_pStatus->OnStatusUpdate("配置文件中未找到相机配置，设置相机一为离线状态");
|
||
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("设备初始化完成");
|
||
|
||
CheckAndUpdateWorkStatus();
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 初始化机械臂协议
|
||
int GrabBagPresenter::InitRobotProtocol()
|
||
{
|
||
LOG_DEBUG("Start initializing robot protocol\n");
|
||
m_pStatus->OnStatusUpdate("机械臂服务初始化...");
|
||
|
||
// 创建RobotProtocol实例
|
||
if(nullptr == m_pRobotProtocol){
|
||
m_pRobotProtocol = new RobotProtocol();
|
||
}
|
||
|
||
// 初始化协议服务（使用端口502）
|
||
int nRet = m_pRobotProtocol->Initialize(5020);
|
||
|
||
// 设置连接状态回调
|
||
m_pRobotProtocol->SetConnectionCallback([this](bool connected) {
|
||
this->OnRobotConnectionChanged(connected);
|
||
});
|
||
|
||
// 设置工作信号回调
|
||
m_pRobotProtocol->SetWorkSignalCallback([this](bool startWork, int cameraIndex) {
|
||
return this->OnRobotWorkSignal(startWork, cameraIndex);
|
||
});
|
||
|
||
|
||
LOG_INFO("Robot protocol initialization completed successfully\n");
|
||
return nRet;
|
||
|
||
}
|
||
|
||
// 初始化算法参数
|
||
int GrabBagPresenter::InitAlgorithmParams()
|
||
{
|
||
LOG_DEBUG("Start initializing algorithm parameters\n");
|
||
QString exePath = QCoreApplication::applicationFilePath();
|
||
|
||
// 初始化手眼标定矩阵为单位矩阵（4x4变换矩阵）
|
||
double initClibMatrix[16] = {
|
||
1.0, 0.0, 0.0, 0.0, // 第一行
|
||
0.0, 1.0, 0.0, 0.0, // 第二行
|
||
0.0, 0.0, 1.0, 0.0, // 第三行
|
||
0.0, 0.0, 0.0, 1.0 // 第四行
|
||
};
|
||
for (int i = 0; i < 16; i++) {
|
||
m_clibMatrix[i] = initClibMatrix[i];
|
||
}
|
||
|
||
// 获取手眼标定文件路径并确保文件存在
|
||
QString clibPath = PathManager::GetCalibrationFilePath();
|
||
|
||
LOG_INFO("Loading hand-eye calibration matrix from: %s\n", clibPath.toStdString().c_str());
|
||
if(QFile::exists(clibPath))
|
||
{
|
||
CVrConvert::LoadClibMatrix(clibPath.toStdString().c_str(), "clib", m_clibMatrix);
|
||
}
|
||
|
||
// 获取配置文件路径
|
||
QString configPath = PathManager::GetConfigFilePath();
|
||
|
||
LOG_INFO("Loading configuration from: %s\n", configPath.toStdString().c_str());
|
||
|
||
// 检查配置文件是否存在
|
||
QFileInfo configFileInfo(configPath);
|
||
if (!configFileInfo.exists()) {
|
||
LOG_ERROR("Configuration file does not exist: %s\n", configPath.toStdString().c_str());
|
||
return ERR_CODE(FILE_ERR_NOEXIST);
|
||
}
|
||
|
||
// 读取配置文件
|
||
ConfigResult configResult = m_vrConfig->LoadConfig(configPath.toStdString());
|
||
const VrAlgorithmParams& xmlParams = configResult.algorithmParams;
|
||
|
||
LOG_INFO("Loaded XML params - Bag: L=%.1f, W=%.1f, H=%.1f\n",
|
||
xmlParams.bagParam.bagL, xmlParams.bagParam.bagW, xmlParams.bagParam.bagH);
|
||
LOG_INFO("Loaded XML params - Pile: L=%.1f, W=%.1f, H=%.1f\n",
|
||
xmlParams.pileParam.pileL, xmlParams.pileParam.pileW, xmlParams.pileParam.pileH);
|
||
|
||
// 初始化算法参数结构
|
||
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;
|
||
|
||
// 设置角点特征参数
|
||
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;
|
||
|
||
// 设置增长参数
|
||
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=%.1f, W=%.1f, H=%.1f\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);
|
||
|
||
// 发送详细的页面状态信息
|
||
if (connected) {
|
||
m_pStatus->OnStatusUpdate("机械臂连接成功，通信正常");
|
||
} else {
|
||
m_pStatus->OnStatusUpdate("机械臂连接断开，请检查网络连接");
|
||
}
|
||
}
|
||
|
||
// 检查并更新工作状态
|
||
CheckAndUpdateWorkStatus();
|
||
}
|
||
|
||
bool GrabBagPresenter::OnRobotWorkSignal(bool startWork, int cameraIndex)
|
||
{
|
||
LOG_INFO("Received robot work signal: %s for camera index: %d\n", (startWork ? "start work" : "stop work"), cameraIndex);
|
||
|
||
int nRet = SUCCESS;
|
||
|
||
if (startWork) {
|
||
QString message = QString("收到开始工作信号，启动相机 %1 检测").arg(cameraIndex);
|
||
if (nullptr != m_pStatus) {
|
||
m_pStatus->OnStatusUpdate(message.toStdString());
|
||
}
|
||
nRet = StartDetection(cameraIndex);
|
||
} else {
|
||
QString message = QString("收到停止工作信号，停止相机 %1 检测").arg(cameraIndex);
|
||
if (nullptr != m_pStatus) {
|
||
m_pStatus->OnStatusUpdate(message.toStdString());
|
||
}
|
||
nRet = StopDetection();
|
||
}
|
||
|
||
return nRet == SUCCESS;
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
// 保存当前使用的相机ID（从1开始编号）
|
||
m_currentCameraIndex = cameraIndex;
|
||
|
||
// 通知UI工作状态变更为"正在工作"
|
||
if (m_pStatus) {
|
||
m_currentWorkStatus = WorkStatus::Working;
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Working);
|
||
}
|
||
|
||
// 设置机械臂工作状态为忙碌
|
||
if (m_pRobotProtocol) {
|
||
m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_BUSY);
|
||
}
|
||
|
||
if(m_vrEyeDeviceList.empty())
|
||
{
|
||
LOG_ERROR("No camera device found\n");
|
||
return ERR_CODE(DEV_NOT_FIND);
|
||
}
|
||
|
||
// 清空检测数据缓存（释放之前的内存）
|
||
_ClearDetectionDataCache();
|
||
|
||
int nRet = SUCCESS;
|
||
|
||
// 根据参数决定启动哪些相机
|
||
|
||
// 启动指定相机（cameraIndex为相机ID，从1开始编号）
|
||
int arrayIndex = cameraIndex - 1; // 转换为数组索引（从0开始）
|
||
if (arrayIndex >= 0 && arrayIndex < static_cast<int>(m_vrEyeDeviceList.size())) {
|
||
nRet = m_vrEyeDeviceList[arrayIndex]->StartDetect(&GrabBagPresenter::_StaticDetectionCallback, this);
|
||
if (nRet == SUCCESS) {
|
||
LOG_INFO("Camera ID %d start detection success\n", cameraIndex);
|
||
m_pStatus->OnStatusUpdate(QString("启动相机 %1 检测成功").arg(cameraIndex).toStdString());
|
||
} else {
|
||
LOG_ERROR("Camera ID %d start detection failed with error: %d\n", cameraIndex, nRet);
|
||
m_pStatus->OnStatusUpdate(QString("启动相机 %1 检测失败").arg(cameraIndex).toStdString());
|
||
}
|
||
} else {
|
||
LOG_ERROR("Invalid camera ID: %d, valid range is 1-%zu\n", cameraIndex, m_vrEyeDeviceList.size());
|
||
m_pStatus->OnStatusUpdate(QString("无效的相机ID: %1，有效范围: 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("检测已停止");
|
||
}
|
||
|
||
// 设置机械臂工作状态为空闲
|
||
if (m_pRobotProtocol) {
|
||
m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_IDLE);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
// 加载调试数据进行检测
|
||
int GrabBagPresenter::LoadDebugDataAndDetect(const std::string& filePath)
|
||
{
|
||
LOG_INFO("Loading debug data from file: %s\n", filePath.c_str());
|
||
|
||
m_currentWorkStatus = WorkStatus::Working;
|
||
m_currentCameraIndex = 1;
|
||
|
||
if (m_pStatus) {
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Working);
|
||
m_pStatus->OnStatusUpdate(QString("正加载: %1").arg(QString::fromStdString(filePath)).toStdString());
|
||
}
|
||
|
||
// 设置机械臂工作状态为忙碌
|
||
if (m_pRobotProtocol) {
|
||
m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_BUSY);
|
||
}
|
||
|
||
|
||
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());
|
||
}
|
||
|
||
// 等待检测完成（这里可以考虑异步处理）
|
||
// 为了简化，这里直接调用检测任务
|
||
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];
|
||
}
|
||
|
||
// 获取所有相机设备列表
|
||
std::vector<IVrEyeDevice*> GrabBagPresenter::GetCameraList()
|
||
{
|
||
return m_vrEyeDeviceList;
|
||
}
|
||
|
||
// 获取相机数量
|
||
int GrabBagPresenter::GetCameraCount()
|
||
{
|
||
return static_cast<int>(m_vrEyeDeviceList.size());
|
||
}
|
||
|
||
// 获取相机列表和名称
|
||
void GrabBagPresenter::GetCameraListWithNames(std::vector<IVrEyeDevice*>& cameraList,
|
||
std::vector<QString>& cameraNames)
|
||
{
|
||
cameraList.clear();
|
||
cameraNames.clear();
|
||
|
||
for (size_t i = 0; i < m_vrEyeDeviceList.size(); i++) {
|
||
if (m_vrEyeDeviceList[i] != nullptr) {
|
||
cameraList.push_back(m_vrEyeDeviceList[i]);
|
||
cameraNames.push_back(QString("相机 %1").arg(i + 1));
|
||
}
|
||
}
|
||
}
|
||
|
||
// 检查指定索引的相机是否连接
|
||
bool GrabBagPresenter::IsCameraConnected(int index)
|
||
{
|
||
if (index < 0 || index >= static_cast<int>(m_vrEyeDeviceList.size())) {
|
||
return false;
|
||
}
|
||
|
||
IVrEyeDevice* pDevice = m_vrEyeDeviceList[index];
|
||
return (pDevice != nullptr);
|
||
}
|
||
|
||
|
||
// 静态回调函数实现
|
||
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");
|
||
|
||
// 发送页面提示信息
|
||
if (m_pStatus) {
|
||
m_pStatus->OnStatusUpdate("相机扫描完成，开始数据处理...");
|
||
}
|
||
|
||
// 通知检测线程开始处理
|
||
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);
|
||
} else {
|
||
m_currentWorkStatus = WorkStatus::Error;
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Error);
|
||
}
|
||
}
|
||
|
||
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("[Algo Thread] 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("[Algo Thread] Using algorithm parameters from XML configuration\n");
|
||
LOG_INFO(" Bag: L=%.1f, W=%.1f, H=%.1f\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);
|
||
}
|
||
|
||
// 5. 调用算法检测接口（使用成员变量参数）
|
||
std::vector<SSG_peakRgnInfo> objOps;
|
||
sg_getBagPosition(static_cast<SVzNL3DLaserLine*>(m_detectionDataCache.data()), m_detectionDataCache.size(), m_algoParam, m_planeCalibParam, objOps);
|
||
|
||
LOG_INFO("[Algo Thread] 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);
|
||
|
||
// 转换检测结果为UI显示格式（使用机械臂坐标系数据）
|
||
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);
|
||
}
|
||
|
||
// 进行坐标转换：从算法坐标系转换到机械臂坐标系
|
||
SVzNL3DPoint targetObj;
|
||
targetObj.x = obj.centerPos.x;
|
||
targetObj.y = obj.centerPos.y;
|
||
targetObj.z = obj.centerPos.z;
|
||
|
||
SVzNL3DPoint robotObj;
|
||
CVrConvert::EyeToRobot(targetObj, robotObj, m_clibMatrix);
|
||
|
||
// 创建位置数据（使用转换后的机械臂坐标）
|
||
GrabBagPosition pos;
|
||
pos.x = robotObj.x; // 机械臂坐标X
|
||
pos.y = robotObj.y; // 机械臂坐标Y
|
||
pos.z = robotObj.z; // 机械臂坐标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("[Algo Thread] Object %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n",
|
||
i, obj.centerPos.x, obj.centerPos.y, obj.centerPos.z);
|
||
LOG_INFO("[Algo Thread] Object %zu Robot Coords: 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. 返回检测结果
|
||
// 调用检测结果回调函数
|
||
m_pStatus->OnDetectionResult(detectionResult);
|
||
|
||
// 更新状态
|
||
QString statusMsg = QString("检测完成，发现%1个目标").arg(objOps.size());
|
||
m_pStatus->OnStatusUpdate(statusMsg.toStdString());
|
||
|
||
// 更新机械臂协议状态（发送转换后的目标位置数据）
|
||
_SendDetectionResultToRobot(detectionResult, m_currentCameraIndex);
|
||
|
||
// 9. 检测完成后，将工作状态更新为"完成"
|
||
if (m_pStatus) {
|
||
m_currentWorkStatus = WorkStatus::Completed;
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Completed);
|
||
}
|
||
|
||
// 设置机械臂工作状态为相应相机工作完成（相机ID从1开始）
|
||
if (m_pRobotProtocol) {
|
||
uint16_t workStatus = (m_currentCameraIndex == 1) ?
|
||
RobotProtocol::WORK_STATUS_CAMERA1_DONE :
|
||
RobotProtocol::WORK_STATUS_CAMERA2_DONE;
|
||
m_pRobotProtocol->SetWorkStatus(workStatus);
|
||
}
|
||
|
||
// 恢复到就绪状态
|
||
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 DetectionResult& detectionResult, int cameraIndex)
|
||
{
|
||
if (!m_pRobotProtocol) {
|
||
LOG_WARNING("[Robot Task] Robot protocol not initialized, cannot send detection result\n");
|
||
return;
|
||
}
|
||
|
||
// 准备多目标数据结构
|
||
RobotProtocol::MultiTargetData multiTargetData;
|
||
|
||
// 检查是否有检测结果
|
||
if (detectionResult.positionLayout.empty() || detectionResult.positionLayout[0].position.empty()) {
|
||
LOG_INFO("[Robot Task] No objects detected, sending empty result to robot\n");
|
||
// 发送空的多目标数据
|
||
multiTargetData.count = 0;
|
||
multiTargetData.targets.clear();
|
||
|
||
if (m_pStatus) {
|
||
m_pStatus->OnStatusUpdate("没有检测到目标，发送空的多目标数据给机械臂");
|
||
}
|
||
return;
|
||
}
|
||
|
||
// 获取检测到的目标位置（已经是机械臂坐标系）
|
||
const auto& positions = detectionResult.positionLayout[0].position;
|
||
multiTargetData.count = static_cast<uint16_t>(positions.size());
|
||
|
||
// 直接使用已经转换好的机械臂坐标
|
||
for (size_t i = 0; i < positions.size(); i++) {
|
||
const GrabBagPosition& pos = positions[i];
|
||
|
||
// 直接使用已转换的坐标数据
|
||
RobotProtocol::TargetPosition robotTarget;
|
||
robotTarget.x = pos.x; // 已转换的X轴坐标
|
||
robotTarget.y = pos.y; // 已转换的Y轴坐标
|
||
robotTarget.z = pos.z; // 已转换的Z轴坐标
|
||
robotTarget.rz = pos.yaw; // Yaw角
|
||
|
||
// 添加到多目标数据
|
||
multiTargetData.targets.push_back(robotTarget);
|
||
}
|
||
|
||
// 发送多目标数据给机械臂（使用相机ID，从1开始编号）
|
||
int result = m_pRobotProtocol->SetMultiTargetData(multiTargetData, cameraIndex);
|
||
LOG_INFO("[Robot Task] SetMultiTargetData result: %d for camera ID: %d\n", result, cameraIndex);
|
||
if (m_pStatus) {
|
||
if (result != SUCCESS) {
|
||
m_pStatus->OnStatusUpdate(QString("发送多目标坐标给机械臂失败，相机ID: %1").arg(cameraIndex).toStdString());
|
||
} else {
|
||
m_pStatus->OnStatusUpdate(QString("发送多目标坐标给机械臂成功，相机ID: %1").arg(cameraIndex).toStdString());
|
||
}
|
||
}
|
||
}
|
||
|
||
// 点云转图像 - 简化版本
|
||
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);
|
||
|
||
// 绘制方向线
|
||
const double deg2rad = PI / 180.0;
|
||
double R = 100;
|
||
const double yaw = objOps[i].centerPos.z_yaw * deg2rad;
|
||
double cy = cos(yaw);
|
||
double sy = sin(yaw);
|
||
double x1 = objOps[i].centerPos.x + R * cy; double y1 = objOps[i].centerPos.y - R * sy;
|
||
double x2 = objOps[i].centerPos.x - R * cy; double y2 = objOps[i].centerPos.y + R * sy;
|
||
int px1 = (int)((x1 - x_range.min) / x_scale + x_skip);
|
||
int py1 = (int)((y1 - y_range.min) / y_scale + y_skip);
|
||
int px2 = (int)((x2 - x_range.min) / x_scale + x_skip);
|
||
int py2 = (int)((y2 - y_range.min) / y_scale + y_skip);
|
||
|
||
// 绘制方向线
|
||
painter.setPen(QPen(objColor, 3));
|
||
painter.drawLine(px1, py1, px2, py2);
|
||
|
||
// 绘制目标编号
|
||
painter.setPen(QPen(Qt::white, 1));
|
||
painter.setFont(QFont("Arial", 10, QFont::Bold));
|
||
painter.drawText(px + 15, py - 10, QString("%1").arg(i + 1));
|
||
|
||
}
|
||
}
|
||
|
||
return image;
|
||
}
|
||
|
||
// 实现配置改变通知接口
|
||
void GrabBagPresenter::OnConfigChanged(const ConfigResult& configResult)
|
||
{
|
||
LOG_INFO("Configuration changed notification received, reloading algorithm parameters\n");
|
||
|
||
// 重新初始化算法参数
|
||
int result = InitAlgorithmParams();
|
||
if (result == SUCCESS) {
|
||
LOG_INFO("Algorithm parameters reloaded successfully after config change\n");
|
||
if (m_pStatus) {
|
||
m_pStatus->OnStatusUpdate("配置已更新，算法参数重新加载成功");
|
||
}
|
||
} else {
|
||
LOG_ERROR("Failed to reload algorithm parameters after config change, error: %d\n", result);
|
||
if (m_pStatus) {
|
||
m_pStatus->OnStatusUpdate("配置更新后算法参数重新加载失败");
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|