352 lines
11 KiB
C++
352 lines
11 KiB
C++
/**
|
||
* @file TCPServerMethods.cpp
|
||
* @brief TireHolePosePresenter TCP/Modbus communication methods
|
||
*
|
||
* TCP 协议（眼在手外）：
|
||
* - 请求: D1 触发相机1检测（无需机械臂位姿）
|
||
* - 响应: count_X1_Y1_Z1_A1_B1_C1_R1_... 返回全部 count 个目标
|
||
* (每个目标 7 个浮点：XYZ + 姿态 A/B/C + 孔半径 R)
|
||
*
|
||
* Modbus 协议（默认端口 5020）：
|
||
* - 触发寄存器地址 0，工作状态地址 1，机器人位姿地址 2 (12 regs)
|
||
* - 结果起始地址 14 (14 regs = 7 floats: XYZ + 姿态 A/B/C + 孔半径 R)
|
||
*/
|
||
|
||
#include "TireHolePosePresenter.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;
|
||
// 结果寄存器：14 = 7 floats * 2 regs，目标点(6 floats) + 孔半径(1 float)
|
||
constexpr uint16_t kModbusResultRegisterCount = 14;
|
||
|
||
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]);
|
||
}
|
||
}
|
||
|
||
} // namespace
|
||
|
||
// ============================================================================
|
||
// TCP 结果发送
|
||
// ============================================================================
|
||
|
||
void TireHolePosePresenter::_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;
|
||
}
|
||
|
||
const int count = static_cast<int>(result.positions.size());
|
||
QString resultText = QString::number(count);
|
||
|
||
// 返回全部检测结果
|
||
for (int i = 0; i < count; ++i) {
|
||
const auto& pos = result.positions[i];
|
||
double angle1 = 0.0;
|
||
double angle2 = 0.0;
|
||
double angle3 = 0.0;
|
||
ReorderPoseAngles(pos.roll, pos.pitch, pos.yaw, poseOutputOrder, angle1, angle2, angle3);
|
||
|
||
// 获取孔半径（优先从 tireHoleInfoList 获取，fallback 为 0）
|
||
double holeR = 0.0;
|
||
if (i < static_cast<int>(result.tireHoleInfoList.size())) {
|
||
holeR = result.tireHoleInfoList[i].holeR;
|
||
}
|
||
|
||
resultText += QString("_%1_%2_%3_%4_%5_%6_%7")
|
||
.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)
|
||
.arg(holeR, 0, 'f', 3);
|
||
}
|
||
|
||
LOG_INFO("TCP result: count=%d, text=%s\n",
|
||
count, resultText.toStdString().c_str());
|
||
|
||
m_pTCPServer->SendResult(resultText);
|
||
}
|
||
|
||
// ============================================================================
|
||
// Modbus 结果发布
|
||
// ============================================================================
|
||
|
||
void TireHolePosePresenter::_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;
|
||
|
||
// 追加输出孔半径 holeR（第 7 个 float，寄存器偏移 12-13）
|
||
double holeR = 0.0;
|
||
if (!result.tireHoleInfoList.empty()) {
|
||
holeR = result.tireHoleInfoList.front().holeR;
|
||
}
|
||
FloatToRegisters(static_cast<float>(holeR), registers[12], registers[13]);
|
||
}
|
||
|
||
WriteModbusRegisters(kModbusResultStartAddress, registers.data(), kModbusResultRegisterCount);
|
||
LOG_DEBUG("Published Modbus pose registers, hasPose=%d\n", hasPose ? 1 : 0);
|
||
|
||
_UpdateModbusWorkStatus(result.success ? 2 : 3);
|
||
}
|
||
|
||
void TireHolePosePresenter::_ResetModbusResultRegisters()
|
||
{
|
||
if (!IsModbusServerRunning()) {
|
||
return;
|
||
}
|
||
|
||
std::array<uint16_t, kModbusResultRegisterCount> zeros{};
|
||
WriteModbusRegisters(kModbusResultStartAddress, zeros.data(), kModbusResultRegisterCount);
|
||
}
|
||
|
||
void TireHolePosePresenter::_UpdateModbusWorkStatus(uint16_t statusValue)
|
||
{
|
||
if (!IsModbusServerRunning()) {
|
||
m_modbusWorkStatus = statusValue;
|
||
return;
|
||
}
|
||
|
||
m_modbusWorkStatus = statusValue;
|
||
WriteModbusRegisters(kModbusWorkStatusAddress, &statusValue, 1);
|
||
}
|
||
|
||
void TireHolePosePresenter::_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 TireHolePosePresenter::_ReadRobotPoseFromModbus()
|
||
{
|
||
RobotPose6D pose;
|
||
auto regToFloat = [this](int offset) -> float {
|
||
uint32_t raw = (static_cast<uint32_t>(m_modbusRobotPoseRegs[offset]) << 16)
|
||
| static_cast<uint32_t>(m_modbusRobotPoseRegs[offset + 1]);
|
||
float val = 0;
|
||
std::memcpy(&val, &raw, sizeof(val));
|
||
return val;
|
||
};
|
||
pose.x = regToFloat(0);
|
||
pose.y = regToFloat(2);
|
||
pose.z = regToFloat(4);
|
||
pose.a = regToFloat(6);
|
||
pose.b = regToFloat(8);
|
||
pose.c = regToFloat(10);
|
||
return pose;
|
||
}
|
||
|
||
// ============================================================================
|
||
// TCP Server 生命周期
|
||
// ============================================================================
|
||
|
||
int TireHolePosePresenter::InitTCPServer()
|
||
{
|
||
if (m_pTCPServer) {
|
||
LOG_WARNING("TCP server already initialized\n");
|
||
return 0;
|
||
}
|
||
|
||
m_pTCPServer = new TireHolePoseTCPProtocol();
|
||
|
||
// 把 TCP 协议对象移到主线程，确保信号能通过事件循环正确投递
|
||
if (QApplication::instance()) {
|
||
m_pTCPServer->moveToThread(QApplication::instance()->thread());
|
||
}
|
||
|
||
// 连接 TCP 连接状态变化信号
|
||
// 注意：必须传入 this (TireHolePosePresenter) 作为 context 对象，
|
||
// 因为 InitTCPServer() 在工作线程中调用，而 Presenter 在主线程创建，
|
||
// 不传 context 会导致 Qt 默认使用工作线程作为 receiver 线程，
|
||
// 信号在主线程 emit 后被 QueuedConnection 投递到工作线程（无事件循环），永远不执行。
|
||
QObject::connect(m_pTCPServer, &TireHolePoseTCPProtocol::ConnectionChanged,
|
||
this, [this](bool connected) {
|
||
OnTCPConnectionChanged(connected);
|
||
});
|
||
|
||
// 连接检测触发信号
|
||
// 眼在手外模式：TCP 命令 D1 为触发相机1检测
|
||
QObject::connect(m_pTCPServer, &TireHolePoseTCPProtocol::DetectionTriggered,
|
||
this, [this](int cameraIndex, const RobotPose6D& robotPose) {
|
||
LOG_DEBUG("TCP triggered detection, cameraIndex: %d\n", cameraIndex);
|
||
TriggerDetection(cameraIndex, 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 (TireHolePose protocol: D1/R1 commands)\n", port);
|
||
return 0;
|
||
}
|
||
|
||
void TireHolePosePresenter::stopServer()
|
||
{
|
||
if (m_pTCPServer) {
|
||
m_pTCPServer->StopServer();
|
||
delete m_pTCPServer;
|
||
m_pTCPServer = nullptr;
|
||
m_bTCPConnected = false;
|
||
LOG_DEBUG("TCP server stopped\n");
|
||
}
|
||
}
|
||
|
||
void TireHolePosePresenter::OnTCPConnectionChanged(bool connected)
|
||
{
|
||
m_bTCPConnected = connected;
|
||
LOG_DEBUG("TCP connection changed: %s\n", connected ? "connected" : "disconnected");
|
||
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnRobotConnectionChanged(connected);
|
||
pStatus->OnStatusUpdate(connected ? "TCP 客户端已连接" : "TCP 客户端已断开");
|
||
}
|
||
|
||
CheckAndUpdateWorkStatus();
|
||
}
|
||
|
||
// ============================================================================
|
||
// 对外发送接口
|
||
// ============================================================================
|
||
|
||
void TireHolePosePresenter::SendDetectionResultToClient(const DetectionResult& result)
|
||
{
|
||
_SendDetectionResultToTCP(result, result.cameraIndex);
|
||
}
|