714 lines
31 KiB
C++
714 lines
31 KiB
C++
#include "ObstaclePlannerPanel.h"
|
|
|
|
#include "CameraManager.h"
|
|
#include "CoordTransformPanel.h"
|
|
#include "OpcUaComm.h"
|
|
#include "YoloDetector.h"
|
|
|
|
#include <QCheckBox>
|
|
#include <QComboBox>
|
|
#include <QDateTime>
|
|
#include <QDir>
|
|
#include <QDoubleSpinBox>
|
|
#include <QFile>
|
|
#include <QFileDialog>
|
|
#include <QFormLayout>
|
|
#include <QGridLayout>
|
|
#include <QGroupBox>
|
|
#include <QHeaderView>
|
|
#include <QHBoxLayout>
|
|
#include <QJsonArray>
|
|
#include <QJsonDocument>
|
|
#include <QJsonObject>
|
|
#include <QLabel>
|
|
#include <QPainter>
|
|
#include <QPen>
|
|
#include <QPushButton>
|
|
#include <QScrollArea>
|
|
#include <QTableWidget>
|
|
#include <QTableWidgetItem>
|
|
#include <QTabWidget>
|
|
#include <QTextStream>
|
|
#include <QVBoxLayout>
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
|
|
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<YoloResult> 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));
|
|
}
|