#include "ObstaclePlannerPanel.h" #include "CameraManager.h" #include "CoordTransformPanel.h" #include "OpcUaComm.h" #include "YoloDetector.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace { QString pointText(const SafetyPoint3D& point) { return QStringLiteral("(%1, %2, %3) mm") .arg(point.x, 0, 'f', 1) .arg(point.y, 0, 'f', 1) .arg(point.z, 0, 'f', 1); } QJsonObject pointJson(const SafetyPoint3D& point) { return {{QStringLiteral("x_mm"), point.x}, {QStringLiteral("y_mm"), point.y}, {QStringLiteral("z_mm"), point.z}}; } QJsonObject boxJson(const SafetyAabb& box) { return {{QStringLiteral("minimum"), pointJson(box.minimum)}, {QStringLiteral("maximum"), pointJson(box.maximum)}}; } } ObstaclePlannerPanel::ObstaclePlannerPanel(CameraManager* cameraManager, YoloDetector* yoloDetector, CoordTransformPanel* coordPanel, OpcUaCommManager* opcua, QWidget* parent) : QWidget(parent), m_cameraManager(cameraManager), m_yoloDetector(yoloDetector), m_coordPanel(coordPanel), m_opcua(opcua) { buildUi(); onGenerateDemo(); } QDoubleSpinBox* ObstaclePlannerPanel::makeSpin(double minimum, double maximum, double value, int decimals) { auto* spin = new QDoubleSpinBox(this); spin->setRange(minimum, maximum); spin->setDecimals(decimals); spin->setSingleStep(decimals == 0 ? 1.0 : 10.0); spin->setValue(value); spin->setSuffix(QStringLiteral(" mm")); return spin; } void ObstaclePlannerPanel::buildUi() { auto* root = new QVBoxLayout(this); auto* title = new QLabel(QString::fromUtf8( "静态障碍物检测与安全路径建议(视觉侧功能验证)"), this); title->setStyleSheet(QStringLiteral("font-size:18px;font-weight:600;color:#17365d;")); root->addWidget(title); auto* boundary = new QLabel(QString::fromUtf8( "功能边界:输出障碍预警、笛卡尔安全途经点和结果文件;机械臂控制器负责关节空间碰撞检查、轨迹插补与执行。"), this); boundary->setWordWrap(true); boundary->setStyleSheet(QStringLiteral("background:#eef5ff;padding:7px;color:#24415f;")); root->addWidget(boundary); m_resultTabs = new QTabWidget(this); root->addWidget(m_resultTabs, 1); auto* controlHost = new QWidget(this); auto* controls = new QVBoxLayout(controlHost); auto* sourceGroup = new QGroupBox(QString::fromUtf8("1. 图像与检测方式"), controlHost); auto* sourceLayout = new QGridLayout(sourceGroup); m_comboDetectionMode = new QComboBox(sourceGroup); m_comboDetectionMode->addItem(QString::fromUtf8("红色立方体(推荐演示)")); m_comboDetectionMode->addItem(QString::fromUtf8("YOLO 类别0(可选)")); m_comboDetectionMode->addItem(QString::fromUtf8("手动包围盒")); m_comboDemoScenario = new QComboBox(sourceGroup); m_comboDemoScenario->addItem(QString::fromUtf8("路径阻挡(推荐演示)")); m_comboDemoScenario->addItem(QString::fromUtf8("无障碍物")); m_comboDemoScenario->addItem(QString::fromUtf8("障碍物不阻挡路径")); m_comboDemoScenario->addItem(QString::fromUtf8("目标点位于安全区")); m_checkObstaclePresent = new QCheckBox(QString::fromUtf8("手动障碍物存在"), sourceGroup); m_checkObstaclePresent->setChecked(true); auto* generate = new QPushButton(QString::fromUtf8("生成演示场景"), sourceGroup); auto* load = new QPushButton(QString::fromUtf8("加载图片"), sourceGroup); auto* capture = new QPushButton(QString::fromUtf8("相机拍照"), sourceGroup); sourceLayout->addWidget(new QLabel(QString::fromUtf8("检测方式")), 0, 0); sourceLayout->addWidget(m_comboDetectionMode, 0, 1, 1, 2); sourceLayout->addWidget(new QLabel(QString::fromUtf8("演示场景")), 1, 0); sourceLayout->addWidget(m_comboDemoScenario, 1, 1, 1, 2); sourceLayout->addWidget(m_checkObstaclePresent, 2, 0, 1, 3); sourceLayout->addWidget(generate, 3, 0); sourceLayout->addWidget(load, 3, 1); sourceLayout->addWidget(capture, 3, 2); controls->addWidget(sourceGroup); auto* pointGroup = new QGroupBox(QString::fromUtf8("2. 起点与目标点(基坐标系)"), controlHost); auto* pointGrid = new QGridLayout(pointGroup); m_startX = makeSpin(-5000, 5000, 100); m_startY = makeSpin(-5000, 5000, 0); m_startZ = makeSpin(-5000, 5000, 50); m_goalX = makeSpin(-5000, 5000, 700); m_goalY = makeSpin(-5000, 5000, 0); m_goalZ = makeSpin(-5000, 5000, 50); pointGrid->addWidget(new QLabel(QStringLiteral("")), 0, 0); pointGrid->addWidget(new QLabel(QStringLiteral("X")), 0, 1); pointGrid->addWidget(new QLabel(QStringLiteral("Y")), 0, 2); pointGrid->addWidget(new QLabel(QStringLiteral("Z")), 0, 3); pointGrid->addWidget(new QLabel(QString::fromUtf8("起点")), 1, 0); pointGrid->addWidget(m_startX, 1, 1); pointGrid->addWidget(m_startY, 1, 2); pointGrid->addWidget(m_startZ, 1, 3); pointGrid->addWidget(new QLabel(QString::fromUtf8("目标")), 2, 0); pointGrid->addWidget(m_goalX, 2, 1); pointGrid->addWidget(m_goalY, 2, 2); pointGrid->addWidget(m_goalZ, 2, 3); auto* readRobot = new QPushButton(QString::fromUtf8("从 OPC UA 读取机械臂起点"), pointGroup); auto* readTarget = new QPushButton(QString::fromUtf8("读取视觉转换目标点"), pointGroup); pointGrid->addWidget(readRobot, 3, 0, 1, 2); pointGrid->addWidget(readTarget, 3, 2, 1, 2); controls->addWidget(pointGroup); auto* obstacleGroup = new QGroupBox(QString::fromUtf8("3. 障碍物与安全参数"), controlHost); auto* obstacleForm = new QFormLayout(obstacleGroup); auto* centerRow = new QWidget(obstacleGroup); auto* centerLayout = new QHBoxLayout(centerRow); centerLayout->setContentsMargins(0, 0, 0, 0); m_obstacleX = makeSpin(-5000, 5000, 400); m_obstacleY = makeSpin(-5000, 5000, 0); m_tableZ = makeSpin(-5000, 5000, 0); centerLayout->addWidget(m_obstacleX); centerLayout->addWidget(m_obstacleY); centerLayout->addWidget(m_tableZ); obstacleForm->addRow(QString::fromUtf8("中心X / 中心Y / 底面Z"), centerRow); auto* sizeRow = new QWidget(obstacleGroup); auto* sizeLayout = new QHBoxLayout(sizeRow); sizeLayout->setContentsMargins(0, 0, 0, 0); m_sizeX = makeSpin(1, 3000, 150); m_sizeY = makeSpin(1, 3000, 100); m_sizeZ = makeSpin(1, 3000, 120); sizeLayout->addWidget(m_sizeX); sizeLayout->addWidget(m_sizeY); sizeLayout->addWidget(m_sizeZ); obstacleForm->addRow(QString::fromUtf8("尺寸X / Y / Z"), sizeRow); m_clearance = makeSpin(0, 1000, 50); m_overhead = makeSpin(0, 1000, 80); m_maxZ = makeSpin(1, 5000, 800); obstacleForm->addRow(QString::fromUtf8("安全膨胀距离"), m_clearance); obstacleForm->addRow(QString::fromUtf8("越障附加高度"), m_overhead); obstacleForm->addRow(QString::fromUtf8("工作区最大Z"), m_maxZ); controls->addWidget(obstacleGroup); auto* mappingGroup = new QGroupBox(QString::fromUtf8("4. 图像对应工作区"), controlHost); auto* mappingGrid = new QGridLayout(mappingGroup); m_workspaceXMin = makeSpin(-5000, 5000, 0); m_workspaceXMax = makeSpin(-5000, 5000, 800); m_workspaceYMin = makeSpin(-5000, 5000, -300); m_workspaceYMax = makeSpin(-5000, 5000, 300); mappingGrid->addWidget(new QLabel(QStringLiteral("X min")), 0, 0); mappingGrid->addWidget(m_workspaceXMin, 0, 1); mappingGrid->addWidget(new QLabel(QStringLiteral("X max")), 0, 2); mappingGrid->addWidget(m_workspaceXMax, 0, 3); mappingGrid->addWidget(new QLabel(QStringLiteral("Y min")), 1, 0); mappingGrid->addWidget(m_workspaceYMin, 1, 1); mappingGrid->addWidget(new QLabel(QStringLiteral("Y max")), 1, 2); mappingGrid->addWidget(m_workspaceYMax, 1, 3); controls->addWidget(mappingGroup); m_buttonPlan = new QPushButton(QString::fromUtf8("检测障碍物并规划路径"), controlHost); m_buttonPlan->setMinimumHeight(42); m_buttonPlan->setStyleSheet(QStringLiteral( "QPushButton{background:#1677ff;color:white;font-weight:600;border-radius:4px;}" "QPushButton:hover{background:#4096ff;}")); controls->addWidget(m_buttonPlan); auto* actionRow = new QHBoxLayout; auto* selfTest = new QPushButton(QString::fromUtf8("算法自检"), controlHost); m_buttonExport = new QPushButton(QString::fromUtf8("导出验证结果"), controlHost); m_buttonExport->setEnabled(false); actionRow->addWidget(selfTest); actionRow->addWidget(m_buttonExport); controls->addLayout(actionRow); controls->addStretch(); auto* controlScroll = new QScrollArea(this); controlScroll->setWidgetResizable(true); controlScroll->setWidget(controlHost); m_resultTabs->addTab(controlScroll, QString::fromUtf8("参数设置")); auto* resultHost = new QWidget(this); auto* resultLayout = new QVBoxLayout(resultHost); m_labelStatus = new QLabel(QString::fromUtf8("等待规划"), resultHost); m_labelStatus->setWordWrap(true); m_labelStatus->setMinimumHeight(54); resultLayout->addWidget(m_labelStatus); m_labelSource = new QLabel(resultHost); m_labelSource->setAlignment(Qt::AlignCenter); m_labelSource->setMinimumSize(470, 260); m_labelSource->setStyleSheet(QStringLiteral("background:#20242a;color:#ddd;border:1px solid #777;")); resultLayout->addWidget(m_labelSource); m_labelPlan = new QLabel(resultHost); m_labelPlan->setAlignment(Qt::AlignCenter); m_labelPlan->setMinimumSize(470, 280); m_labelPlan->setStyleSheet(QStringLiteral("background:white;border:1px solid #aaa;")); resultLayout->addWidget(m_labelPlan); m_tableWaypoints = new QTableWidget(resultHost); m_tableWaypoints->setColumnCount(4); m_tableWaypoints->setHorizontalHeaderLabels( {QString::fromUtf8("序号"), QStringLiteral("X / mm"), QStringLiteral("Y / mm"), QStringLiteral("Z / mm")}); m_tableWaypoints->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); m_tableWaypoints->setMaximumHeight(180); resultLayout->addWidget(m_tableWaypoints); auto* resultScroll = new QScrollArea(this); resultScroll->setWidgetResizable(true); resultScroll->setWidget(resultHost); m_resultTabs->addTab(resultScroll, QString::fromUtf8("规划结果")); connect(generate, &QPushButton::clicked, this, &ObstaclePlannerPanel::onGenerateDemo); connect(load, &QPushButton::clicked, this, &ObstaclePlannerPanel::onLoadImage); connect(capture, &QPushButton::clicked, this, &ObstaclePlannerPanel::onCaptureImage); connect(m_buttonPlan, &QPushButton::clicked, this, &ObstaclePlannerPanel::onDetectAndPlan); connect(readRobot, &QPushButton::clicked, this, &ObstaclePlannerPanel::onReadRobotPose); connect(readTarget, &QPushButton::clicked, this, &ObstaclePlannerPanel::onReadVisualTarget); connect(selfTest, &QPushButton::clicked, this, &ObstaclePlannerPanel::onSelfTest); connect(m_buttonExport, &QPushButton::clicked, this, &ObstaclePlannerPanel::onExport); } SafetyPoint3D ObstaclePlannerPanel::startPoint() const { return {m_startX->value(), m_startY->value(), m_startZ->value()}; } SafetyPoint3D ObstaclePlannerPanel::goalPoint() const { return {m_goalX->value(), m_goalY->value(), m_goalZ->value()}; } void ObstaclePlannerPanel::setStartPoint(const SafetyPoint3D& point) { m_startX->setValue(point.x); m_startY->setValue(point.y); m_startZ->setValue(point.z); } void ObstaclePlannerPanel::setGoalPoint(const SafetyPoint3D& point) { m_goalX->setValue(point.x); m_goalY->setValue(point.y); m_goalZ->setValue(point.z); } SafetyAabb ObstaclePlannerPanel::manualObstacle() const { const double halfX = m_sizeX->value() * 0.5; const double halfY = m_sizeY->value() * 0.5; return {{m_obstacleX->value() - halfX, m_obstacleY->value() - halfY, m_tableZ->value()}, {m_obstacleX->value() + halfX, m_obstacleY->value() + halfY, m_tableZ->value() + m_sizeZ->value()}}; } void ObstaclePlannerPanel::onGenerateDemo() { const int scenario = m_comboDemoScenario->currentIndex(); setStartPoint({100.0, 0.0, 50.0}); setGoalPoint({700.0, 0.0, 50.0}); m_obstacleX->setValue(400.0); m_obstacleY->setValue(0.0); m_checkObstaclePresent->setChecked(scenario != 1); if (scenario == 2) m_obstacleY->setValue(180.0); if (scenario == 3) setGoalPoint({400.0, 0.0, 60.0}); m_comboDetectionMode->setCurrentIndex(0); m_sourceImage = ObstaclePlanner::createDemoScene( m_checkObstaclePresent->isChecked(), m_obstacleX->value(), m_obstacleY->value(), m_workspaceXMin->value(), m_workspaceXMax->value(), m_workspaceYMin->value(), m_workspaceYMax->value(), m_sizeX->value(), m_sizeY->value()); showSourceImage(m_sourceImage); emit logMessage(QString::fromUtf8("[避障规划] 已生成演示场景")); onDetectAndPlan(); } void ObstaclePlannerPanel::onLoadImage() { const QString path = QFileDialog::getOpenFileName( this, QString::fromUtf8("加载障碍物图像"), QString(), QString::fromUtf8("图像 (*.png *.jpg *.jpeg *.bmp *.tif *.tiff);;全部文件 (*)")); if (path.isEmpty()) return; QImage image(path); if (image.isNull()) { m_labelStatus->setText(QString::fromUtf8("图像加载失败:") + path); return; } m_sourceImage = image; showSourceImage(m_sourceImage); m_hasResult = false; m_buttonExport->setEnabled(false); emit logMessage(QString::fromUtf8("[避障规划] 已加载图像: ") + path); } void ObstaclePlannerPanel::onCaptureImage() { if (!m_cameraManager) return; CameraManager::CamIndex selected = CameraManager::LEFT; if (!m_cameraManager->isConnected(selected)) selected = CameraManager::RIGHT; if (!m_cameraManager->isConnected(selected)) { m_labelStatus->setText(QString::fromUtf8("相机未连接,请先连接左相机或右相机")); return; } const QImage image = m_cameraManager->captureSingleFrame(selected); if (image.isNull()) { m_labelStatus->setText(QString::fromUtf8("相机拍照失败")); return; } m_sourceImage = image; showSourceImage(m_sourceImage); emit logMessage(QString::fromUtf8("[避障规划] 相机拍照完成")); } void ObstaclePlannerPanel::onReadRobotPose() { if (!m_opcua || !m_opcua->client() || !m_opcua->client()->isConnected()) { m_labelStatus->setText(QString::fromUtf8("OPC UA 客户端未连接,无法读取机械臂起点")); return; } PoseData pose; if (!m_opcua->client()->readRobotPose(pose)) { m_labelStatus->setText(QString::fromUtf8("读取机械臂位姿失败")); return; } setStartPoint({pose.X, pose.Y, pose.Z}); emit logMessage(QString::fromUtf8("[避障规划] 已读取机械臂起点 ") + pointText(startPoint())); } void ObstaclePlannerPanel::onReadVisualTarget() { if (!m_coordPanel || !m_coordPanel->robotPoseReady()) { m_labelStatus->setText(QString::fromUtf8("尚无有效的视觉目标基坐标,请先完成坐标转换")); return; } const VisionPoseResult pose = m_coordPanel->robotPose(); setGoalPoint({pose.tx * 1000.0, pose.ty * 1000.0, pose.tz * 1000.0}); emit logMessage(QString::fromUtf8("[避障规划] 已读取视觉转换目标点 ") + pointText(goalPoint())); } void ObstaclePlannerPanel::onDetectAndPlan() { const int mode = m_comboDetectionMode->currentIndex(); QRect pixelBox; bool obstacleDetected = false; SafetyAabb obstacle; QString description; if (mode == 0) { if (m_sourceImage.isNull()) { m_labelStatus->setText(QString::fromUtf8("请先生成、加载或拍摄图像")); return; } obstacleDetected = ObstaclePlanner::detectRedObstacle(m_sourceImage, pixelBox); description = obstacleDetected ? QString::fromUtf8("HSV红色障碍物") : QString::fromUtf8("HSV未检出障碍物"); } else if (mode == 1) { if (m_sourceImage.isNull()) { m_labelStatus->setText(QString::fromUtf8("请先加载或拍摄图像")); return; } if (!m_yoloDetector || !m_yoloDetector->isLoaded()) { m_labelStatus->setText(QString::fromUtf8("YOLO 模型未加载;可切换到红色立方体演示模式")); return; } const QVector detections = m_yoloDetector->detect(m_sourceImage, 0.25f, 0.45f, 0); if (!detections.isEmpty()) { const auto best = std::max_element(detections.begin(), detections.end(), [](const YoloResult& lhs, const YoloResult& rhs) { return lhs.conf < rhs.conf; }); pixelBox = best->box.toAlignedRect(); obstacleDetected = pixelBox.isValid(); description = QString::fromUtf8("YOLO类别0: %1, conf=%2") .arg(best->label).arg(best->conf, 0, 'f', 3); } else { description = QString::fromUtf8("YOLO类别0未检出"); } } else { obstacleDetected = m_checkObstaclePresent->isChecked(); description = obstacleDetected ? QString::fromUtf8("手动三维包围盒") : QString::fromUtf8("手动设定无障碍物"); } if (obstacleDetected) { if (mode == 2) { obstacle = manualObstacle(); } else { obstacle = ObstaclePlanner::pixelBoxToKnownCuboid( pixelBox, m_sourceImage.size(), m_workspaceXMin->value(), m_workspaceXMax->value(), m_workspaceYMin->value(), m_workspaceYMax->value(), m_sizeX->value(), m_sizeY->value(), m_sizeZ->value(), m_tableZ->value()); m_obstacleX->setValue((obstacle.minimum.x + obstacle.maximum.x) * 0.5); m_obstacleY->setValue((obstacle.minimum.y + obstacle.maximum.y) * 0.5); } } const SafetyPathResult result = ObstaclePlanner::planLiftOverPath( startPoint(), goalPoint(), obstacleDetected ? &obstacle : nullptr, m_clearance->value(), m_overhead->value(), m_maxZ->value()); showResult(result, pixelBox, description); } void ObstaclePlannerPanel::showSourceImage(const QImage& image) { if (image.isNull()) { m_labelSource->setText(QString::fromUtf8("无图像")); return; } m_labelSource->setPixmap(QPixmap::fromImage(image).scaled( m_labelSource->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation)); } QImage ObstaclePlannerPanel::makeAnnotatedImage(const QRect& detectedPixelBox, const QString& detectorDescription) const { QImage output = m_sourceImage; if (output.isNull()) { output = QImage(960, 600, QImage::Format_RGB32); output.fill(Qt::white); } QPainter painter(&output); painter.setRenderHint(QPainter::Antialiasing); if (detectedPixelBox.isValid()) { painter.setPen(QPen(QColor(255, 210, 0), 5)); painter.drawRect(detectedPixelBox); QRect textBox = painter.fontMetrics().boundingRect(detectorDescription).adjusted(-8, -5, 8, 5); textBox.moveTopLeft(detectedPixelBox.topLeft() + QPoint(0, -textBox.height())); if (textBox.top() < 0) textBox.moveTop(detectedPixelBox.top()); painter.fillRect(textBox, QColor(0, 0, 0, 180)); painter.setPen(Qt::white); painter.drawText(textBox, Qt::AlignCenter, detectorDescription); } else { painter.setPen(QPen(QColor(30, 90, 150), 2)); painter.drawText(QRect(10, 10, output.width() - 20, 40), Qt::AlignLeft | Qt::AlignVCenter, detectorDescription); } return output; } QImage ObstaclePlannerPanel::makePlanImage(const SafetyPathResult& result) const { QImage image(1200, 560, QImage::Format_RGB32); image.fill(Qt::white); QPainter painter(&image); painter.setRenderHint(QPainter::Antialiasing); const QRectF topArea(70, 55, 480, 420); const QRectF sideArea(660, 55, 480, 420); double xMin = m_workspaceXMin->value(); double xMax = m_workspaceXMax->value(); double yMin = m_workspaceYMin->value(); double yMax = m_workspaceYMax->value(); if (xMax <= xMin) xMax = xMin + 1.0; if (yMax <= yMin) yMax = yMin + 1.0; const double zMin = std::min(0.0, std::min(startPoint().z, goalPoint().z)); const double zMax = std::max(m_maxZ->value(), zMin + 1.0); auto topPoint = [&](const SafetyPoint3D& point) { return QPointF(topArea.left() + (point.x - xMin) / (xMax - xMin) * topArea.width(), topArea.bottom() - (point.y - yMin) / (yMax - yMin) * topArea.height()); }; auto sidePoint = [&](const SafetyPoint3D& point) { return QPointF(sideArea.left() + (point.x - xMin) / (xMax - xMin) * sideArea.width(), sideArea.bottom() - (point.z - zMin) / (zMax - zMin) * sideArea.height()); }; auto topBox = [&](const SafetyAabb& box) { const QPointF first = topPoint({box.minimum.x, box.maximum.y, 0}); const QPointF second = topPoint({box.maximum.x, box.minimum.y, 0}); return QRectF(first, second).normalized(); }; auto sideBox = [&](const SafetyAabb& box) { const QPointF first = sidePoint({box.minimum.x, 0, box.maximum.z}); const QPointF second = sidePoint({box.maximum.x, 0, box.minimum.z}); return QRectF(first, second).normalized(); }; painter.setPen(QPen(QColor(90, 100, 110), 1)); painter.drawRect(topArea); painter.drawRect(sideArea); painter.setPen(QColor(25, 55, 90)); painter.drawText(QRectF(70, 12, 480, 35), Qt::AlignCenter, QString::fromUtf8("俯视图 X-Y(障碍位置与路径)")); painter.drawText(QRectF(660, 12, 480, 35), Qt::AlignCenter, QString::fromUtf8("高度剖面 X-Z(抬高越障路径)")); if (result.obstacleDetected) { painter.fillRect(topBox(result.obstacle), QColor(220, 55, 55, 150)); painter.fillRect(sideBox(result.obstacle), QColor(220, 55, 55, 150)); painter.setBrush(Qt::NoBrush); painter.setPen(QPen(QColor(230, 140, 20), 2, Qt::DashLine)); painter.drawRect(topBox(result.safeBox)); painter.drawRect(sideBox(result.safeBox)); } if (result.pathBlocked) { painter.setPen(QPen(QColor(210, 40, 40), 2, Qt::DashLine)); painter.drawLine(topPoint(startPoint()), topPoint(goalPoint())); painter.drawLine(sidePoint(startPoint()), sidePoint(goalPoint())); } if (!result.waypoints.isEmpty()) { painter.setPen(QPen(QColor(0, 150, 80), 4)); for (int index = 0; index + 1 < result.waypoints.size(); ++index) { painter.drawLine(topPoint(result.waypoints[index]), topPoint(result.waypoints[index + 1])); painter.drawLine(sidePoint(result.waypoints[index]), sidePoint(result.waypoints[index + 1])); } painter.setBrush(QColor(0, 150, 80)); for (const SafetyPoint3D& waypoint : result.waypoints) { painter.drawEllipse(topPoint(waypoint), 5, 5); painter.drawEllipse(sidePoint(waypoint), 5, 5); } } painter.setPen(QColor(60, 60, 60)); painter.drawText(QRectF(topArea.left(), topArea.bottom() + 7, topArea.width(), 25), Qt::AlignCenter, QString::fromUtf8("X / mm")); painter.drawText(QRectF(sideArea.left(), sideArea.bottom() + 7, sideArea.width(), 25), Qt::AlignCenter, QString::fromUtf8("X / mm")); painter.save(); painter.translate(18, topArea.center().y()); painter.rotate(-90); painter.drawText(QRectF(-100, -15, 200, 25), Qt::AlignCenter, QString::fromUtf8("Y / mm")); painter.restore(); painter.save(); painter.translate(610, sideArea.center().y()); painter.rotate(-90); painter.drawText(QRectF(-100, -15, 200, 25), Qt::AlignCenter, QString::fromUtf8("Z / mm")); painter.restore(); painter.setPen(QColor(90, 90, 90)); painter.drawText(QRectF(70, 510, 1070, 35), Qt::AlignCenter, QString::fromUtf8("红色:障碍物 橙色虚线:安全包围盒 红色虚线:受阻直线 绿色:输出路径")); return image; } void ObstaclePlannerPanel::fillWaypointTable(const SafetyPathResult& result) { m_tableWaypoints->setRowCount(result.waypoints.size()); for (int row = 0; row < result.waypoints.size(); ++row) { const SafetyPoint3D& point = result.waypoints[row]; m_tableWaypoints->setItem(row, 0, new QTableWidgetItem(QString::number(row))); m_tableWaypoints->setItem(row, 1, new QTableWidgetItem(QString::number(point.x, 'f', 3))); m_tableWaypoints->setItem(row, 2, new QTableWidgetItem(QString::number(point.y, 'f', 3))); m_tableWaypoints->setItem(row, 3, new QTableWidgetItem(QString::number(point.z, 'f', 3))); } } void ObstaclePlannerPanel::showResult(const SafetyPathResult& result, const QRect& detectedPixelBox, const QString& detectorDescription) { m_lastResult = result; m_lastPixelBox = detectedPixelBox; m_lastDetectorDescription = detectorDescription; m_annotatedImage = makeAnnotatedImage(detectedPixelBox, detectorDescription); m_planImage = makePlanImage(result); m_hasResult = true; m_buttonExport->setEnabled(true); m_resultTabs->setCurrentIndex(1); showSourceImage(m_annotatedImage); m_labelPlan->setPixmap(QPixmap::fromImage(m_planImage).scaled( m_labelPlan->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation)); fillWaypointTable(result); const QString status = QString::fromUtf8("%1 | %2 | 检测:%3") .arg(result.statusCode, result.message, detectorDescription); m_labelStatus->setText(status); if (!result.valid) { m_labelStatus->setStyleSheet(QStringLiteral( "background:#fff1f0;color:#a8071a;padding:9px;font-weight:600;border:1px solid #ffa39e;")); } else if (result.avoidanceRequired) { m_labelStatus->setStyleSheet(QStringLiteral( "background:#fff7e6;color:#ad4e00;padding:9px;font-weight:600;border:1px solid #ffd591;")); } else { m_labelStatus->setStyleSheet(QStringLiteral( "background:#f6ffed;color:#237804;padding:9px;font-weight:600;border:1px solid #b7eb8f;")); } emit logMessage(QString::fromUtf8("[避障规划] %1,途经点=%2") .arg(status).arg(result.waypoints.size())); } void ObstaclePlannerPanel::onSelfTest() { QString report; const bool passed = ObstaclePlanner::selfTest(&report); m_labelStatus->setText((passed ? QString::fromUtf8("算法自检通过:") : QString::fromUtf8("算法自检失败:")) + report); m_labelStatus->setStyleSheet(passed ? QStringLiteral("background:#f6ffed;color:#237804;padding:9px;font-weight:600;") : QStringLiteral("background:#fff1f0;color:#a8071a;padding:9px;font-weight:600;")); emit logMessage(QString::fromUtf8("[避障规划] ") + report); } bool ObstaclePlannerPanel::exportResultFiles(const QString& directory, QString* error) const { if (!m_hasResult) { if (error) *error = QString::fromUtf8("没有可导出的规划结果"); return false; } if (!QDir().mkpath(directory)) { if (error) *error = QString::fromUtf8("无法创建导出目录"); return false; } const QDir dir(directory); if (!m_sourceImage.isNull()) m_sourceImage.save(dir.filePath(QStringLiteral("01_source.png"))); if (!m_annotatedImage.isNull()) m_annotatedImage.save(dir.filePath(QStringLiteral("02_detection.png"))); if (!m_planImage.isNull()) m_planImage.save(dir.filePath(QStringLiteral("03_path_plan.png"))); QJsonObject root; root.insert(QStringLiteral("generated_at"), QDateTime::currentDateTime().toString(Qt::ISODate)); root.insert(QStringLiteral("feature"), QString::fromUtf8("静态障碍物检测与安全路径建议")); root.insert(QStringLiteral("detector"), m_lastDetectorDescription); root.insert(QStringLiteral("status_code"), m_lastResult.statusCode); root.insert(QStringLiteral("message"), m_lastResult.message); root.insert(QStringLiteral("valid"), m_lastResult.valid); root.insert(QStringLiteral("obstacle_detected"), m_lastResult.obstacleDetected); root.insert(QStringLiteral("path_blocked"), m_lastResult.pathBlocked); root.insert(QStringLiteral("avoidance_required"), m_lastResult.avoidanceRequired); root.insert(QStringLiteral("start"), pointJson(startPoint())); root.insert(QStringLiteral("goal"), pointJson(goalPoint())); root.insert(QStringLiteral("clearance_mm"), m_clearance->value()); root.insert(QStringLiteral("overhead_mm"), m_overhead->value()); root.insert(QStringLiteral("max_workspace_z_mm"), m_maxZ->value()); if (m_lastResult.obstacleDetected) { root.insert(QStringLiteral("obstacle_aabb"), boxJson(m_lastResult.obstacle)); root.insert(QStringLiteral("inflated_safe_aabb"), boxJson(m_lastResult.safeBox)); } QJsonArray waypoints; for (const SafetyPoint3D& point : m_lastResult.waypoints) waypoints.append(pointJson(point)); root.insert(QStringLiteral("waypoints"), waypoints); QFile jsonFile(dir.filePath(QStringLiteral("obstacle_path_result.json"))); if (!jsonFile.open(QIODevice::WriteOnly)) { if (error) *error = jsonFile.errorString(); return false; } jsonFile.write(QJsonDocument(root).toJson(QJsonDocument::Indented)); jsonFile.close(); QFile csvFile(dir.filePath(QStringLiteral("waypoints.csv"))); if (!csvFile.open(QIODevice::WriteOnly | QIODevice::Text)) { if (error) *error = csvFile.errorString(); return false; } QTextStream stream(&csvFile); stream.setCodec("UTF-8"); stream << "index,x_mm,y_mm,z_mm\n"; for (int index = 0; index < m_lastResult.waypoints.size(); ++index) { const SafetyPoint3D& point = m_lastResult.waypoints[index]; stream << index << ',' << QString::number(point.x, 'f', 6) << ',' << QString::number(point.y, 'f', 6) << ',' << QString::number(point.z, 'f', 6) << '\n'; } return true; } void ObstaclePlannerPanel::onExport() { const QString base = QDir::currentPath() + QStringLiteral("/ObstaclePlanningResults/") + QDateTime::currentDateTime().toString(QStringLiteral("yyyyMMdd_HHmmss")); const QString directory = QFileDialog::getExistingDirectory( this, QString::fromUtf8("选择结果导出目录"), base); if (directory.isEmpty()) return; QString error; if (!exportResultFiles(directory, &error)) { m_labelStatus->setText(QString::fromUtf8("结果导出失败:") + error); return; } m_labelStatus->setText(QString::fromUtf8("结果已导出:") + QDir::toNativeSeparators(directory)); emit logMessage(QString::fromUtf8("[避障规划] 结果已导出: ") + QDir::toNativeSeparators(directory)); }