145 lines
4.2 KiB
C++
145 lines
4.2 KiB
C++
#include "DetectionOutputConverter.h"
|
|
#include "IYScrewPositionStatus.h"
|
|
#include <cmath>
|
|
|
|
#ifndef M_PI
|
|
#define M_PI 3.14159265358979323846
|
|
#endif
|
|
|
|
static void AxialDirToEuler(double axialDirX, double axialDirY, double axialDirZ,
|
|
double rotateAngle,
|
|
double& roll, double& pitch, double& yaw)
|
|
{
|
|
roll = rotateAngle;
|
|
|
|
const double xyLen = std::sqrt(axialDirX * axialDirX + axialDirY * axialDirY);
|
|
pitch = std::atan2(-axialDirZ, xyLen) * 180.0 / M_PI;
|
|
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(
|
|
const std::vector<ScrewPosition>& positions,
|
|
int cameraIndex,
|
|
bool success,
|
|
int errorCode,
|
|
const QString& message)
|
|
{
|
|
ProtocolDetectionOutput output;
|
|
output.type = DETECTION_TYPE_TOOL_DISK;
|
|
output.success = success;
|
|
output.errorCode = errorCode;
|
|
output.message = message;
|
|
output.cameraIndex = cameraIndex;
|
|
|
|
for (const auto& pos : positions) {
|
|
ToolDiskDetectOutput toolOutput;
|
|
toolOutput.x = pos.x;
|
|
toolOutput.y = pos.y;
|
|
toolOutput.z = pos.z;
|
|
toolOutput.roll = pos.roll;
|
|
toolOutput.pitch = pos.pitch;
|
|
toolOutput.yaw = pos.yaw;
|
|
output.toolDiskOutputs.push_back(toolOutput);
|
|
}
|
|
|
|
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);
|
|
}
|