296 lines
9.9 KiB
C++
296 lines
9.9 KiB
C++
#include "WeldSeamPresenter.h"
|
|
|
|
#include "VrConvert.h"
|
|
#include "VrError.h"
|
|
#include "VrLog.h"
|
|
|
|
#include <QApplication>
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <cstring>
|
|
#include <vector>
|
|
|
|
namespace {
|
|
|
|
constexpr uint16_t kCommandAddress = 0;
|
|
constexpr uint16_t kStatusAddress = 1;
|
|
constexpr uint16_t kPointCountAddress = 2;
|
|
constexpr uint16_t kPointDataAddress = 3;
|
|
constexpr size_t kMaxModbusPoints = 40;
|
|
constexpr size_t kRegistersPerPoint = 6; // X/Y/Z: 3 float32
|
|
|
|
int CacheSlotForCamera(int cameraIndex)
|
|
{
|
|
return (cameraIndex - 1) / 2;
|
|
}
|
|
|
|
void FloatToRegisters(float value, uint16_t& high, uint16_t& low)
|
|
{
|
|
uint32_t raw = 0;
|
|
std::memcpy(&raw, &value, sizeof(value));
|
|
high = static_cast<uint16_t>(raw >> 16);
|
|
low = static_cast<uint16_t>(raw & 0xffff);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
void WeldSeamPresenter::SendTCPResult(const DetectionResult& result)
|
|
{
|
|
if (!m_pTCPServer || !m_pTCPServer->IsConnected()) return;
|
|
if (!result.success || result.positions.empty()) {
|
|
m_pTCPServer->SendMessage(QStringLiteral("RESULT_NG"));
|
|
return;
|
|
}
|
|
|
|
QString packet = QStringLiteral("RESULT_OK_%1")
|
|
.arg(static_cast<int>(result.positions.size()));
|
|
for (const auto& point : result.positions) {
|
|
packet += QStringLiteral("_%1_%2_%3")
|
|
.arg(point.x, 0, 'f', 3)
|
|
.arg(point.y, 0, 'f', 3)
|
|
.arg(point.z, 0, 'f', 3);
|
|
}
|
|
m_pTCPServer->SendMessage(packet);
|
|
}
|
|
|
|
void WeldSeamPresenter::NotifyDetectionCompleted()
|
|
{
|
|
if (!m_pTCPServer || !m_pTCPServer->IsConnected()) {
|
|
return;
|
|
}
|
|
m_pTCPServer->SendMessage(QStringLiteral("RESULT_READY"));
|
|
}
|
|
|
|
int WeldSeamPresenter::GetLogicalCameraCount()
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_cameraStatusMutex);
|
|
return m_logicalCameraCount;
|
|
}
|
|
|
|
void WeldSeamPresenter::MarkCachedResultPending(int cameraIndex)
|
|
{
|
|
const int cacheSlot = CacheSlotForCamera(cameraIndex);
|
|
if (cacheSlot < 0 || cacheSlot >= static_cast<int>(m_cachedDetectionResults.size())) {
|
|
return;
|
|
}
|
|
|
|
std::lock_guard<std::mutex> lock(m_resultCacheMutex);
|
|
CachedDetectionResult& cached = m_cachedDetectionResults[static_cast<size_t>(cacheSlot)];
|
|
cached = CachedDetectionResult();
|
|
cached.state = ResultCacheState::Pending;
|
|
cached.result.cameraIndex = cameraIndex;
|
|
}
|
|
|
|
void WeldSeamPresenter::ClearCachedResult(int cameraIndex)
|
|
{
|
|
const int cacheSlot = CacheSlotForCamera(cameraIndex);
|
|
if (cacheSlot < 0 || cacheSlot >= static_cast<int>(m_cachedDetectionResults.size())) {
|
|
return;
|
|
}
|
|
|
|
std::lock_guard<std::mutex> lock(m_resultCacheMutex);
|
|
m_cachedDetectionResults[static_cast<size_t>(cacheSlot)] = CachedDetectionResult();
|
|
}
|
|
|
|
void WeldSeamPresenter::StoreCachedResult(const DetectionResult& result)
|
|
{
|
|
const int cacheSlot = CacheSlotForCamera(result.cameraIndex);
|
|
if (cacheSlot < 0 || cacheSlot >= static_cast<int>(m_cachedDetectionResults.size())) {
|
|
LOG_WARNING("WeldSeam ignored result cache for invalid camera %d\n", result.cameraIndex);
|
|
return;
|
|
}
|
|
|
|
std::lock_guard<std::mutex> lock(m_resultCacheMutex);
|
|
CachedDetectionResult& cached = m_cachedDetectionResults[static_cast<size_t>(cacheSlot)];
|
|
cached.result = result;
|
|
cached.state = result.success ? ResultCacheState::Ready : ResultCacheState::Failed;
|
|
}
|
|
|
|
bool WeldSeamPresenter::TransformDetectionResult(const DetectionResult& eyeResult,
|
|
int cameraIndex,
|
|
DetectionResult& robotResult) const
|
|
{
|
|
robotResult = eyeResult;
|
|
robotResult.cameraIndex = cameraIndex;
|
|
robotResult.positions.clear();
|
|
if (!eyeResult.success) {
|
|
return false;
|
|
}
|
|
if (!m_pConfigManager) {
|
|
robotResult.success = false;
|
|
robotResult.errorCode = ERR_CODE(DEV_NOT_FIND);
|
|
robotResult.message = QStringLiteral("配置未初始化");
|
|
return false;
|
|
}
|
|
|
|
const ConfigResult config = m_pConfigManager->GetConfigResult();
|
|
const VrHandEyeCalibMatrix* handEye = config.FindHandEyeMatrix(cameraIndex);
|
|
if (!handEye) {
|
|
robotResult.success = false;
|
|
robotResult.errorCode = ERR_CODE(DEV_DATA_INVALID);
|
|
robotResult.message = QStringLiteral("机械臂通道%1未配置手眼矩阵").arg(cameraIndex);
|
|
LOG_ERROR("WeldSeam camera %d has no hand-eye matrix\n", cameraIndex);
|
|
return false;
|
|
}
|
|
|
|
// 每次查询都从相机坐标副本转换，原始缓存可供同组另一机械臂复用。
|
|
robotResult.positions.reserve(eyeResult.positions.size());
|
|
for (const auto& point : eyeResult.positions) {
|
|
const SVzNL3DPoint eyePoint = {point.x, point.y, point.z};
|
|
SVzNL3DPoint robotPoint = {};
|
|
if (!CVrConvert::EyeToRobot(eyePoint, robotPoint, handEye->matrix)) {
|
|
robotResult.success = false;
|
|
robotResult.errorCode = ERR_CODE(DEV_DATA_INVALID);
|
|
robotResult.message = QStringLiteral("手眼坐标转换失败");
|
|
robotResult.positions.clear();
|
|
return false;
|
|
}
|
|
robotResult.positions.emplace_back(robotPoint.x, robotPoint.y, robotPoint.z);
|
|
}
|
|
robotResult.success = true;
|
|
robotResult.errorCode = 0;
|
|
return true;
|
|
}
|
|
|
|
void WeldSeamPresenter::SendCachedTCPResult(int cameraIndex)
|
|
{
|
|
if (!m_pTCPServer || !m_pTCPServer->IsConnected()) {
|
|
return;
|
|
}
|
|
if (cameraIndex <= 0 || cameraIndex > GetLogicalCameraCount()) {
|
|
m_pTCPServer->SendMessage(QStringLiteral("ERROR_INVALID_CAMERA"));
|
|
return;
|
|
}
|
|
|
|
const int cacheSlot = CacheSlotForCamera(cameraIndex);
|
|
CachedDetectionResult cached;
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_resultCacheMutex);
|
|
cached = m_cachedDetectionResults[static_cast<size_t>(cacheSlot)];
|
|
}
|
|
|
|
if (cached.state == ResultCacheState::Empty) {
|
|
m_pTCPServer->SendMessage(QStringLiteral("RESULT_EMPTY"));
|
|
return;
|
|
}
|
|
if (cached.state == ResultCacheState::Pending) {
|
|
m_pTCPServer->SendMessage(QStringLiteral("RESULT_PENDING"));
|
|
return;
|
|
}
|
|
if (cached.state == ResultCacheState::Failed) {
|
|
m_pTCPServer->SendMessage(QStringLiteral("RESULT_NG"));
|
|
return;
|
|
}
|
|
|
|
DetectionResult robotResult;
|
|
if (!TransformDetectionResult(cached.result, cameraIndex, robotResult)) {
|
|
m_pTCPServer->SendMessage(QStringLiteral("RESULT_NG"));
|
|
return;
|
|
}
|
|
SendTCPResult(robotResult);
|
|
}
|
|
|
|
void WeldSeamPresenter::PublishModbusResult(const DetectionResult& result)
|
|
{
|
|
if (!IsModbusServerRunning()) return;
|
|
const size_t count = result.success
|
|
? std::min(result.positions.size(), kMaxModbusPoints) : 0;
|
|
const uint16_t countRegister = static_cast<uint16_t>(count);
|
|
WriteModbusRegisters(kPointCountAddress, &countRegister, 1);
|
|
|
|
std::vector<uint16_t> registers(kMaxModbusPoints * kRegistersPerPoint, 0);
|
|
for (size_t i = 0; i < count; ++i) {
|
|
const float values[3] = {
|
|
static_cast<float>(result.positions[i].x),
|
|
static_cast<float>(result.positions[i].y),
|
|
static_cast<float>(result.positions[i].z)
|
|
};
|
|
for (size_t axis = 0; axis < 3; ++axis) {
|
|
const size_t offset = i * kRegistersPerPoint + axis * 2;
|
|
FloatToRegisters(values[axis], registers[offset], registers[offset + 1]);
|
|
}
|
|
}
|
|
WriteModbusRegisters(kPointDataAddress, registers.data(),
|
|
static_cast<uint16_t>(registers.size()));
|
|
UpdateModbusStatus(result.success && count > 0 ? 3 : 4);
|
|
}
|
|
|
|
void WeldSeamPresenter::ResetModbusResults()
|
|
{
|
|
if (!IsModbusServerRunning()) return;
|
|
std::vector<uint16_t> zeros(1 + kMaxModbusPoints * kRegistersPerPoint, 0);
|
|
WriteModbusRegisters(kPointCountAddress, zeros.data(), static_cast<uint16_t>(zeros.size()));
|
|
}
|
|
|
|
void WeldSeamPresenter::UpdateModbusStatus(uint16_t status)
|
|
{
|
|
if (IsModbusServerRunning()) WriteModbusRegisters(kStatusAddress, &status, 1);
|
|
}
|
|
|
|
int WeldSeamPresenter::InitTCPServer()
|
|
{
|
|
if (m_pTCPServer) return SUCCESS;
|
|
m_pTCPServer = new WeldSeamTCPProtocol();
|
|
if (QApplication::instance()) {
|
|
m_pTCPServer->moveToThread(QApplication::instance()->thread());
|
|
}
|
|
|
|
connect(m_pTCPServer, &WeldSeamTCPProtocol::ConnectionChanged,
|
|
this, &WeldSeamPresenter::OnTCPConnectionChanged);
|
|
connect(m_pTCPServer, &WeldSeamTCPProtocol::StartScanRequested,
|
|
this, [this](int cameraIndex)
|
|
{
|
|
if ((cameraIndex != 1 && cameraIndex != 3) ||
|
|
cameraIndex > GetLogicalCameraCount()) {
|
|
if (m_pTCPServer) {
|
|
m_pTCPServer->SendMessage(QStringLiteral("ERROR_INVALID_CAMERA"));
|
|
}
|
|
return;
|
|
}
|
|
const QString response = StartDynamicScan(cameraIndex);
|
|
if (m_pTCPServer) {
|
|
m_pTCPServer->SendMessage(response);
|
|
}
|
|
});
|
|
connect(m_pTCPServer, &WeldSeamTCPProtocol::StopScanRequested, this, [this]()
|
|
{
|
|
const QString response = StopDynamicScanAndDetect();
|
|
if (m_pTCPServer) {
|
|
m_pTCPServer->SendMessage(response);
|
|
}
|
|
});
|
|
connect(m_pTCPServer, &WeldSeamTCPProtocol::GetResultRequested,
|
|
this, &WeldSeamPresenter::SendCachedTCPResult);
|
|
|
|
const uint16_t port = m_pConfigManager
|
|
? m_pConfigManager->GetConfigResult().tcpPort : 7800;
|
|
if (!m_pTCPServer->StartServer(port)) {
|
|
delete m_pTCPServer;
|
|
m_pTCPServer = nullptr;
|
|
return ERR_CODE(DEV_OPEN_ERR);
|
|
}
|
|
m_tcpPort = port;
|
|
return SUCCESS;
|
|
}
|
|
|
|
void WeldSeamPresenter::StopTCPServer()
|
|
{
|
|
if (!m_pTCPServer) return;
|
|
m_pTCPServer->StopServer();
|
|
delete m_pTCPServer;
|
|
m_pTCPServer = nullptr;
|
|
m_bTCPConnected = false;
|
|
}
|
|
|
|
void WeldSeamPresenter::OnTCPConnectionChanged(bool connected)
|
|
{
|
|
m_bTCPConnected = connected;
|
|
if (!connected && m_scanState.load() == ScanState::Scanning) {
|
|
CancelDynamicScan();
|
|
}
|
|
if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) {
|
|
status->OnRobotConnectionChanged(connected);
|
|
status->OnStatusUpdate(connected ? "TCP客户端已连接" : "TCP客户端已断开");
|
|
}
|
|
}
|