345 lines
11 KiB
C++
345 lines
11 KiB
C++
/**
|
||
* @file TCPServerMethods.cpp
|
||
* @brief DiscHolePosePresenter TCP/Modbus communication methods
|
||
*
|
||
* TCP 协议：
|
||
* - 请求: D1_X_Y_Z_A_B_C 表示 disc hole 检测
|
||
* - 请求: R1_X_Y_Z_A_B_C 表示 disc rack 检测
|
||
* - 响应: count_X_Y_Z_A_B_C
|
||
* (6 个浮点：目标点 XYZ + 姿态 A/B/C)
|
||
*
|
||
* Modbus 协议（默认端口 5020）：
|
||
* - 触发寄存器地址 0，工作状态地址 1，机器人位姿地址 2 (12 regs)
|
||
* - 结果起始地址 14 (12 regs = 6 floats)
|
||
*/
|
||
|
||
#include "DiscHolePosePresenter.h"
|
||
|
||
#include <QApplication>
|
||
|
||
#include "DetectionOutputConverter.h"
|
||
#include "VrLog.h"
|
||
|
||
#include <array>
|
||
#include <cstdio>
|
||
#include <cstring>
|
||
|
||
namespace {
|
||
|
||
constexpr uint16_t kModbusTriggerAddress = 0;
|
||
constexpr uint16_t kModbusWorkStatusAddress = 1;
|
||
constexpr uint16_t kModbusRobotPoseAddress = 2;
|
||
constexpr uint16_t kModbusRobotPoseRegisterCount = 12;
|
||
constexpr uint16_t kModbusResultStartAddress = 14;
|
||
// 结果寄存器：12 = 6 floats * 2 regs，目标点(6 floats)
|
||
constexpr uint16_t kModbusResultRegisterCount = 12;
|
||
|
||
constexpr uint16_t kDefaultModbusPort = 5020;
|
||
|
||
constexpr int kPoseOutputRxRyRz = 0;
|
||
constexpr int kPoseOutputRxRzRy = 1;
|
||
constexpr int kPoseOutputRyRxRz = 2;
|
||
constexpr int kPoseOutputRyRzRx = 3;
|
||
constexpr int kPoseOutputRzRxRy = 4;
|
||
constexpr int kPoseOutputRzRyRx = 5;
|
||
|
||
void FloatToRegisters(float value, uint16_t& high, uint16_t& low)
|
||
{
|
||
uint32_t raw = 0;
|
||
static_assert(sizeof(raw) == sizeof(value), "float size mismatch");
|
||
std::memcpy(&raw, &value, sizeof(value));
|
||
high = static_cast<uint16_t>((raw >> 16) & 0xFFFF);
|
||
low = static_cast<uint16_t>(raw & 0xFFFF);
|
||
}
|
||
|
||
void ReorderPoseAngles(double roll,
|
||
double pitch,
|
||
double yaw,
|
||
int poseOutputOrder,
|
||
double& angle1,
|
||
double& angle2,
|
||
double& angle3)
|
||
{
|
||
switch (poseOutputOrder) {
|
||
case kPoseOutputRxRzRy:
|
||
angle1 = roll;
|
||
angle2 = yaw;
|
||
angle3 = pitch;
|
||
break;
|
||
case kPoseOutputRyRxRz:
|
||
angle1 = pitch;
|
||
angle2 = roll;
|
||
angle3 = yaw;
|
||
break;
|
||
case kPoseOutputRyRzRx:
|
||
angle1 = pitch;
|
||
angle2 = yaw;
|
||
angle3 = roll;
|
||
break;
|
||
case kPoseOutputRzRxRy:
|
||
angle1 = yaw;
|
||
angle2 = roll;
|
||
angle3 = pitch;
|
||
break;
|
||
case kPoseOutputRzRyRx:
|
||
angle1 = yaw;
|
||
angle2 = pitch;
|
||
angle3 = roll;
|
||
break;
|
||
case kPoseOutputRxRyRz:
|
||
default:
|
||
angle1 = roll;
|
||
angle2 = pitch;
|
||
angle3 = yaw;
|
||
break;
|
||
}
|
||
}
|
||
|
||
template<typename PoseT>
|
||
void FillPoseRegisters(const PoseT& pose,
|
||
int poseOutputOrder,
|
||
std::array<uint16_t, kModbusResultRegisterCount>& registers)
|
||
{
|
||
double angle1 = 0.0;
|
||
double angle2 = 0.0;
|
||
double angle3 = 0.0;
|
||
ReorderPoseAngles(pose.roll, pose.pitch, pose.yaw, poseOutputOrder, angle1, angle2, angle3);
|
||
|
||
// 目标点（6 floats）
|
||
const float values[6] = {
|
||
static_cast<float>(pose.x),
|
||
static_cast<float>(pose.y),
|
||
static_cast<float>(pose.z),
|
||
static_cast<float>(angle1),
|
||
static_cast<float>(angle2),
|
||
static_cast<float>(angle3)
|
||
};
|
||
|
||
for (size_t i = 0; i < 6; ++i) {
|
||
FloatToRegisters(values[i], registers[i * 2], registers[i * 2 + 1]);
|
||
}
|
||
}
|
||
|
||
// 检测类型对应的中文标签
|
||
const char* DetectionTypeLabel(DetectionType type)
|
||
{
|
||
switch (type) {
|
||
case DETECTION_TYPE_DISC_HOLE:
|
||
return "盘孔";
|
||
case DETECTION_TYPE_DISC_RACK:
|
||
return "架子";
|
||
default:
|
||
return "未知";
|
||
}
|
||
}
|
||
|
||
} // namespace
|
||
|
||
// ============================================================================
|
||
// TCP 结果发送
|
||
// ============================================================================
|
||
|
||
void DiscHolePosePresenter::_SendDetectionResultToTCP(const DetectionResult& result, int cameraIndex)
|
||
{
|
||
if (!m_pTCPServer || !m_pTCPServer->IsConnected()) {
|
||
LOG_WARNING("TCP not connected, skip sending result\n");
|
||
return;
|
||
}
|
||
|
||
(void)cameraIndex;
|
||
|
||
int poseOutputOrder = 0;
|
||
if (m_pConfigManager) {
|
||
poseOutputOrder = m_pConfigManager->GetConfigResult().poseOutputOrder;
|
||
}
|
||
|
||
int count = static_cast<int>(result.positions.size());
|
||
QString resultText = QString::number(count);
|
||
|
||
// 每次只发送一个目标，取第一个
|
||
if (!result.positions.empty()) {
|
||
const auto& pos = result.positions.front();
|
||
double angle1 = 0.0;
|
||
double angle2 = 0.0;
|
||
double angle3 = 0.0;
|
||
ReorderPoseAngles(pos.roll, pos.pitch, pos.yaw, poseOutputOrder, angle1, angle2, angle3);
|
||
|
||
// 输出 6 个浮点：目标点(X/Y/Z + A/B/C)
|
||
// 使用 QString::arg 格式化
|
||
resultText += QString("_%1_%2_%3_%4_%5_%6")
|
||
.arg(pos.x, 0, 'f', 3)
|
||
.arg(pos.y, 0, 'f', 3)
|
||
.arg(pos.z, 0, 'f', 3)
|
||
.arg(angle1, 0, 'f', 3)
|
||
.arg(angle2, 0, 'f', 3)
|
||
.arg(angle3, 0, 'f', 3);
|
||
}
|
||
|
||
LOG_INFO("TCP result text (%s): %s\n",
|
||
DetectionTypeLabel(m_currentDetectionType),
|
||
resultText.toStdString().c_str());
|
||
|
||
m_pTCPServer->SendResult(resultText);
|
||
}
|
||
|
||
// ============================================================================
|
||
// Modbus 结果发布
|
||
// ============================================================================
|
||
|
||
void DiscHolePosePresenter::_PublishDetectionResultToModbus(const DetectionResult& result)
|
||
{
|
||
if (!IsModbusServerRunning()) {
|
||
return;
|
||
}
|
||
|
||
std::array<uint16_t, kModbusResultRegisterCount> registers{};
|
||
bool hasPose = false;
|
||
int poseOutputOrder = 0;
|
||
if (m_pConfigManager) {
|
||
poseOutputOrder = m_pConfigManager->GetConfigResult().poseOutputOrder;
|
||
}
|
||
|
||
if (!result.positions.empty()) {
|
||
FillPoseRegisters(result.positions.front(), poseOutputOrder, registers);
|
||
hasPose = true;
|
||
}
|
||
|
||
WriteModbusRegisters(kModbusResultStartAddress, registers.data(), kModbusResultRegisterCount);
|
||
LOG_DEBUG("Published Modbus pose registers (%s), hasPose=%d\n",
|
||
DetectionTypeLabel(m_currentDetectionType), hasPose ? 1 : 0);
|
||
|
||
_UpdateModbusWorkStatus(result.success ? 2 : 3);
|
||
}
|
||
|
||
void DiscHolePosePresenter::_ResetModbusResultRegisters()
|
||
{
|
||
if (!IsModbusServerRunning()) {
|
||
return;
|
||
}
|
||
|
||
std::array<uint16_t, kModbusResultRegisterCount> zeros{};
|
||
WriteModbusRegisters(kModbusResultStartAddress, zeros.data(), kModbusResultRegisterCount);
|
||
}
|
||
|
||
void DiscHolePosePresenter::_UpdateModbusWorkStatus(uint16_t statusValue)
|
||
{
|
||
if (!IsModbusServerRunning()) {
|
||
m_modbusWorkStatus = statusValue;
|
||
return;
|
||
}
|
||
|
||
m_modbusWorkStatus = statusValue;
|
||
WriteModbusRegisters(kModbusWorkStatusAddress, &statusValue, 1);
|
||
}
|
||
|
||
void DiscHolePosePresenter::_InitializeModbusRegisters()
|
||
{
|
||
if (m_modbusRegistersInitialized || !IsModbusServerRunning()) {
|
||
return;
|
||
}
|
||
|
||
const uint16_t zero = 0;
|
||
WriteModbusRegisters(kModbusTriggerAddress, &zero, 1);
|
||
|
||
std::array<uint16_t, kModbusRobotPoseRegisterCount> robotZeros{};
|
||
WriteModbusRegisters(kModbusRobotPoseAddress, robotZeros.data(), kModbusRobotPoseRegisterCount);
|
||
|
||
_ResetModbusResultRegisters();
|
||
_UpdateModbusWorkStatus(0);
|
||
m_modbusRegistersInitialized = true;
|
||
}
|
||
|
||
RobotPose6D DiscHolePosePresenter::_ReadRobotPoseFromModbus()
|
||
{
|
||
RobotPose6D pose;
|
||
return pose;
|
||
}
|
||
|
||
// ============================================================================
|
||
// TCP Server 生命周期
|
||
// ============================================================================
|
||
|
||
int DiscHolePosePresenter::InitTCPServer()
|
||
{
|
||
if (m_pTCPServer) {
|
||
LOG_WARNING("TCP server already initialized\n");
|
||
return 0;
|
||
}
|
||
|
||
m_pTCPServer = new DiscHolePoseTCPProtocol();
|
||
|
||
// 把 TCP 协议对象移到主线程，确保信号能通过事件循环正确投递
|
||
if (QApplication::instance()) {
|
||
m_pTCPServer->moveToThread(QApplication::instance()->thread());
|
||
}
|
||
|
||
// 连接 TCP 连接状态变化信号
|
||
// 注意：必须传入 this (DiscHolePosePresenter) 作为 context 对象，
|
||
// 因为 InitTCPServer() 在工作线程中调用，而 Presenter 在主线程创建，
|
||
// 不传 context 会导致 Qt 默认使用工作线程作为 receiver 线程，
|
||
// 信号在主线程 emit 后被 QueuedConnection 投递到工作线程（无事件循环），永远不执行。
|
||
QObject::connect(m_pTCPServer, &DiscHolePoseTCPProtocol::ConnectionChanged,
|
||
this, [this](bool connected) {
|
||
OnTCPConnectionChanged(connected);
|
||
});
|
||
|
||
// 连接检测触发信号
|
||
// TCP 命令：D1_X_Y_Z_A_B_C 为 disc hole，R1_X_Y_Z_A_B_C 为 disc rack
|
||
QObject::connect(m_pTCPServer, &DiscHolePoseTCPProtocol::DetectionTriggered,
|
||
this, [this](int cameraIndex, DetectionType detectionType, const RobotPose6D& robotPose) {
|
||
LOG_DEBUG("TCP triggered detection, cameraIndex: %d, type: %d (%s)\n",
|
||
cameraIndex, static_cast<int>(detectionType),
|
||
DetectionTypeLabel(detectionType));
|
||
TriggerDetection(cameraIndex, detectionType, robotPose);
|
||
});
|
||
|
||
uint16_t port = 7800;
|
||
if (m_pConfigManager) {
|
||
const ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
port = configResult.tcpPort;
|
||
}
|
||
|
||
if (!m_pTCPServer->StartServer(port)) {
|
||
LOG_ERROR("Failed to start TCP server on port %d\n", port);
|
||
delete m_pTCPServer;
|
||
m_pTCPServer = nullptr;
|
||
return -1;
|
||
}
|
||
|
||
LOG_DEBUG("TCP server started on port %d (DiscHolePose protocol: D1/R1 commands)\n", port);
|
||
return 0;
|
||
}
|
||
|
||
void DiscHolePosePresenter::stopServer()
|
||
{
|
||
if (m_pTCPServer) {
|
||
m_pTCPServer->StopServer();
|
||
delete m_pTCPServer;
|
||
m_pTCPServer = nullptr;
|
||
m_bTCPConnected = false;
|
||
LOG_DEBUG("TCP server stopped\n");
|
||
}
|
||
}
|
||
|
||
void DiscHolePosePresenter::OnTCPConnectionChanged(bool connected)
|
||
{
|
||
m_bTCPConnected = connected;
|
||
LOG_DEBUG("TCP connection changed: %s\n", connected ? "connected" : "disconnected");
|
||
|
||
if (auto pStatus = GetStatusCallback<IYDiscHolePoseStatus>()) {
|
||
pStatus->OnRobotConnectionChanged(connected);
|
||
pStatus->OnStatusUpdate(connected ? "TCP 客户端已连接" : "TCP 客户端已断开");
|
||
}
|
||
|
||
CheckAndUpdateWorkStatus();
|
||
}
|
||
|
||
// ============================================================================
|
||
// 对外发送接口
|
||
// ============================================================================
|
||
|
||
void DiscHolePosePresenter::SendDetectionResultToClient(const DetectionResult& result)
|
||
{
|
||
_SendDetectionResultToTCP(result, result.cameraIndex);
|
||
}
|