529 lines
17 KiB
C++
529 lines
17 KiB
C++
#include "GrabBagPresenter.h"
|
||
#include "VrError.h"
|
||
#include "VrSimpleLog.h"
|
||
#include <QtCore/QCoreApplication>
|
||
#include <QtCore/QFileInfo>
|
||
#include <QtCore/QDir>
|
||
|
||
GrabBagPresenter::GrabBagPresenter()
|
||
: m_vrConfig(nullptr)
|
||
, m_pStatus(nullptr)
|
||
, m_pRobotProtocol(nullptr)
|
||
, m_bCameraConnected(false)
|
||
, m_bRobotConnected(false)
|
||
, m_currentWorkStatus(WorkStatus::Error)
|
||
{
|
||
|
||
}
|
||
|
||
GrabBagPresenter::~GrabBagPresenter()
|
||
{
|
||
// 释放机械臂协议
|
||
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("机械臂服务初始化成功");
|
||
}
|
||
|
||
// 通知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) {
|
||
this->OnRobotWorkSignal(startWork);
|
||
});
|
||
|
||
// 初始化协议服务（使用端口503，避免与原有ModbusTCP冲突）
|
||
int nRet = m_pRobotProtocol->Initialize();
|
||
ERR_CODE_RETURN(nRet);
|
||
|
||
LOG_INFO("Robot protocol initialization successful\n");
|
||
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)
|
||
{
|
||
LOG_INFO("Received robot work signal: %s\n", (startWork ? "start work" : "stop work"));
|
||
|
||
if (nullptr == m_pStatus) {
|
||
return;
|
||
}
|
||
if (startWork) {
|
||
m_pStatus->OnStatusUpdate("收到开始工作信号，启动检测");
|
||
StartDetection();
|
||
} else {
|
||
m_pStatus->OnStatusUpdate("收到停止工作信号，停止检测");
|
||
StopDetection();
|
||
}
|
||
}
|
||
|
||
void GrabBagPresenter::SetStatusCallback(IYGrabBagStatus* status)
|
||
{
|
||
m_pStatus = status;
|
||
}
|
||
|
||
// 模拟检测函数，用于演示
|
||
int GrabBagPresenter::StartDetection()
|
||
{
|
||
LOG_INFO("Start detection\n");
|
||
|
||
// 检查设备状态是否准备就绪
|
||
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);
|
||
}
|
||
|
||
// 清空检测数据缓存
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_detectionDataMutex);
|
||
m_detectionDataCache.clear();
|
||
}
|
||
|
||
// 启动所有相机的检测
|
||
for (size_t i = 0; i < m_vrEyeDeviceList.size(); ++i) {
|
||
IVrEyeDevice* pDevice = m_vrEyeDeviceList[i];
|
||
if (pDevice) {
|
||
int ret = pDevice->StartDetect(&GrabBagPresenter::_StaticDetectionCallback, this);
|
||
if (ret == 0) {
|
||
LOG_INFO("Camera %zu start detection successfully\n", i + 1);
|
||
} else {
|
||
LOG_WARNING("Camera %zu start detection failed, error code: %d\n", i + 1, ret);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
return 0;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
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, dataLength=%u\n", (int)eStatus, nDataLength);
|
||
|
||
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/trigger signal from camera\n");
|
||
|
||
// 如果当前状态为就绪，通知检测线程开始处理
|
||
if (m_currentWorkStatus == WorkStatus::Ready) {
|
||
LOG_INFO("Notifying detection thread to start processing\n");
|
||
|
||
// 通知算法检测线程
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_algoDetectMutex);
|
||
// 这里可以设置一个标志位，或者直接通知条件变量
|
||
}
|
||
m_algoDetectCondition.notify_one();
|
||
|
||
if (m_pStatus) {
|
||
m_pStatus->OnStatusUpdate("收到扫描信号，开始检测处理");
|
||
}
|
||
} else {
|
||
LOG_WARNING("Received scan signal but device not ready, current status: %d\n", (int)m_currentWorkStatus);
|
||
}
|
||
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;
|
||
}
|
||
|
||
// 将检测数据保存到缓存中
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_detectionDataMutex);
|
||
m_detectionDataCache.push_back(pLaserLinePoint);
|
||
|
||
// LOG_DEBUG("Detection data cached: Frame %llu, Points: %d\n",
|
||
// pLaserLinePoint->sLineData.llFrameIdx,
|
||
// pLaserLinePoint->sLineData.nPointCount);
|
||
}
|
||
}
|
||
|
||
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::Ready;
|
||
});
|
||
|
||
// 检查设备状态是否准备就绪
|
||
|
||
_DetectTask();
|
||
}
|
||
}
|
||
|
||
int GrabBagPresenter::_DetectTask()
|
||
{
|
||
// 创建测试图像
|
||
QImage image(":/resource/result_image.jpg");
|
||
|
||
// 创建测试数据
|
||
DetectionResult testResult;
|
||
testResult.image = image;
|
||
|
||
// 创建第一层数据
|
||
GrabBagPositionLayout layer1;
|
||
layer1.layerIndex = 1;
|
||
|
||
GrabBagPosition pos1;
|
||
pos1.x = 100.5;
|
||
pos1.y = 200.3;
|
||
pos1.z = 50.22;
|
||
pos1.roll = 45.0;
|
||
pos1.pitch = 10.0;
|
||
pos1.yaw = 20.0;
|
||
|
||
GrabBagPosition pos2;
|
||
pos2.x = 110.8;
|
||
pos2.y = 220.5;
|
||
pos2.z = 52.1;
|
||
pos2.roll = 48.5;
|
||
pos2.pitch = 12.3;
|
||
pos2.yaw = 22.7;
|
||
|
||
layer1.position.push_back(pos1);
|
||
layer1.position.push_back(pos2);
|
||
|
||
// 创建第二层数据
|
||
GrabBagPositionLayout layer2;
|
||
layer2.layerIndex = 2;
|
||
|
||
GrabBagPosition pos3;
|
||
pos3.x = 150.2;
|
||
pos3.y = 180.7;
|
||
pos3.z = 55.0;
|
||
pos3.roll = 30.0;
|
||
pos3.pitch = 15.0;
|
||
pos3.yaw = 25.0;
|
||
|
||
layer2.position.push_back(pos3);
|
||
|
||
// 添加层数据到结果
|
||
testResult.positionLayout.push_back(layer1);
|
||
testResult.positionLayout.push_back(layer2);
|
||
|
||
// 调用检测结果回调函数
|
||
m_pStatus->OnDetectionResult(testResult);
|
||
|
||
// 更新状态
|
||
m_pStatus->OnStatusUpdate("检测完成，发现3个目标");
|
||
|
||
// 检测完成后，将工作状态更新为"完成"
|
||
if (m_pStatus) {
|
||
m_currentWorkStatus = WorkStatus::Completed;
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Completed);
|
||
}
|
||
|
||
// 更新机械臂协议状态
|
||
if (m_pRobotProtocol) {
|
||
|
||
// 更新检测结果坐标（以第一个检测到的目标为例）
|
||
if (!testResult.positionLayout.empty() && !testResult.positionLayout[0].position.empty()) {
|
||
const auto& firstPos = testResult.positionLayout[0].position[0];
|
||
RobotProtocol::RobotCoordinate detectedCoord;
|
||
detectedCoord.x = firstPos.x;
|
||
detectedCoord.y = firstPos.y;
|
||
detectedCoord.z = firstPos.z;
|
||
detectedCoord.rx = firstPos.roll;
|
||
detectedCoord.ry = firstPos.pitch;
|
||
detectedCoord.rz = firstPos.yaw;
|
||
m_pRobotProtocol->SetRobotCoordinate(detectedCoord);
|
||
|
||
LOG_INFO("Update detection result coordinates: X=%.2f, Y=%.2f, Z=%.2f\n",
|
||
detectedCoord.x, detectedCoord.y, detectedCoord.z);
|
||
}
|
||
}
|
||
|
||
m_currentWorkStatus = WorkStatus::Ready;
|
||
return SUCCESS;
|
||
}
|