770 lines
40 KiB
C++
770 lines
40 KiB
C++
#include "VrConfig.h"
|
||
#include <iostream>
|
||
#include <vector>
|
||
#include <string>
|
||
|
||
#include "VrLog.h"
|
||
|
||
using namespace tinyxml2;
|
||
|
||
namespace
|
||
{
|
||
DebugDataStorageType ParseDebugStorageType(const char* value)
|
||
{
|
||
if (!value)
|
||
{
|
||
return DebugDataStorageType::Text;
|
||
}
|
||
const std::string type(value);
|
||
return type == "1" || type == "binary" || type == "Binary"
|
||
? DebugDataStorageType::Binary : DebugDataStorageType::Text;
|
||
}
|
||
|
||
const char* DebugStorageTypeName(DebugDataStorageType type)
|
||
{
|
||
return type == DebugDataStorageType::Binary ? "binary" : "text";
|
||
}
|
||
}
|
||
|
||
CVrConfig::CVrConfig() : m_pNotify(nullptr)
|
||
{
|
||
// 构造函数
|
||
}
|
||
|
||
CVrConfig::~CVrConfig()
|
||
{
|
||
// 析构函数
|
||
}
|
||
|
||
int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResult)
|
||
{
|
||
// 使用tinyxml2库加载XML文件
|
||
XMLDocument doc;
|
||
XMLError err = doc.LoadFile(filePath.c_str());
|
||
if (err != XML_SUCCESS)
|
||
{
|
||
LOG_ERR("open config file failed: %s\n", filePath.c_str());
|
||
return LOAD_CONFIG_FILE_NOT_FOUND;
|
||
}
|
||
|
||
// 获取根元素
|
||
XMLElement* root = doc.RootElement();
|
||
if (!root || std::string(root->Name()) != "StatorWorkpiecePositionConfig")
|
||
{
|
||
std::cerr << "config file format error: root element is not StatorWorkpiecePositionConfig" << std::endl;
|
||
return LOAD_CONFIG_INVALID_FORMAT;
|
||
}
|
||
|
||
// 解析摄像头列表
|
||
XMLElement* camerasElement = root->FirstChildElement("Cameras");
|
||
if (camerasElement)
|
||
{
|
||
XMLElement* cameraElement = camerasElement->FirstChildElement("Camera");
|
||
while (cameraElement)
|
||
{
|
||
DeviceInfo camera;
|
||
if (cameraElement->Attribute("name"))
|
||
camera.name = cameraElement->Attribute("name");
|
||
|
||
if (cameraElement->Attribute("ip"))
|
||
camera.ip = cameraElement->Attribute("ip");
|
||
configResult.cameraList.push_back(camera);
|
||
cameraElement = cameraElement->NextSiblingElement("Camera");
|
||
}
|
||
}
|
||
|
||
// 解析设备列表
|
||
XMLElement* devicesElement = root->FirstChildElement("Devices");
|
||
if (devicesElement)
|
||
{
|
||
XMLElement* deviceElement = devicesElement->FirstChildElement("Device");
|
||
while (deviceElement)
|
||
{
|
||
DeviceInfo device;
|
||
if (deviceElement->Attribute("name"))
|
||
device.name = deviceElement->Attribute("name");
|
||
|
||
if (deviceElement->Attribute("ip"))
|
||
device.ip = deviceElement->Attribute("ip");
|
||
|
||
configResult.deviceList.push_back(device);
|
||
deviceElement = deviceElement->NextSiblingElement("Device");
|
||
}
|
||
}
|
||
|
||
// 解析算法参数
|
||
configResult.algorithmParams.pointCloudFilterParams.clear();
|
||
configResult.algorithmParams.modelGraySelection = VrModelGraySelectionParam();
|
||
configResult.algorithmParams.modelDetectionRegion = VrModelDetectionRegion();
|
||
XMLElement* algoParamsElement = root->FirstChildElement("AlgorithmParams");
|
||
if (algoParamsElement)
|
||
{
|
||
for (XMLElement* filter = algoParamsElement->FirstChildElement("PointCloudFilterParam");
|
||
filter != nullptr;
|
||
filter = filter->NextSiblingElement("PointCloudFilterParam"))
|
||
{
|
||
VrPointCloudFilterParam param;
|
||
filter->QueryIntAttribute("cameraIndex", ¶m.cameraIndex);
|
||
if (const char* name = filter->Attribute("cameraName"))
|
||
{
|
||
param.cameraName = name;
|
||
}
|
||
filter->QueryBoolAttribute("enabled", ¶m.enabled);
|
||
filter->QueryDoubleAttribute("minX", ¶m.minX);
|
||
filter->QueryDoubleAttribute("maxX", ¶m.maxX);
|
||
filter->QueryDoubleAttribute("minY", ¶m.minY);
|
||
filter->QueryDoubleAttribute("maxY", ¶m.maxY);
|
||
filter->QueryDoubleAttribute("minZ", ¶m.minZ);
|
||
filter->QueryDoubleAttribute("maxZ", ¶m.maxZ);
|
||
configResult.algorithmParams.pointCloudFilterParams.push_back(param);
|
||
}
|
||
|
||
// 解析工具姿态与目标点补偿参数（与 WorkpieceHole 配置方式一致）
|
||
XMLElement* toolParamElement = algoParamsElement->FirstChildElement("ToolParam");
|
||
if (toolParamElement)
|
||
{
|
||
if (toolParamElement->Attribute("eulerOrder"))
|
||
configResult.algorithmParams.toolParam.eulerOrder = toolParamElement->IntAttribute("eulerOrder");
|
||
if (toolParamElement->Attribute("rotX"))
|
||
configResult.algorithmParams.toolParam.rotX = toolParamElement->DoubleAttribute("rotX");
|
||
if (toolParamElement->Attribute("rotY"))
|
||
configResult.algorithmParams.toolParam.rotY = toolParamElement->DoubleAttribute("rotY");
|
||
if (toolParamElement->Attribute("rotZ"))
|
||
configResult.algorithmParams.toolParam.rotZ = toolParamElement->DoubleAttribute("rotZ");
|
||
if (toolParamElement->Attribute("offsetX"))
|
||
configResult.algorithmParams.toolParam.offsetX = toolParamElement->DoubleAttribute("offsetX");
|
||
if (toolParamElement->Attribute("offsetY"))
|
||
configResult.algorithmParams.toolParam.offsetY = toolParamElement->DoubleAttribute("offsetY");
|
||
if (toolParamElement->Attribute("offsetZ"))
|
||
configResult.algorithmParams.toolParam.offsetZ = toolParamElement->DoubleAttribute("offsetZ");
|
||
}
|
||
|
||
// 类别映射独立于有序的曝光/增益/阈值参数列表。
|
||
XMLElement* modelParamElement = algoParamsElement->FirstChildElement("WorkpieceClassMapping");
|
||
if (!modelParamElement)
|
||
{
|
||
// 兼容旧配置中的单项节点，读取后不再写回该节点。
|
||
modelParamElement = algoParamsElement->FirstChildElement("WorkpieceModelParam");
|
||
}
|
||
if (modelParamElement)
|
||
{
|
||
VrWorkpieceModelParam& modelParam = configResult.algorithmParams.modelParam;
|
||
if (modelParamElement->Attribute("exposure"))
|
||
modelParam.exposure = modelParamElement->UnsignedAttribute("exposure");
|
||
if (modelParamElement->Attribute("gain"))
|
||
modelParam.gain = modelParamElement->UnsignedAttribute("gain");
|
||
if (modelParamElement->Attribute("minimumScore"))
|
||
modelParam.minimumScore = modelParamElement->DoubleAttribute("minimumScore");
|
||
if (modelParamElement->Attribute("class0WorkpieceType"))
|
||
modelParam.class0WorkpieceType = modelParamElement->IntAttribute("class0WorkpieceType");
|
||
if (modelParamElement->Attribute("class1WorkpieceType"))
|
||
modelParam.class1WorkpieceType = modelParamElement->IntAttribute("class1WorkpieceType");
|
||
if (modelParamElement->Attribute("class2WorkpieceType"))
|
||
modelParam.class2WorkpieceType = modelParamElement->IntAttribute("class2WorkpieceType");
|
||
}
|
||
configResult.algorithmParams.modelParams.clear();
|
||
XMLElement* modelParamsElement = algoParamsElement->FirstChildElement("WorkpieceModelParams");
|
||
if (modelParamsElement)
|
||
{
|
||
if (XMLElement* regionElement = modelParamsElement->FirstChildElement("DetectionRegion"))
|
||
{
|
||
VrModelDetectionRegion region;
|
||
regionElement->QueryBoolAttribute("enabled", ®ion.enabled);
|
||
regionElement->QueryIntAttribute("left", ®ion.left);
|
||
regionElement->QueryIntAttribute("top", ®ion.top);
|
||
regionElement->QueryIntAttribute("right", ®ion.right);
|
||
regionElement->QueryIntAttribute("bottom", ®ion.bottom);
|
||
if (region.isValid())
|
||
{
|
||
configResult.algorithmParams.modelDetectionRegion = region;
|
||
}
|
||
}
|
||
VrModelGraySelectionParam graySelection;
|
||
modelParamsElement->QueryDoubleAttribute("targetGray", &graySelection.targetGray);
|
||
modelParamsElement->QueryDoubleAttribute("maxGrayDeviation", &graySelection.maxGrayDeviation);
|
||
if (graySelection.isValid())
|
||
{
|
||
configResult.algorithmParams.modelGraySelection = graySelection;
|
||
}
|
||
for (XMLElement* item = modelParamsElement->FirstChildElement("Model"); item;
|
||
item = item->NextSiblingElement("Model"))
|
||
{
|
||
VrWorkpieceModelParam model;
|
||
if (item->Attribute("exposure"))
|
||
model.exposure = item->UnsignedAttribute("exposure");
|
||
if (item->Attribute("gain"))
|
||
model.gain = item->UnsignedAttribute("gain");
|
||
if (item->Attribute("minimumScore"))
|
||
model.minimumScore = item->DoubleAttribute("minimumScore");
|
||
configResult.algorithmParams.modelParams.push_back(model);
|
||
}
|
||
}
|
||
XMLElement* fullImageCaptureElement = algoParamsElement->FirstChildElement("FullImageCaptureParam");
|
||
if (fullImageCaptureElement)
|
||
{
|
||
VrFullImageCaptureParam& captureParam = configResult.algorithmParams.fullImageCaptureParam;
|
||
if (fullImageCaptureElement->Attribute("exposure"))
|
||
captureParam.exposure = fullImageCaptureElement->UnsignedAttribute("exposure");
|
||
if (fullImageCaptureElement->Attribute("gain"))
|
||
captureParam.gain = fullImageCaptureElement->UnsignedAttribute("gain");
|
||
}
|
||
if (modelParamsElement != nullptr && !configResult.algorithmParams.modelParams.empty())
|
||
{
|
||
// 兼容仍使用 modelParam 的调用方，但曝光、增益和阈值以列表首项为准。
|
||
const VrWorkpieceModelParam& firstModel = configResult.algorithmParams.modelParams.front();
|
||
configResult.algorithmParams.modelParam.exposure = firstModel.exposure;
|
||
configResult.algorithmParams.modelParam.gain = firstModel.gain;
|
||
configResult.algorithmParams.modelParam.minimumScore = firstModel.minimumScore;
|
||
}
|
||
else
|
||
{
|
||
// 没有列表时兼容旧的全图采集参数，再迁移为单个列表项。
|
||
if (fullImageCaptureElement)
|
||
{
|
||
configResult.algorithmParams.modelParam.exposure =
|
||
configResult.algorithmParams.fullImageCaptureParam.exposure;
|
||
configResult.algorithmParams.modelParam.gain =
|
||
configResult.algorithmParams.fullImageCaptureParam.gain;
|
||
}
|
||
configResult.algorithmParams.modelParams.push_back(configResult.algorithmParams.modelParam);
|
||
}
|
||
|
||
XMLElement* binParamElement = algoParamsElement->FirstChildElement("BinParam");
|
||
if (binParamElement)
|
||
{
|
||
VrBinParam& binParam = configResult.algorithmParams.binParam;
|
||
if (binParamElement->Attribute("useConfiguredBin"))
|
||
binParam.useConfiguredBin = binParamElement->BoolAttribute("useConfiguredBin");
|
||
if (binParamElement->Attribute("binHeight"))
|
||
binParam.binHeight = binParamElement->DoubleAttribute("binHeight");
|
||
if (binParamElement->Attribute("bin_zSliceRange"))
|
||
binParam.bin_zSliceRange = binParamElement->DoubleAttribute("bin_zSliceRange");
|
||
if (binParamElement->Attribute("length"))
|
||
binParam.length = binParamElement->DoubleAttribute("length");
|
||
if (binParamElement->Attribute("width"))
|
||
binParam.width = binParamElement->DoubleAttribute("width");
|
||
if (binParamElement->Attribute("binTopZ"))
|
||
binParam.binTopZ = binParamElement->DoubleAttribute("binTopZ");
|
||
|
||
auto readVec3 = [binParamElement](const char* prefix, double values[3]) {
|
||
char attrName[64];
|
||
for (int i = 0; i < 3; ++i) {
|
||
sprintf(attrName, "%s%d", prefix, i);
|
||
if (binParamElement->Attribute(attrName))
|
||
values[i] = binParamElement->DoubleAttribute(attrName);
|
||
}
|
||
};
|
||
readVec3("center", binParam.center);
|
||
readVec3("bottomNormal", binParam.bottomNormal);
|
||
readVec3("xDir", binParam.xDir);
|
||
readVec3("yDir", binParam.yDir);
|
||
for (int vertex = 0; vertex < 4; ++vertex) {
|
||
char attrName[64];
|
||
for (int axis = 0; axis < 3; ++axis) {
|
||
sprintf(attrName, "minRectVertex%d_%d", vertex, axis);
|
||
if (binParamElement->Attribute(attrName))
|
||
binParam.minRectVertex[vertex][axis] =
|
||
binParamElement->DoubleAttribute(attrName);
|
||
}
|
||
}
|
||
}
|
||
|
||
XMLElement* rotorCoreParamElement = algoParamsElement->FirstChildElement("RotorCoreParam");
|
||
if (rotorCoreParamElement)
|
||
{
|
||
VrRotorCoreParam& rotorParam = configResult.algorithmParams.rotorCoreParam;
|
||
if (rotorCoreParamElement->Attribute("radius"))
|
||
rotorParam.radius = rotorCoreParamElement->DoubleAttribute("radius");
|
||
if (rotorCoreParamElement->Attribute("height"))
|
||
rotorParam.height = rotorCoreParamElement->DoubleAttribute("height");
|
||
if (rotorCoreParamElement->Attribute("spacerLength"))
|
||
{
|
||
rotorParam.spacerLength = rotorCoreParamElement->DoubleAttribute("spacerLength");
|
||
}
|
||
if (rotorCoreParamElement->Attribute("spacerWidth"))
|
||
{
|
||
rotorParam.spacerWidth = rotorCoreParamElement->DoubleAttribute("spacerWidth");
|
||
}
|
||
if (rotorCoreParamElement->Attribute("layerRatio"))
|
||
rotorParam.layerRatio = rotorCoreParamElement->DoubleAttribute("layerRatio");
|
||
if (rotorCoreParamElement->Attribute("distToSideRatio"))
|
||
rotorParam.distToSideRatio = rotorCoreParamElement->DoubleAttribute("distToSideRatio");
|
||
if (rotorCoreParamElement->Attribute("groundRemoveOffset"))
|
||
rotorParam.groundRemoveOffset = rotorCoreParamElement->DoubleAttribute("groundRemoveOffset");
|
||
|
||
// 旧配置或无效枚举回退到工件自身法向，仅 1 表示使用调平后的地面法向。
|
||
rotorParam.useGroundCalibNormal = rotorCoreParamElement->IntAttribute("useGroundCalibNormal") == 1 ? 1 : 0;
|
||
|
||
rotorParam.workpieceSizes.clear();
|
||
XMLElement* sizesElement = rotorCoreParamElement->FirstChildElement("WorkpieceSizes");
|
||
if (sizesElement)
|
||
{
|
||
rotorParam.defaultWorkpieceIndex = sizesElement->IntAttribute("defaultIndex");
|
||
for (XMLElement* item = sizesElement->FirstChildElement("Workpiece"); item;
|
||
item = item->NextSiblingElement("Workpiece"))
|
||
{
|
||
VrRotorWorkpieceSize size;
|
||
size.index = item->IntAttribute("index");
|
||
size.radius = item->DoubleAttribute("radius");
|
||
size.height = item->DoubleAttribute("height");
|
||
rotorParam.workpieceSizes.push_back(size);
|
||
}
|
||
// 无效列表保留供界面修正，启动检测时拒绝使用，不能静默换成默认尺寸。
|
||
if (!rotorParam.isWorkpieceListValid())
|
||
{
|
||
LOG_WARNING("Invalid rotor WorkpieceSizes; detection requires a valid size list\n");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 旧配置迁移为 1 号工件，保留现场原有半径和高度。
|
||
VrRotorWorkpieceSize size;
|
||
size.radius = rotorParam.radius;
|
||
size.height = rotorParam.height;
|
||
rotorParam.defaultWorkpieceIndex = 1;
|
||
rotorParam.workpieceSizes.push_back(size);
|
||
}
|
||
}
|
||
|
||
// 解析多相机平面校准参数
|
||
XMLElement* planeCalibParamsElement = algoParamsElement->FirstChildElement("PlaneCalibParams");
|
||
if (planeCalibParamsElement)
|
||
{
|
||
XMLElement* cameraCalibParamElement = planeCalibParamsElement->FirstChildElement("CameraCalibParam");
|
||
while (cameraCalibParamElement)
|
||
{
|
||
VrCameraPlaneCalibParam cameraParam;
|
||
|
||
// 读取相机基础信息
|
||
if (cameraCalibParamElement->Attribute("cameraIndex"))
|
||
cameraParam.cameraIndex = cameraCalibParamElement->IntAttribute("cameraIndex");
|
||
if (cameraCalibParamElement->Attribute("cameraName"))
|
||
cameraParam.cameraName = cameraCalibParamElement->Attribute("cameraName");
|
||
if (cameraCalibParamElement->Attribute("isCalibrated"))
|
||
cameraParam.isCalibrated = cameraCalibParamElement->BoolAttribute("isCalibrated");
|
||
if (cameraCalibParamElement->Attribute("planeHeight"))
|
||
cameraParam.planeHeight = cameraCalibParamElement->DoubleAttribute("planeHeight");
|
||
|
||
// 读取旋转矩阵planeCalib[9] (3x3矩阵)
|
||
if (cameraCalibParamElement->Attribute("planeCalib_00"))
|
||
cameraParam.planeCalib[0] = cameraCalibParamElement->DoubleAttribute("planeCalib_00");
|
||
if (cameraCalibParamElement->Attribute("planeCalib_01"))
|
||
cameraParam.planeCalib[1] = cameraCalibParamElement->DoubleAttribute("planeCalib_01");
|
||
if (cameraCalibParamElement->Attribute("planeCalib_02"))
|
||
cameraParam.planeCalib[2] = cameraCalibParamElement->DoubleAttribute("planeCalib_02");
|
||
if (cameraCalibParamElement->Attribute("planeCalib_10"))
|
||
cameraParam.planeCalib[3] = cameraCalibParamElement->DoubleAttribute("planeCalib_10");
|
||
if (cameraCalibParamElement->Attribute("planeCalib_11"))
|
||
cameraParam.planeCalib[4] = cameraCalibParamElement->DoubleAttribute("planeCalib_11");
|
||
if (cameraCalibParamElement->Attribute("planeCalib_12"))
|
||
cameraParam.planeCalib[5] = cameraCalibParamElement->DoubleAttribute("planeCalib_12");
|
||
if (cameraCalibParamElement->Attribute("planeCalib_20"))
|
||
cameraParam.planeCalib[6] = cameraCalibParamElement->DoubleAttribute("planeCalib_20");
|
||
if (cameraCalibParamElement->Attribute("planeCalib_21"))
|
||
cameraParam.planeCalib[7] = cameraCalibParamElement->DoubleAttribute("planeCalib_21");
|
||
if (cameraCalibParamElement->Attribute("planeCalib_22"))
|
||
cameraParam.planeCalib[8] = cameraCalibParamElement->DoubleAttribute("planeCalib_22");
|
||
|
||
// 读取逆旋转矩阵invRMatrix[9] (3x3矩阵)
|
||
if (cameraCalibParamElement->Attribute("invRMatrix_00"))
|
||
cameraParam.invRMatrix[0] = cameraCalibParamElement->DoubleAttribute("invRMatrix_00");
|
||
if (cameraCalibParamElement->Attribute("invRMatrix_01"))
|
||
cameraParam.invRMatrix[1] = cameraCalibParamElement->DoubleAttribute("invRMatrix_01");
|
||
if (cameraCalibParamElement->Attribute("invRMatrix_02"))
|
||
cameraParam.invRMatrix[2] = cameraCalibParamElement->DoubleAttribute("invRMatrix_02");
|
||
if (cameraCalibParamElement->Attribute("invRMatrix_10"))
|
||
cameraParam.invRMatrix[3] = cameraCalibParamElement->DoubleAttribute("invRMatrix_10");
|
||
if (cameraCalibParamElement->Attribute("invRMatrix_11"))
|
||
cameraParam.invRMatrix[4] = cameraCalibParamElement->DoubleAttribute("invRMatrix_11");
|
||
if (cameraCalibParamElement->Attribute("invRMatrix_12"))
|
||
cameraParam.invRMatrix[5] = cameraCalibParamElement->DoubleAttribute("invRMatrix_12");
|
||
if (cameraCalibParamElement->Attribute("invRMatrix_20"))
|
||
cameraParam.invRMatrix[6] = cameraCalibParamElement->DoubleAttribute("invRMatrix_20");
|
||
if (cameraCalibParamElement->Attribute("invRMatrix_21"))
|
||
cameraParam.invRMatrix[7] = cameraCalibParamElement->DoubleAttribute("invRMatrix_21");
|
||
if (cameraCalibParamElement->Attribute("invRMatrix_22"))
|
||
cameraParam.invRMatrix[8] = cameraCalibParamElement->DoubleAttribute("invRMatrix_22");
|
||
|
||
// 添加到结果中
|
||
configResult.algorithmParams.planeCalibParam.cameraCalibParams.push_back(cameraParam);
|
||
|
||
// 移动到下一个相机配置
|
||
cameraCalibParamElement = cameraCalibParamElement->NextSiblingElement("CameraCalibParam");
|
||
}
|
||
}
|
||
}
|
||
|
||
// 解析调试参数（在AlgorithmParams外面）
|
||
XMLElement* debugParamElement = root->FirstChildElement("DebugParam");
|
||
if (debugParamElement)
|
||
{
|
||
if (debugParamElement->Attribute("enableDebug"))
|
||
configResult.debugParam.enableDebug = debugParamElement->BoolAttribute("enableDebug");
|
||
if (debugParamElement->Attribute("savePointCloud"))
|
||
configResult.debugParam.savePointCloud = debugParamElement->BoolAttribute("savePointCloud");
|
||
if (debugParamElement->Attribute("saveDebugImage"))
|
||
configResult.debugParam.saveDebugImage = debugParamElement->BoolAttribute("saveDebugImage");
|
||
if (debugParamElement->Attribute("printDetailLog"))
|
||
configResult.debugParam.printDetailLog = debugParamElement->BoolAttribute("printDetailLog");
|
||
if (debugParamElement->Attribute("debugOutputPath"))
|
||
configResult.debugParam.debugOutputPath = debugParamElement->Attribute("debugOutputPath");
|
||
configResult.debugParam.storageType = ParseDebugStorageType(
|
||
debugParamElement->Attribute("storageType"));
|
||
}
|
||
|
||
// 解析串口配置
|
||
XMLElement* serialConfigElement = root->FirstChildElement("SerialConfig");
|
||
if (serialConfigElement)
|
||
{
|
||
if (serialConfigElement->Attribute("portName"))
|
||
configResult.serialConfig.portName = serialConfigElement->Attribute("portName");
|
||
if (serialConfigElement->Attribute("baudRate"))
|
||
configResult.serialConfig.baudRate = serialConfigElement->IntAttribute("baudRate");
|
||
if (serialConfigElement->Attribute("dataBits"))
|
||
configResult.serialConfig.dataBits = serialConfigElement->IntAttribute("dataBits");
|
||
if (serialConfigElement->Attribute("stopBits"))
|
||
configResult.serialConfig.stopBits = serialConfigElement->IntAttribute("stopBits");
|
||
if (serialConfigElement->Attribute("parity"))
|
||
configResult.serialConfig.parity = serialConfigElement->IntAttribute("parity");
|
||
if (serialConfigElement->Attribute("flowControl"))
|
||
configResult.serialConfig.flowControl = serialConfigElement->IntAttribute("flowControl");
|
||
if (serialConfigElement->Attribute("enabled"))
|
||
configResult.serialConfig.enabled = serialConfigElement->BoolAttribute("enabled");
|
||
}
|
||
|
||
// 解析手眼标定矩阵
|
||
XMLElement* handEyeElement = root->FirstChildElement("HandEyeCalibMatrix");
|
||
if (handEyeElement)
|
||
{
|
||
for (int i = 0; i < 16; i++)
|
||
{
|
||
char attrName[32];
|
||
sprintf(attrName, "m%d", i);
|
||
if (handEyeElement->Attribute(attrName))
|
||
configResult.handEyeCalibMatrix.matrix[i] = handEyeElement->DoubleAttribute(attrName);
|
||
}
|
||
// 读取旋转顺序，如果不存在则使用默认值11（外旋ZYX）
|
||
if (handEyeElement->Attribute("eulerOrder"))
|
||
configResult.handEyeCalibMatrix.eulerOrder = handEyeElement->IntAttribute("eulerOrder");
|
||
else
|
||
configResult.handEyeCalibMatrix.eulerOrder = 11; // 默认外旋ZYX
|
||
}
|
||
|
||
// 解析 TCP/IP 文本服务端端口和 Modbus 寄存器配置
|
||
XMLElement* tcpServerConfigElement = root->FirstChildElement("TcpServerConfig");
|
||
if (tcpServerConfigElement)
|
||
{
|
||
if (tcpServerConfigElement->Attribute("robotServerPort"))
|
||
configResult.plcRobotServerConfig.robotServerPort = tcpServerConfigElement->IntAttribute("robotServerPort");
|
||
|
||
// 解析Modbus寄存器地址配置
|
||
if (tcpServerConfigElement->Attribute("addrTrigger"))
|
||
configResult.plcRobotServerConfig.registerConfig.addrTrigger = tcpServerConfigElement->IntAttribute("addrTrigger");
|
||
if (tcpServerConfigElement->Attribute("addrWorkStatus"))
|
||
configResult.plcRobotServerConfig.registerConfig.addrWorkStatus = tcpServerConfigElement->IntAttribute("addrWorkStatus");
|
||
if (tcpServerConfigElement->Attribute("addrCoordDataStart"))
|
||
configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart = tcpServerConfigElement->IntAttribute("addrCoordDataStart");
|
||
|
||
// 解析姿态输出顺序配置
|
||
if (tcpServerConfigElement->Attribute("poseOutputOrder"))
|
||
configResult.plcRobotServerConfig.poseOutputOrder = tcpServerConfigElement->IntAttribute("poseOutputOrder");
|
||
else
|
||
configResult.plcRobotServerConfig.poseOutputOrder = POSE_ORDER_RPY; // 默认RPY
|
||
|
||
// 解析方向向量反向配置
|
||
if (tcpServerConfigElement->Attribute("dirVectorInvert"))
|
||
configResult.plcRobotServerConfig.dirVectorInvert = tcpServerConfigElement->IntAttribute("dirVectorInvert");
|
||
else
|
||
configResult.plcRobotServerConfig.dirVectorInvert = DIR_INVERT_YZ; // 默认YZ反向（兼容原有行为）
|
||
|
||
// 解析数据字节序配置
|
||
if (tcpServerConfigElement->Attribute("byteOrder"))
|
||
configResult.plcRobotServerConfig.byteOrder = tcpServerConfigElement->IntAttribute("byteOrder");
|
||
else
|
||
configResult.plcRobotServerConfig.byteOrder = BYTE_ORDER_BIG_ENDIAN; // 默认大端序
|
||
}
|
||
|
||
return LOAD_CONFIG_SUCCESS;
|
||
}
|
||
|
||
bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResult)
|
||
{
|
||
// 创建XML文档
|
||
XMLDocument doc;
|
||
|
||
// 添加声明
|
||
XMLDeclaration* declaration = doc.NewDeclaration("xml version=\"1.0\" encoding=\"UTF-8\"");
|
||
doc.InsertFirstChild(declaration);
|
||
|
||
// 创建根元素
|
||
XMLElement* root = doc.NewElement("StatorWorkpiecePositionConfig");
|
||
doc.InsertEndChild(root);
|
||
|
||
// 添加摄像头列表
|
||
XMLElement* camerasElement = doc.NewElement("Cameras");
|
||
root->InsertEndChild(camerasElement);
|
||
|
||
for (const auto& camera : configResult.cameraList)
|
||
{
|
||
XMLElement* cameraElement = doc.NewElement("Camera");
|
||
cameraElement->SetAttribute("name", camera.name.c_str());
|
||
cameraElement->SetAttribute("ip", camera.ip.c_str());
|
||
camerasElement->InsertEndChild(cameraElement);
|
||
}
|
||
|
||
// 添加设备列表
|
||
XMLElement* devicesElement = doc.NewElement("Devices");
|
||
root->InsertEndChild(devicesElement);
|
||
|
||
for (const auto& device : configResult.deviceList)
|
||
{
|
||
XMLElement* deviceElement = doc.NewElement("Device");
|
||
deviceElement->SetAttribute("name", device.name.c_str());
|
||
deviceElement->SetAttribute("ip", device.ip.c_str());
|
||
devicesElement->InsertEndChild(deviceElement);
|
||
}
|
||
|
||
// 添加算法参数
|
||
XMLElement* algoParamsElement = doc.NewElement("AlgorithmParams");
|
||
root->InsertEndChild(algoParamsElement);
|
||
|
||
for (const VrPointCloudFilterParam& param : configResult.algorithmParams.pointCloudFilterParams)
|
||
{
|
||
XMLElement* filterElement = doc.NewElement("PointCloudFilterParam");
|
||
filterElement->SetAttribute("cameraIndex", param.cameraIndex);
|
||
filterElement->SetAttribute("cameraName", param.cameraName.c_str());
|
||
filterElement->SetAttribute("enabled", param.enabled);
|
||
filterElement->SetAttribute("minX", param.minX);
|
||
filterElement->SetAttribute("maxX", param.maxX);
|
||
filterElement->SetAttribute("minY", param.minY);
|
||
filterElement->SetAttribute("maxY", param.maxY);
|
||
filterElement->SetAttribute("minZ", param.minZ);
|
||
filterElement->SetAttribute("maxZ", param.maxZ);
|
||
algoParamsElement->InsertEndChild(filterElement);
|
||
}
|
||
|
||
// 添加工具姿态与目标点补偿参数
|
||
XMLElement* toolParamElement = doc.NewElement("ToolParam");
|
||
toolParamElement->SetAttribute("eulerOrder", configResult.algorithmParams.toolParam.eulerOrder);
|
||
toolParamElement->SetAttribute("rotX", configResult.algorithmParams.toolParam.rotX);
|
||
toolParamElement->SetAttribute("rotY", configResult.algorithmParams.toolParam.rotY);
|
||
toolParamElement->SetAttribute("rotZ", configResult.algorithmParams.toolParam.rotZ);
|
||
toolParamElement->SetAttribute("offsetX", configResult.algorithmParams.toolParam.offsetX);
|
||
toolParamElement->SetAttribute("offsetY", configResult.algorithmParams.toolParam.offsetY);
|
||
toolParamElement->SetAttribute("offsetZ", configResult.algorithmParams.toolParam.offsetZ);
|
||
algoParamsElement->InsertEndChild(toolParamElement);
|
||
|
||
const auto& modelParams = configResult.algorithmParams.modelParams;
|
||
const VrWorkpieceModelParam& legacyModelParam = configResult.algorithmParams.modelParam;
|
||
|
||
XMLElement* modelParamElement = doc.NewElement("WorkpieceClassMapping");
|
||
modelParamElement->SetAttribute("class0WorkpieceType",
|
||
legacyModelParam.class0WorkpieceType);
|
||
modelParamElement->SetAttribute("class1WorkpieceType",
|
||
legacyModelParam.class1WorkpieceType);
|
||
modelParamElement->SetAttribute("class2WorkpieceType",
|
||
legacyModelParam.class2WorkpieceType);
|
||
algoParamsElement->InsertEndChild(modelParamElement);
|
||
|
||
XMLElement* modelParamsElement = doc.NewElement("WorkpieceModelParams");
|
||
modelParamsElement->SetAttribute("targetGray", configResult.algorithmParams.modelGraySelection.targetGray);
|
||
modelParamsElement->SetAttribute("maxGrayDeviation", configResult.algorithmParams.modelGraySelection.maxGrayDeviation);
|
||
const VrModelDetectionRegion& region = configResult.algorithmParams.modelDetectionRegion;
|
||
XMLElement* regionElement = doc.NewElement("DetectionRegion");
|
||
regionElement->SetAttribute("enabled", region.enabled);
|
||
regionElement->SetAttribute("left", region.left);
|
||
regionElement->SetAttribute("top", region.top);
|
||
regionElement->SetAttribute("right", region.right);
|
||
regionElement->SetAttribute("bottom", region.bottom);
|
||
modelParamsElement->InsertEndChild(regionElement);
|
||
std::vector<VrWorkpieceModelParam> modelParamsToWrite = modelParams;
|
||
if (modelParamsToWrite.empty())
|
||
{
|
||
modelParamsToWrite.push_back(VrWorkpieceModelParam());
|
||
}
|
||
// 其他共用配置页面仍编辑兼容字段，列表首项同步该值；转子页面会从列表更新兼容字段。
|
||
modelParamsToWrite.front().exposure = legacyModelParam.exposure;
|
||
modelParamsToWrite.front().gain = legacyModelParam.gain;
|
||
modelParamsToWrite.front().minimumScore = legacyModelParam.minimumScore;
|
||
int modelIndex = 1;
|
||
for (const auto& model : modelParamsToWrite)
|
||
{
|
||
XMLElement* item = doc.NewElement("Model");
|
||
item->SetAttribute("index", modelIndex++);
|
||
item->SetAttribute("exposure", model.exposure);
|
||
item->SetAttribute("gain", model.gain);
|
||
item->SetAttribute("minimumScore", model.minimumScore);
|
||
modelParamsElement->InsertEndChild(item);
|
||
}
|
||
algoParamsElement->InsertEndChild(modelParamsElement);
|
||
|
||
XMLElement* binParamElement = doc.NewElement("BinParam");
|
||
const VrBinParam& binParam = configResult.algorithmParams.binParam;
|
||
binParamElement->SetAttribute("useConfiguredBin", binParam.useConfiguredBin);
|
||
binParamElement->SetAttribute("binHeight", binParam.binHeight);
|
||
binParamElement->SetAttribute("bin_zSliceRange", binParam.bin_zSliceRange);
|
||
binParamElement->SetAttribute("length", binParam.length);
|
||
binParamElement->SetAttribute("width", binParam.width);
|
||
binParamElement->SetAttribute("binTopZ", binParam.binTopZ);
|
||
auto writeVec3 = [binParamElement](const char* prefix, const double values[3]) {
|
||
char attrName[64];
|
||
for (int i = 0; i < 3; ++i) {
|
||
sprintf(attrName, "%s%d", prefix, i);
|
||
binParamElement->SetAttribute(attrName, values[i]);
|
||
}
|
||
};
|
||
writeVec3("center", binParam.center);
|
||
writeVec3("bottomNormal", binParam.bottomNormal);
|
||
writeVec3("xDir", binParam.xDir);
|
||
writeVec3("yDir", binParam.yDir);
|
||
for (int vertex = 0; vertex < 4; ++vertex) {
|
||
char attrName[64];
|
||
for (int axis = 0; axis < 3; ++axis) {
|
||
sprintf(attrName, "minRectVertex%d_%d", vertex, axis);
|
||
binParamElement->SetAttribute(attrName, binParam.minRectVertex[vertex][axis]);
|
||
}
|
||
}
|
||
algoParamsElement->InsertEndChild(binParamElement);
|
||
|
||
XMLElement* rotorCoreParamElement = doc.NewElement("RotorCoreParam");
|
||
rotorCoreParamElement->SetAttribute("radius", configResult.algorithmParams.rotorCoreParam.radius);
|
||
rotorCoreParamElement->SetAttribute("height", configResult.algorithmParams.rotorCoreParam.height);
|
||
rotorCoreParamElement->SetAttribute("spacerLength", configResult.algorithmParams.rotorCoreParam.spacerLength);
|
||
rotorCoreParamElement->SetAttribute("spacerWidth", configResult.algorithmParams.rotorCoreParam.spacerWidth);
|
||
rotorCoreParamElement->SetAttribute("layerRatio", configResult.algorithmParams.rotorCoreParam.layerRatio);
|
||
rotorCoreParamElement->SetAttribute("distToSideRatio", configResult.algorithmParams.rotorCoreParam.distToSideRatio);
|
||
rotorCoreParamElement->SetAttribute("groundRemoveOffset", configResult.algorithmParams.rotorCoreParam.groundRemoveOffset);
|
||
rotorCoreParamElement->SetAttribute("useGroundCalibNormal", configResult.algorithmParams.rotorCoreParam.useGroundCalibNormal);
|
||
XMLElement* sizesElement = doc.NewElement("WorkpieceSizes");
|
||
sizesElement->SetAttribute("defaultIndex", configResult.algorithmParams.rotorCoreParam.defaultWorkpieceIndex);
|
||
for (const auto& size : configResult.algorithmParams.rotorCoreParam.workpieceSizes)
|
||
{
|
||
XMLElement* item = doc.NewElement("Workpiece");
|
||
item->SetAttribute("index", size.index);
|
||
item->SetAttribute("radius", size.radius);
|
||
item->SetAttribute("height", size.height);
|
||
sizesElement->InsertEndChild(item);
|
||
}
|
||
rotorCoreParamElement->InsertEndChild(sizesElement);
|
||
algoParamsElement->InsertEndChild(rotorCoreParamElement);
|
||
|
||
// 添加多相机平面校准参数
|
||
XMLElement* planeCalibParamsElement = doc.NewElement("PlaneCalibParams");
|
||
|
||
for (const auto& cameraParam : configResult.algorithmParams.planeCalibParam.cameraCalibParams)
|
||
{
|
||
XMLElement* cameraCalibParamElement = doc.NewElement("CameraCalibParam");
|
||
cameraCalibParamElement->SetAttribute("cameraIndex", cameraParam.cameraIndex);
|
||
cameraCalibParamElement->SetAttribute("cameraName", cameraParam.cameraName.c_str());
|
||
cameraCalibParamElement->SetAttribute("isCalibrated", cameraParam.isCalibrated);
|
||
cameraCalibParamElement->SetAttribute("planeHeight", cameraParam.planeHeight);
|
||
|
||
// 保存旋转矩阵planeCalib[9] (3x3矩阵)
|
||
cameraCalibParamElement->SetAttribute("planeCalib_00", cameraParam.planeCalib[0]);
|
||
cameraCalibParamElement->SetAttribute("planeCalib_01", cameraParam.planeCalib[1]);
|
||
cameraCalibParamElement->SetAttribute("planeCalib_02", cameraParam.planeCalib[2]);
|
||
cameraCalibParamElement->SetAttribute("planeCalib_10", cameraParam.planeCalib[3]);
|
||
cameraCalibParamElement->SetAttribute("planeCalib_11", cameraParam.planeCalib[4]);
|
||
cameraCalibParamElement->SetAttribute("planeCalib_12", cameraParam.planeCalib[5]);
|
||
cameraCalibParamElement->SetAttribute("planeCalib_20", cameraParam.planeCalib[6]);
|
||
cameraCalibParamElement->SetAttribute("planeCalib_21", cameraParam.planeCalib[7]);
|
||
cameraCalibParamElement->SetAttribute("planeCalib_22", cameraParam.planeCalib[8]);
|
||
|
||
// 保存逆旋转矩阵invRMatrix[9] (3x3矩阵)
|
||
cameraCalibParamElement->SetAttribute("invRMatrix_00", cameraParam.invRMatrix[0]);
|
||
cameraCalibParamElement->SetAttribute("invRMatrix_01", cameraParam.invRMatrix[1]);
|
||
cameraCalibParamElement->SetAttribute("invRMatrix_02", cameraParam.invRMatrix[2]);
|
||
cameraCalibParamElement->SetAttribute("invRMatrix_10", cameraParam.invRMatrix[3]);
|
||
cameraCalibParamElement->SetAttribute("invRMatrix_11", cameraParam.invRMatrix[4]);
|
||
cameraCalibParamElement->SetAttribute("invRMatrix_12", cameraParam.invRMatrix[5]);
|
||
cameraCalibParamElement->SetAttribute("invRMatrix_20", cameraParam.invRMatrix[6]);
|
||
cameraCalibParamElement->SetAttribute("invRMatrix_21", cameraParam.invRMatrix[7]);
|
||
cameraCalibParamElement->SetAttribute("invRMatrix_22", cameraParam.invRMatrix[8]);
|
||
|
||
planeCalibParamsElement->InsertEndChild(cameraCalibParamElement);
|
||
}
|
||
|
||
algoParamsElement->InsertEndChild(planeCalibParamsElement);
|
||
|
||
// 添加调试参数（在AlgorithmParams外面）
|
||
XMLElement* debugParamElement = doc.NewElement("DebugParam");
|
||
debugParamElement->SetAttribute("enableDebug", configResult.debugParam.enableDebug);
|
||
debugParamElement->SetAttribute("savePointCloud", configResult.debugParam.savePointCloud);
|
||
debugParamElement->SetAttribute("saveDebugImage", configResult.debugParam.saveDebugImage);
|
||
debugParamElement->SetAttribute("printDetailLog", configResult.debugParam.printDetailLog);
|
||
debugParamElement->SetAttribute("debugOutputPath", configResult.debugParam.debugOutputPath.c_str());
|
||
debugParamElement->SetAttribute("storageType", DebugStorageTypeName(configResult.debugParam.storageType));
|
||
root->InsertEndChild(debugParamElement);
|
||
|
||
// 添加串口配置
|
||
XMLElement* serialConfigElement = doc.NewElement("SerialConfig");
|
||
serialConfigElement->SetAttribute("portName", configResult.serialConfig.portName.c_str());
|
||
serialConfigElement->SetAttribute("baudRate", configResult.serialConfig.baudRate);
|
||
serialConfigElement->SetAttribute("dataBits", configResult.serialConfig.dataBits);
|
||
serialConfigElement->SetAttribute("stopBits", configResult.serialConfig.stopBits);
|
||
serialConfigElement->SetAttribute("parity", configResult.serialConfig.parity);
|
||
serialConfigElement->SetAttribute("flowControl", configResult.serialConfig.flowControl);
|
||
serialConfigElement->SetAttribute("enabled", configResult.serialConfig.enabled);
|
||
root->InsertEndChild(serialConfigElement);
|
||
|
||
// 添加手眼标定矩阵
|
||
XMLElement* handEyeElement = doc.NewElement("HandEyeCalibMatrix");
|
||
for (int i = 0; i < 16; i++)
|
||
{
|
||
char attrName[32];
|
||
sprintf(attrName, "m%d", i);
|
||
handEyeElement->SetAttribute(attrName, configResult.handEyeCalibMatrix.matrix[i]);
|
||
}
|
||
// 保存旋转顺序
|
||
handEyeElement->SetAttribute("eulerOrder", configResult.handEyeCalibMatrix.eulerOrder);
|
||
root->InsertEndChild(handEyeElement);
|
||
|
||
// 添加 TCP/IP 文本服务端端口和 Modbus 寄存器配置
|
||
XMLElement* tcpServerConfigElement = doc.NewElement("TcpServerConfig");
|
||
tcpServerConfigElement->SetAttribute("robotServerPort", configResult.plcRobotServerConfig.robotServerPort);
|
||
// 保存Modbus寄存器地址配置
|
||
tcpServerConfigElement->SetAttribute("addrTrigger", configResult.plcRobotServerConfig.registerConfig.addrTrigger);
|
||
tcpServerConfigElement->SetAttribute("addrWorkStatus", configResult.plcRobotServerConfig.registerConfig.addrWorkStatus);
|
||
tcpServerConfigElement->SetAttribute("addrCoordDataStart", configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart);
|
||
// 保存姿态输出顺序配置
|
||
tcpServerConfigElement->SetAttribute("poseOutputOrder", configResult.plcRobotServerConfig.poseOutputOrder);
|
||
// 保存方向向量反向配置
|
||
tcpServerConfigElement->SetAttribute("dirVectorInvert", configResult.plcRobotServerConfig.dirVectorInvert);
|
||
// 保存数据字节序配置
|
||
tcpServerConfigElement->SetAttribute("byteOrder", configResult.plcRobotServerConfig.byteOrder);
|
||
root->InsertEndChild(tcpServerConfigElement);
|
||
|
||
// 保存到文件
|
||
XMLError err = doc.SaveFile(filePath.c_str());
|
||
if (err != XML_SUCCESS)
|
||
{
|
||
std::cerr << "无法保存配置文件: " << filePath << std::endl;
|
||
return false;
|
||
}
|
||
|
||
// 触发配置改变通知
|
||
if (m_pNotify)
|
||
{
|
||
m_pNotify->OnConfigChanged(configResult);
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
// 设置配置改变通知回调
|
||
void CVrConfig::SetConfigChangeNotify(IVrConfigChangeNotify* notify)
|
||
{
|
||
m_pNotify = notify;
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 创建实例
|
||
* @return 实例
|
||
*/
|
||
bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
|
||
{
|
||
*ppVrConfig = new CVrConfig();
|
||
return *ppVrConfig != nullptr;
|
||
}
|