144 lines
4.3 KiB
C++
144 lines
4.3 KiB
C++
#include "DetectionOutputConverter.h"
|
||
#include "IYScrewPositionStatus.h"
|
||
#include <QDateTime>
|
||
#include <cmath>
|
||
|
||
#ifndef M_PI
|
||
#define M_PI 3.14159265358979323846
|
||
#endif
|
||
|
||
/**
|
||
* @brief 将轴向方向向量转换为欧拉角（Roll, Pitch, Yaw）
|
||
*
|
||
* 轴向方向向量表示螺杆的朝向，转换为：
|
||
* - Pitch: 绕Y轴旋转角（俯仰角），由Z分量和XY平面分量计算
|
||
* - Yaw: 绕Z轴旋转角（偏航角），由X和Y分量计算
|
||
* - Roll: 使用算法输出的旋转角度 rotateAngle
|
||
*/
|
||
static void AxialDirToEuler(double axialDirX, double axialDirY, double axialDirZ,
|
||
double rotateAngle,
|
||
double& roll, double& pitch, double& yaw)
|
||
{
|
||
// Roll 直接使用算法的旋转角度
|
||
roll = rotateAngle;
|
||
|
||
// Pitch = atan2(-z, sqrt(x^2 + y^2)) 俯仰角
|
||
double xyLen = std::sqrt(axialDirX * axialDirX + axialDirY * axialDirY);
|
||
pitch = std::atan2(-axialDirZ, xyLen) * 180.0 / M_PI;
|
||
|
||
// Yaw = atan2(y, x) 偏航角
|
||
yaw = std::atan2(axialDirY, axialDirX) * 180.0 / M_PI;
|
||
}
|
||
|
||
ProtocolDetectionOutput DetectionOutputConverter::ConvertScrewResult(
|
||
const std::vector<ScrewInfo>& screwInfoList,
|
||
int cameraIndex,
|
||
bool success,
|
||
int errorCode,
|
||
const QString& message)
|
||
{
|
||
ProtocolDetectionOutput output;
|
||
output.type = DETECTION_TYPE_SCREW;
|
||
output.success = success;
|
||
output.errorCode = errorCode;
|
||
output.message = message;
|
||
output.cameraIndex = cameraIndex;
|
||
|
||
for (const auto& screw : screwInfoList) {
|
||
ScrewDetectOutput screwOutput;
|
||
screwOutput.x = screw.centerX;
|
||
screwOutput.y = screw.centerY;
|
||
screwOutput.z = screw.centerZ;
|
||
|
||
// 轴向方向转欧拉角
|
||
AxialDirToEuler(screw.axialDirX, screw.axialDirY, screw.axialDirZ,
|
||
screw.rotateAngle,
|
||
screwOutput.roll, screwOutput.pitch, screwOutput.yaw);
|
||
|
||
output.screwOutputs.push_back(screwOutput);
|
||
}
|
||
|
||
return output;
|
||
}
|
||
|
||
ProtocolDetectionOutput DetectionOutputConverter::ConvertToolDiskResult(int cameraIndex)
|
||
{
|
||
ProtocolDetectionOutput output;
|
||
output.type = DETECTION_TYPE_TOOL_DISK;
|
||
output.success = false;
|
||
output.errorCode = -100;
|
||
output.message = "工具盘检测算法接口尚未实现";
|
||
output.cameraIndex = cameraIndex;
|
||
return output;
|
||
}
|
||
|
||
QJsonObject DetectionOutputConverter::ScrewOutputToJson(const ProtocolDetectionOutput& output)
|
||
{
|
||
QJsonObject response;
|
||
response["MessageType"] = "ScrewResult";
|
||
response["Timestamp"] = output.timestamp;
|
||
|
||
QJsonObject data;
|
||
data["Success"] = output.success;
|
||
data["ErrorCode"] = output.errorCode;
|
||
data["Message"] = output.message;
|
||
data["CameraIndex"] = output.cameraIndex;
|
||
|
||
QJsonArray screwArray;
|
||
for (const auto& screw : output.screwOutputs) {
|
||
QJsonObject screwObj;
|
||
screwObj["X"] = screw.x;
|
||
screwObj["Y"] = screw.y;
|
||
screwObj["Z"] = screw.z;
|
||
screwObj["Roll"] = screw.roll;
|
||
screwObj["Pitch"] = screw.pitch;
|
||
screwObj["Yaw"] = screw.yaw;
|
||
screwArray.append(screwObj);
|
||
}
|
||
|
||
data["Count"] = static_cast<int>(output.screwOutputs.size());
|
||
data["Screws"] = screwArray;
|
||
|
||
response["Data"] = data;
|
||
return response;
|
||
}
|
||
|
||
QJsonObject DetectionOutputConverter::ToolDiskOutputToJson(const ProtocolDetectionOutput& output)
|
||
{
|
||
QJsonObject response;
|
||
response["MessageType"] = "ToolResult";
|
||
response["Timestamp"] = output.timestamp;
|
||
|
||
QJsonObject data;
|
||
data["Success"] = output.success;
|
||
data["ErrorCode"] = output.errorCode;
|
||
data["Message"] = output.message;
|
||
data["CameraIndex"] = output.cameraIndex;
|
||
|
||
QJsonArray toolDiskArray;
|
||
for (const auto& toolDisk : output.toolDiskOutputs) {
|
||
QJsonObject obj;
|
||
obj["X"] = toolDisk.x;
|
||
obj["Y"] = toolDisk.y;
|
||
obj["Z"] = toolDisk.z;
|
||
obj["Roll"] = toolDisk.roll;
|
||
obj["Pitch"] = toolDisk.pitch;
|
||
obj["Yaw"] = toolDisk.yaw;
|
||
toolDiskArray.append(obj);
|
||
}
|
||
|
||
data["Count"] = static_cast<int>(output.toolDiskOutputs.size());
|
||
data["ToolDisks"] = toolDiskArray;
|
||
|
||
response["Data"] = data;
|
||
return response;
|
||
}
|
||
|
||
QJsonObject DetectionOutputConverter::ToJson(const ProtocolDetectionOutput& output)
|
||
{
|
||
if (output.type == DETECTION_TYPE_TOOL_DISK) {
|
||
return ToolDiskOutputToJson(output);
|
||
}
|
||
return ScrewOutputToJson(output);
|
||
}
|