From dc4d3b75c7dd0af77deac642f570c07b0f7e265a Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Wed, 23 Sep 2026 16:04:01 +0800 Subject: [PATCH] Improve arm control UI and fault reporting --- README.md | 12 +- src/mainwindow.cpp | 223 +++++++++++++++++++++++--- src/mainwindow.h | 13 +- src/mainwindow_ui.cpp | 327 ++++++++++++++++++++++++++++++++------- src/robotclient.cpp | 39 ++++- src/robotclient.h | 2 + src/urdfviewerwidget.cpp | 249 +++++++++++++++++++++++++++++ src/urdfviewerwidget.h | 11 ++ styles.qss | 161 +++++++++++++++++++ 9 files changed, 946 insertions(+), 91 deletions(-) diff --git a/README.md b/README.md index 8796eeb..8f1db04 100644 --- a/README.md +++ b/README.md @@ -61,18 +61,20 @@ CMVR Control Station 是一个基于 Qt 5 的桌面控制站,用于连接 CMVR “模型与控制”页将 URDF 模型、速度点动和只读遥测放在同一工作区: -- “关节速度”按实时自由度生成每个关节的当前位置与 `−/+` 按钮。按住按钮时 - 调用 `speedJ`,速度为左侧最大速度乘以速度倍率;松开按钮立即调用 - `stopMotion`。 +- “关节速度”按实时自由度生成每个关节的当前位置与 `−/+` 按钮。每条位置条 + 使用当前精确 URDF 的关节上下限映射实时位置;按住按钮时调用 `speedJ`,速度 + 为左侧 SpeedJ 最大速度乘以速度倍率,松开按钮立即调用 `stopMotion`。 - “笛卡尔速度”显示当前 X/Y/Z、RX/RY/RZ,并使用相同的按住点动交互调用 - `speedL`。基座坐标和工具坐标名称通过 `ListBaseFrame`、`ListTCPFrame` 自动 + `speedL`。SpeedL 使用独立的笛卡尔速度(m/s)和加速度(m/s²)限制,不再 + 复用 SpeedJ 的关节单位参数。基座坐标和工具坐标名称通过 `ListBaseFrame`、`ListTCPFrame` 自动 获取;选择变化后,遥测通过 `getPose(base_link, ee_link)` 显示对应坐标对的 末端位姿。 - 当前 cmvr-es 的 `speedL` 协议只接受 Base/Tool 类型,不接受命名坐标系。 因此点动命令按 Base 语义发送,坐标名称用于选定实时位姿的参考基座和 TCP; 后续若需要在任意命名坐标系轴向点动,需要扩展 `SpeedL.Request` 及边缘端驱动。 - 下方“实时状态”只读显示关节位置、速度、由连续速度采样计算的加速度,以及 - 当前坐标对下的末端位置姿态,不再提供 MoveJ/MoveL 目标输入和示教按钮。 + 当前坐标对下的末端位置姿态;关节与末端两个状态区域等宽显示,不再提供 + MoveJ/MoveL 目标输入和示教按钮。 ## 技术栈和目录 diff --git a/src/mainwindow.cpp b/src/mainwindow.cpp index 71afda8..18ccab8 100644 --- a/src/mainwindow.cpp +++ b/src/mainwindow.cpp @@ -502,6 +502,8 @@ MainWindow::MainWindow(QWidget *parent) connect(m_client, &RobotClient::connectionChanged, this, &MainWindow::updateConnection); connect(m_client, &RobotClient::armInfoReceived, this, &MainWindow::updateArmInfo); connect(m_client, &RobotClient::telemetryReceived, this, &MainWindow::updateTelemetry); + connect(m_client, &RobotClient::armTelemetryHealthChanged, + this, &MainWindow::updateArmTelemetryHealth); connect(m_client, &RobotClient::systemStatusReceived, this, &MainWindow::updateSystemStatus); connect(m_client, &RobotClient::systemInfoReceived, this, &MainWindow::updateSystemInfo); connect(m_client, &RobotClient::imageReceived, this, &MainWindow::updateImage); @@ -905,6 +907,7 @@ void MainWindow::updateConnection(bool connected, int latencyMs, const QString & m_armSideCombo->setEnabled(true); } if (!connected) { + m_armAlertStates.clear(); const QList armStates = {m_servoStatusValue, m_faultStatusValue}; for (QLabel *label : armStates) { if (!label) @@ -955,6 +958,7 @@ void MainWindow::rebuildArmJogControls() delete item; } m_jointJogPositionLabels.clear(); + m_jointJogPositionSliders.clear(); m_jointJogMinusButtons.clear(); m_jointJogPlusButtons.clear(); @@ -982,8 +986,9 @@ void MainWindow::rebuildArmJogControls() m_activeArmJointNames.at(jointIndex))); QSlider *direction = new QSlider(Qt::Horizontal, row); direction->setObjectName(QStringLiteral("jogDirectionIndicator")); - direction->setRange(-100, 100); + direction->setRange(0, 1000); direction->setValue(0); + direction->setToolTip(QStringLiteral("等待 URDF 关节限位和实时位置")); direction->setEnabled(false); layout->addWidget(name); layout->addWidget(position); @@ -998,6 +1003,7 @@ void MainWindow::rebuildArmJogControls() connect(plus, &QPushButton::released, this, &MainWindow::stopArmJog); m_jointJogRowsLayout->addWidget(row); m_jointJogPositionLabels.append(position); + m_jointJogPositionSliders.append(direction); m_jointJogMinusButtons.append(minus); m_jointJogPlusButtons.append(plus); } @@ -1032,13 +1038,17 @@ void MainWindow::startCartesianJog(int axisIndex, int direction) for (const QString &key : keys) velocity.insert(key, 0.0); const double ratio = m_jogSpeedSlider->value() / 100.0; + const double maximumVelocity = m_cartesianVelocitySpin + ? m_cartesianVelocitySpin->value() : 0.25; velocity.insert(keys.at(axisIndex), - direction * m_velocitySpin->value() * ratio); + direction * maximumVelocity * ratio); m_armJogActive = true; // The current edge protocol applies named base/TCP selections to pose // queries, while speedL accepts only the Base/Tool enum. Use Base here. m_client->requestSpeedL(m_armIdEdit->text().trimmed(), velocity, - m_accelerationSpin->value(), 60.0, false); + m_cartesianAccelerationSpin + ? m_cartesianAccelerationSpin->value() : 0.5, + 60.0, false); } void MainWindow::stopArmJog() @@ -1101,10 +1111,13 @@ void MainWindow::updateTelemetry(const QString &deviceId, const QVariantList &jo const qint64 nowMs = QDateTime::currentMSecsSinceEpoch(); const double deltaSeconds = m_lastArmTelemetryMs > 0 ? (nowMs - m_lastArmTelemetryMs) / 1000.0 : 0.0; + const QStringList modelJointNames = m_urdfViewer + ? m_urdfViewer->jointNames() : QStringList(); + const QHash> modelJointLimits = m_urdfViewer + ? m_urdfViewer->jointLimits() : QHash>(); if (m_urdfViewer) { QHash modelPositions = jointPositions; - const QStringList modelJointNames = m_urdfViewer->jointNames(); int directMatches = 0; for (const QString &name : modelJointNames) { if (jointPositions.contains(name)) @@ -1142,6 +1155,33 @@ void MainWindow::updateTelemetry(const QString &deviceId, const QVariantList &jo if (row < m_jointJogPositionLabels.size()) m_jointJogPositionLabels.at(row)->setText( QStringLiteral("%1 rad").arg(position, 0, 'f', 4)); + if (row < m_jointJogPositionSliders.size()) { + QSlider *positionSlider = m_jointJogPositionSliders.at(row); + QString limitJointName = nameItem->text(); + if (!modelJointLimits.contains(limitJointName) + && row < modelJointNames.size()) { + limitJointName = modelJointNames.at(row); + } + const auto limitIt = modelJointLimits.constFind(limitJointName); + if (limitIt != modelJointLimits.constEnd() + && qIsFinite(limitIt.value().first) + && qIsFinite(limitIt.value().second) + && limitIt.value().second > limitIt.value().first) { + const double lower = limitIt.value().first; + const double upper = limitIt.value().second; + const double progress = qBound(0.0, (position - lower) / (upper - lower), 1.0); + positionSlider->setValue(qRound(progress * positionSlider->maximum())); + positionSlider->setToolTip( + QStringLiteral("%1:下限 %2 rad · 当前 %3 rad · 上限 %4 rad") + .arg(nameItem->text()) + .arg(lower, 0, 'f', 4) + .arg(position, 0, 'f', 4) + .arg(upper, 0, 'f', 4)); + } else { + positionSlider->setValue(0); + positionSlider->setToolTip(QStringLiteral("当前 URDF 未提供有效关节限位")); + } + } if (row == 0 && m_trend) m_trend->appendSample(position, velocity); } @@ -1149,17 +1189,15 @@ void MainWindow::updateTelemetry(const QString &deviceId, const QVariantList &jo if (!pose.isEmpty()) { m_latestArmPose = pose; - m_poseValue->setText(QStringLiteral("X %1 m\nY %2 m\nZ %3 m\n\nRX %4 rad\nRY %5 rad\nRZ %6 rad") - .arg(pose.value(QStringLiteral("x")).toDouble(), 0, 'f', 4) - .arg(pose.value(QStringLiteral("y")).toDouble(), 0, 'f', 4) - .arg(pose.value(QStringLiteral("z")).toDouble(), 0, 'f', 4) - .arg(pose.value(QStringLiteral("rx")).toDouble(), 0, 'f', 4) - .arg(pose.value(QStringLiteral("ry")).toDouble(), 0, 'f', 4) - .arg(pose.value(QStringLiteral("rz")).toDouble(), 0, 'f', 4)); static const QStringList keys = { QStringLiteral("x"), QStringLiteral("y"), QStringLiteral("z"), QStringLiteral("rx"), QStringLiteral("ry"), QStringLiteral("rz") }; + for (int index = 0; index < keys.size() + && index < m_poseMetricValues.size(); ++index) { + m_poseMetricValues.at(index)->setText( + QString::number(pose.value(keys.at(index)).toDouble(), 'f', 4)); + } for (int index = 0; index < keys.size() && index < m_cartesianJogPoseLabels.size(); ++index) { m_cartesianJogPoseLabels.at(index)->setText( @@ -1169,22 +1207,75 @@ void MainWindow::updateTelemetry(const QString &deviceId, const QVariantList &jo } } if (!state.isEmpty()) { + const bool connected = state.value(QStringLiteral("connected")).toBool(); const bool powered = state.value(QStringLiteral("poweredOn")).toBool(); const bool moving = state.value(QStringLiteral("moving")).toBool(); const bool fault = state.value(QStringLiteral("fault")).toBool(); const bool emergency = state.value(QStringLiteral("emergencyStopped")).toBool(); const bool protective = state.value(QStringLiteral("protectiveStopped")).toBool(); - if (m_servoStatusValue) - m_servoStatusValue->setText(powered - ? (moving ? QStringLiteral("已使能 · 运动中") : QStringLiteral("已使能 · 空闲")) - : QStringLiteral("未使能")); - if (m_faultStatusValue) - m_faultStatusValue->setText(emergency ? QStringLiteral("急停") - : protective ? QStringLiteral("保护停止") - : fault ? QStringLiteral("故障") : QStringLiteral("正常")); + const bool freedrive = state.value(QStringLiteral("freedrive")).toBool() + || state.value(QStringLiteral("controlMode")).toInt() == 6; + const int robotMode = state.value(QStringLiteral("robotMode")).toInt(); + const int safetyMode = state.value(QStringLiteral("safetyMode")).toInt(); + const bool isDisconnected = !connected || robotMode == 1; + const bool isEmergency = emergency || safetyMode == 4 || safetyMode == 6; + const bool isFault = fault || robotMode == 7 || safetyMode == 7; + const bool isProtective = protective || safetyMode == 3 || safetyMode == 5; + + const auto setStateLabel = [](QLabel *label, const QString &text, + const QString &visualState) { + if (!label) + return; + label->setText(text); + label->setProperty("state", visualState); + label->style()->unpolish(label); + label->style()->polish(label); + }; + setStateLabel( + m_servoStatusValue, + isDisconnected ? QStringLiteral("已断开") + : freedrive ? QStringLiteral("拖动示教") + : powered ? (moving ? QStringLiteral("已使能 · 运动中") + : QStringLiteral("已使能 · 空闲")) + : QStringLiteral("未使能"), + (isDisconnected || isEmergency || isFault || isProtective) + ? QStringLiteral("error") : QStringLiteral("good")); + setStateLabel( + m_faultStatusValue, + isDisconnected ? QStringLiteral("通信断开") + : isEmergency ? QStringLiteral("急停") + : isFault ? QStringLiteral("故障") + : isProtective ? QStringLiteral("保护停止") + : freedrive ? QStringLiteral("拖动示教") : QStringLiteral("正常"), + (isDisconnected || isEmergency || isFault || isProtective || freedrive) + ? QStringLiteral("error") : QStringLiteral("good")); + + if (isDisconnected) { + updateArmAlert(deviceId, QStringLiteral("disconnected"), + QStringLiteral("ERROR"), QStringLiteral("机械臂已断开"), + QStringLiteral("请检查机械臂电源、网线以及控制器地址。")); + } else if (isEmergency) { + updateArmAlert(deviceId, QStringLiteral("emergency-stop"), + QStringLiteral("ERROR"), QStringLiteral("机械臂处于急停状态"), + QStringLiteral("请确认现场安全后释放急停并清除故障。")); + } else if (isFault) { + updateArmAlert(deviceId, QStringLiteral("fault"), + QStringLiteral("ERROR"), QStringLiteral("机械臂发生故障"), + QStringLiteral("请检查控制器告警信息,排除原因后清除故障。")); + } else if (isProtective) { + updateArmAlert(deviceId, QStringLiteral("protective-stop"), + QStringLiteral("WARN"), QStringLiteral("机械臂处于保护停止状态"), + QStringLiteral("请检查碰撞、限位或安全输入。")); + } else if (freedrive) { + updateArmAlert(deviceId, QStringLiteral("freedrive"), + QStringLiteral("WARN"), QStringLiteral("机械臂处于拖动示教状态"), + QStringLiteral("上位机运动控制暂不可用,请退出拖动示教后再操作。")); + } else { + updateArmAlert(deviceId, QString(), QString(), QString()); + } } - m_updatedValue->setText(QStringLiteral("更新于 %1").arg(QDateTime::currentDateTime().toString("HH:mm:ss.zzz"))); - if (m_bottomLogLevel && m_bottomLogMessage) { + if (m_armAlertStates.value(deviceId).isEmpty() + && m_bottomLogLevel && m_bottomLogMessage) { m_bottomLogLevel->setText(QStringLiteral("[OK]")); m_bottomLogLevel->setProperty("level", QStringLiteral("ok")); m_bottomLogLevel->style()->unpolish(m_bottomLogLevel); @@ -1194,6 +1285,67 @@ void MainWindow::updateTelemetry(const QString &deviceId, const QVariantList &jo } } +void MainWindow::updateArmTelemetryHealth(const QString &deviceId, bool available, + const QString &detail) +{ + if (!m_armIdEdit || deviceId != m_armIdEdit->text().trimmed()) + return; + if (available) { + if (m_armAlertStates.value(deviceId) == QStringLiteral("communication")) + updateArmAlert(deviceId, QString(), QString(), QString()); + return; + } + + const auto markError = [](QLabel *label, const QString &text) { + if (!label) + return; + label->setText(text); + label->setProperty("state", QStringLiteral("error")); + label->style()->unpolish(label); + label->style()->polish(label); + }; + markError(m_servoStatusValue, QStringLiteral("遥测中断")); + markError(m_faultStatusValue, QStringLiteral("通信异常")); + updateArmAlert(deviceId, QStringLiteral("communication"), + QStringLiteral("ERROR"), QStringLiteral("机械臂遥测通信异常"), detail); +} + +void MainWindow::updateArmAlert(const QString &deviceId, const QString &alertKey, + const QString &level, const QString &summary, + const QString &detail) +{ + const QString previous = m_armAlertStates.value(deviceId); + if (previous == alertKey) + return; + if (alertKey.isEmpty()) { + m_armAlertStates.remove(deviceId); + if (!previous.isEmpty()) + appendLog(QStringLiteral("OK"), + QStringLiteral("机械臂 %1 状态已恢复正常").arg(deviceId)); + return; + } + + m_armAlertStates.insert(deviceId, alertKey); + const QString message = detail.isEmpty() + ? QStringLiteral("机械臂 %1:%2").arg(deviceId, summary) + : QStringLiteral("机械臂 %1:%2。%3").arg(deviceId, summary, detail); + appendLog(level, message); + + const QString popupKey = deviceId + QLatin1Char('/') + alertKey; + const qint64 nowMs = QDateTime::currentMSecsSinceEpoch(); + const qint64 lastPopupMs = m_armLastAlertPopupMs.value(popupKey, 0); + if (lastPopupMs > 0 && nowMs - lastPopupMs < 30000) + return; + m_armLastAlertPopupMs.insert(popupKey, nowMs); + + QMessageBox *popup = new QMessageBox( + level == QStringLiteral("ERROR") ? QMessageBox::Critical : QMessageBox::Warning, + QStringLiteral("机械臂状态提醒"), message, QMessageBox::Ok, this); + popup->setAttribute(Qt::WA_DeleteOnClose); + popup->setWindowModality(Qt::NonModal); + popup->open(); +} + void MainWindow::updateSystemStatus(double cpu, double memory, double disk, const QVariantList &devices) { @@ -1593,8 +1745,18 @@ void MainWindow::loadSettings() m_cameraStreamModeCombo->setCurrentIndex(modeIndex); } m_agvIdEdit->setText(settings.value(QStringLiteral("agv/device"), QStringLiteral("agv")).toString()); - m_velocitySpin->setValue(settings.value(QStringLiteral("motion/velocity"), 1.0).toDouble()); - m_accelerationSpin->setValue(settings.value(QStringLiteral("motion/acceleration"), 0.5).toDouble()); + m_velocitySpin->setValue(settings.value( + QStringLiteral("motion/speedJVelocity"), + settings.value(QStringLiteral("motion/velocity"), 1.0)).toDouble()); + m_accelerationSpin->setValue(settings.value( + QStringLiteral("motion/speedJAcceleration"), + settings.value(QStringLiteral("motion/acceleration"), 0.5)).toDouble()); + if (m_cartesianVelocitySpin) + m_cartesianVelocitySpin->setValue(settings.value( + QStringLiteral("motion/speedLVelocity"), 0.25).toDouble()); + if (m_cartesianAccelerationSpin) + m_cartesianAccelerationSpin->setValue(settings.value( + QStringLiteral("motion/speedLAcceleration"), 0.5).toDouble()); const QDir applicationDirectory(QCoreApplication::applicationDirPath()); const QString defaultUrdf = applicationDirectory.filePath(QStringLiteral("models/demo_arm.urdf")); QString configuredUrdf = settings.value(QStringLiteral("arm/urdf"), defaultUrdf) @@ -1648,8 +1810,15 @@ void MainWindow::saveSettings() settings.setValue(QStringLiteral("camera/streamMode"), m_cameraStreamModeCombo->currentData().toString()); settings.setValue(QStringLiteral("agv/device"), m_agvIdEdit->text().trimmed()); - settings.setValue(QStringLiteral("motion/velocity"), m_velocitySpin->value()); - settings.setValue(QStringLiteral("motion/acceleration"), m_accelerationSpin->value()); + settings.setValue(QStringLiteral("motion/speedJVelocity"), m_velocitySpin->value()); + settings.setValue(QStringLiteral("motion/speedJAcceleration"), + m_accelerationSpin->value()); + if (m_cartesianVelocitySpin) + settings.setValue(QStringLiteral("motion/speedLVelocity"), + m_cartesianVelocitySpin->value()); + if (m_cartesianAccelerationSpin) + settings.setValue(QStringLiteral("motion/speedLAcceleration"), + m_cartesianAccelerationSpin->value()); if (m_urdfViewer && !m_urdfViewer->currentFile().isEmpty()) settings.setValue(QStringLiteral("arm/urdf"), m_urdfViewer->currentFile()); else if (!m_pendingUrdfPath.isEmpty()) @@ -1685,6 +1854,10 @@ void MainWindow::setControlsEnabled(bool enabled) m_velocitySpin->setEnabled(enabled); if (m_accelerationSpin) m_accelerationSpin->setEnabled(enabled); + if (m_cartesianVelocitySpin) + m_cartesianVelocitySpin->setEnabled(enabled); + if (m_cartesianAccelerationSpin) + m_cartesianAccelerationSpin->setEnabled(enabled); if (m_calibrateZeroButton) m_calibrateZeroButton->setEnabled(enabled && m_zeroJointCombo && m_zeroJointCombo->count() > 0); diff --git a/src/mainwindow.h b/src/mainwindow.h index 341bb8e..cb9d409 100644 --- a/src/mainwindow.h +++ b/src/mainwindow.h @@ -60,6 +60,8 @@ private slots: void updateArmInfo(const QString &deviceId, const QVariantMap &info); void updateTelemetry(const QString &deviceId, const QVariantList &joints, const QVariantMap &pose, const QVariantMap &state); + void updateArmTelemetryHealth(const QString &deviceId, bool available, + const QString &detail); void updateSystemStatus(double cpu, double memory, double disk, const QVariantList &devices); void updateSystemInfo(const QVariantMap &info); @@ -102,6 +104,9 @@ private: void startJointJog(int jointIndex, int direction); void startCartesianJog(int axisIndex, int direction); void stopArmJog(); + void updateArmAlert(const QString &deviceId, const QString &alertKey, + const QString &level, const QString &summary, + const QString &detail = QString()); QString endpoint() const; RobotClient *m_client = nullptr; @@ -158,8 +163,7 @@ private: QLabel *m_armDeviceSummary = nullptr; QLabel *m_servoStatusValue = nullptr; QLabel *m_faultStatusValue = nullptr; - QLabel *m_poseValue = nullptr; - QLabel *m_updatedValue = nullptr; + QList m_poseMetricValues; TrendWidget *m_trend = nullptr; QTableWidget *m_jointTable = nullptr; QTableWidget *m_motionTable = nullptr; @@ -176,6 +180,7 @@ private: QWidget *m_jointJogRowsHost = nullptr; QVBoxLayout *m_jointJogRowsLayout = nullptr; QList m_jointJogPositionLabels; + QList m_jointJogPositionSliders; QList m_jointJogMinusButtons; QList m_jointJogPlusButtons; QList m_cartesianJogPoseLabels; @@ -186,9 +191,13 @@ private: QPushButton *m_jogStopButton = nullptr; QHash m_previousJointVelocities; qint64 m_lastArmTelemetryMs = 0; + QHash m_armAlertStates; + QHash m_armLastAlertPopupMs; bool m_armJogActive = false; QDoubleSpinBox *m_velocitySpin = nullptr; QDoubleSpinBox *m_accelerationSpin = nullptr; + QDoubleSpinBox *m_cartesianVelocitySpin = nullptr; + QDoubleSpinBox *m_cartesianAccelerationSpin = nullptr; QPushButton *m_sendMoveButton = nullptr; QPushButton *m_torqueOnButton = nullptr; QPushButton *m_torqueOffButton = nullptr; diff --git a/src/mainwindow_ui.cpp b/src/mainwindow_ui.cpp index ab572c4..3222c2a 100644 --- a/src/mainwindow_ui.cpp +++ b/src/mainwindow_ui.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -45,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -269,6 +271,35 @@ QWidget *MainWindow::createArmPage() m_faultStatusValue = ui.faultStatusValue; m_velocitySpin = ui.velocitySpin; m_accelerationSpin = ui.accelerationSpin; + ui.velocityLabel->setText(QStringLiteral("速度")); + ui.accelerationLabel->setText(QStringLiteral("加速度")); + m_velocitySpin->setToolTip(QStringLiteral("SpeedJ 关节点动的最大速度")); + m_accelerationSpin->setToolTip(QStringLiteral("SpeedJ 关节点动的最大加速度")); + + QLabel *jointLimitsTitle = new QLabel(QStringLiteral("SpeedJ 关节限制"), ui.commandBody); + jointLimitsTitle->setObjectName(QStringLiteral("eyebrow")); + ui.motionForm->insertRow(0, jointLimitsTitle); + + QLabel *cartesianLimitsTitle = new QLabel( + QStringLiteral("SpeedL 笛卡尔限制"), ui.commandBody); + cartesianLimitsTitle->setObjectName(QStringLiteral("eyebrow")); + ui.motionForm->insertRow(3, cartesianLimitsTitle); + m_cartesianVelocitySpin = new QDoubleSpinBox(ui.commandBody); + m_cartesianVelocitySpin->setRange(0.001, 5.0); + m_cartesianVelocitySpin->setDecimals(3); + m_cartesianVelocitySpin->setSingleStep(0.01); + m_cartesianVelocitySpin->setSuffix(QStringLiteral(" m/s")); + m_cartesianVelocitySpin->setToolTip( + QStringLiteral("SpeedL 平移点动的最大线速度;旋转点动按相同数值使用 rad/s")); + m_cartesianAccelerationSpin = new QDoubleSpinBox(ui.commandBody); + m_cartesianAccelerationSpin->setRange(0.001, 20.0); + m_cartesianAccelerationSpin->setDecimals(3); + m_cartesianAccelerationSpin->setSingleStep(0.01); + m_cartesianAccelerationSpin->setSuffix(QStringLiteral(" m/s²")); + m_cartesianAccelerationSpin->setToolTip( + QStringLiteral("SpeedL 使用的笛卡尔加速度限制")); + ui.motionForm->addRow(QStringLiteral("速度"), m_cartesianVelocitySpin); + ui.motionForm->addRow(QStringLiteral("加速度"), m_cartesianAccelerationSpin); m_torqueOnButton = ui.torqueOnButton; m_torqueOffButton = ui.torqueOffButton; m_clearFaultButton = ui.clearFaultButton; @@ -278,8 +309,6 @@ QWidget *MainWindow::createArmPage() m_motionTable = ui.motionTable; m_sendMoveButton = ui.sendMoveButton; m_jointTable = ui.jointTable; - m_poseValue = ui.poseValue; - m_updatedValue = ui.updatedValue; m_urdfViewer = ui.urdfViewer; m_teachModeButton = ui.teachModeButton; m_teachRestoreButton = ui.teachRestoreButton; @@ -290,6 +319,53 @@ QWidget *MainWindow::createArmPage() ui.jointTitle->setText(QStringLiteral("关节实时状态")); ui.jointTitle->setContentsMargins(16, 0, 0, 4); ui.poseTitle->setText(QStringLiteral("末端实时位姿")); + ui.poseSubtitle->setObjectName(QStringLiteral("poseFrameBadge")); + ui.poseSubtitle->setAlignment(Qt::AlignCenter); + ui.poseValue->hide(); + ui.updatedValue->hide(); + + QFrame *poseMetrics = new QFrame(ui.telemetryPanel); + poseMetrics->setObjectName(QStringLiteral("poseMetrics")); + QGridLayout *poseMetricsLayout = new QGridLayout(poseMetrics); + poseMetricsLayout->setContentsMargins(0, 0, 0, 0); + poseMetricsLayout->setHorizontalSpacing(8); + poseMetricsLayout->setVerticalSpacing(8); + const QStringList poseAxes = { + QStringLiteral("X"), QStringLiteral("Y"), QStringLiteral("Z"), + QStringLiteral("RX"), QStringLiteral("RY"), QStringLiteral("RZ") + }; + const QStringList poseUnits = { + QStringLiteral("m"), QStringLiteral("m"), QStringLiteral("m"), + QStringLiteral("rad"), QStringLiteral("rad"), QStringLiteral("rad") + }; + m_poseMetricValues.clear(); + for (int index = 0; index < poseAxes.size(); ++index) { + QFrame *metricCard = new QFrame(poseMetrics); + metricCard->setObjectName(QStringLiteral("poseMetricCard")); + QVBoxLayout *metricLayout = new QVBoxLayout(metricCard); + metricLayout->setContentsMargins(10, 8, 10, 8); + metricLayout->setSpacing(3); + QLabel *axisLabel = new QLabel(poseAxes.at(index), metricCard); + axisLabel->setObjectName(QStringLiteral("poseMetricAxis")); + QLabel *valueLabel = new QLabel(QStringLiteral("--"), metricCard); + valueLabel->setObjectName(QStringLiteral("poseMetricValue")); + QLabel *unitLabel = new QLabel(poseUnits.at(index), metricCard); + unitLabel->setObjectName(QStringLiteral("poseMetricUnit")); + QHBoxLayout *valueLayout = new QHBoxLayout; + valueLayout->setContentsMargins(0, 0, 0, 0); + valueLayout->setSpacing(5); + valueLayout->addWidget(valueLabel); + valueLayout->addWidget(unitLabel); + valueLayout->addStretch(1); + metricLayout->addWidget(axisLabel); + metricLayout->addLayout(valueLayout); + poseMetricsLayout->addWidget(metricCard, index / 3, index % 3); + m_poseMetricValues.append(valueLabel); + } + ui.poseLayout->insertWidget(2, poseMetrics, 1); + ui.telemetryLayout->setSpacing(14); + ui.telemetryLayout->setStretch(0, 1); + ui.telemetryLayout->setStretch(1, 1); // Replace the old target/teaching workspace with hold-to-jog controls and // a read-only telemetry panel. @@ -360,55 +436,200 @@ QWidget *MainWindow::createArmPage() frameGrid->addWidget(m_tcpFrameCombo, 0, 3); cartesianLayout->addLayout(frameGrid); - const QStringList cartesianAxes = { - QStringLiteral("X"), QStringLiteral("Y"), QStringLiteral("Z"), - QStringLiteral("RX"), QStringLiteral("RY"), QStringLiteral("RZ") + m_cartesianJogPoseLabels.clear(); + const auto createJogButton = [this](QWidget *parent, const QString &text, + int axis, int direction, + const QString &tone, + const QString &role, + int width, int height) { + QPushButton *button = new QPushButton(text, parent); + button->setObjectName(QStringLiteral("cartesianPadButton")); + button->setProperty("axisTone", tone); + button->setProperty("padRole", role); + button->setFixedSize(width, height); + button->setAutoRepeat(false); + connect(button, &QPushButton::pressed, this, + [this, axis, direction]() { startCartesianJog(axis, direction); }); + connect(button, &QPushButton::released, this, &MainWindow::stopArmJog); + return button; }; - for (int axis = 0; axis < cartesianAxes.size(); ++axis) { - if (axis == 0 || axis == 3) { - QLabel *group = new QLabel(axis == 0 ? QStringLiteral("平移(当前位置)") - : QStringLiteral("旋转(当前姿态)"), - cartesianJogPage); - group->setObjectName(QStringLiteral("eyebrow")); - cartesianLayout->addWidget(group); + const auto createPoseStrip = [this](QWidget *parent, const QStringList &axes, + const QStringList &units) { + QWidget *strip = new QWidget(parent); + QHBoxLayout *layout = new QHBoxLayout(strip); + layout->setContentsMargins(0, 0, 0, 0); + layout->setSpacing(6); + for (int index = 0; index < axes.size(); ++index) { + QFrame *chip = new QFrame(strip); + chip->setObjectName(QStringLiteral("cartesianPoseChip")); + QHBoxLayout *chipLayout = new QHBoxLayout(chip); + chipLayout->setContentsMargins(7, 4, 7, 4); + chipLayout->setSpacing(5); + QLabel *axisLabel = new QLabel(axes.at(index), chip); + axisLabel->setObjectName(QStringLiteral("cartesianPoseAxis")); + QLabel *valueLabel = new QLabel( + QStringLiteral("-- %1").arg(units.at(index)), chip); + valueLabel->setObjectName(QStringLiteral("cartesianPoseValue")); + chipLayout->addWidget(axisLabel); + chipLayout->addStretch(1); + chipLayout->addWidget(valueLabel); + layout->addWidget(chip, 1); + m_cartesianJogPoseLabels.append(valueLabel); } - QFrame *row = new QFrame(cartesianJogPage); - row->setObjectName(QStringLiteral("jogAxisRow")); - QHBoxLayout *rowLayout = new QHBoxLayout(row); - rowLayout->setContentsMargins(0, 0, 0, 0); - rowLayout->setSpacing(6); - QLabel *axisLabel = new QLabel(cartesianAxes.at(axis), row); - axisLabel->setMinimumWidth(26); - QLabel *poseLabel = new QLabel(axis < 3 ? QStringLiteral("-- m") - : QStringLiteral("-- rad"), row); - poseLabel->setObjectName(QStringLiteral("jogCurrentValue")); - poseLabel->setMinimumWidth(92); - QPushButton *minus = new QPushButton(QStringLiteral("−"), row); - QPushButton *plus = new QPushButton(QStringLiteral("+"), row); - minus->setObjectName(QStringLiteral("jogDirectionButton")); - plus->setObjectName(QStringLiteral("jogDirectionButton")); - minus->setFixedSize(38, 32); - plus->setFixedSize(38, 32); - QSlider *direction = new QSlider(Qt::Horizontal, row); - direction->setObjectName(QStringLiteral("jogDirectionIndicator")); - direction->setRange(-100, 100); - direction->setValue(0); - direction->setEnabled(false); - rowLayout->addWidget(axisLabel); - rowLayout->addWidget(poseLabel); - rowLayout->addWidget(minus); - rowLayout->addWidget(direction, 1); - rowLayout->addWidget(plus); - m_cartesianJogPoseLabels.append(poseLabel); - connect(minus, &QPushButton::pressed, this, - [this, axis]() { startCartesianJog(axis, -1); }); - connect(plus, &QPushButton::pressed, this, - [this, axis]() { startCartesianJog(axis, 1); }); - connect(minus, &QPushButton::released, this, &MainWindow::stopArmJog); - connect(plus, &QPushButton::released, this, &MainWindow::stopArmJog); - cartesianLayout->addWidget(row); - } + return strip; + }; + const auto createDirectionPad = [&createJogButton]( + QWidget *parent, + int verticalAxis, + const QString &verticalName, + const QString &verticalTone, + int horizontalAxis, + const QString &horizontalName, + const QString &horizontalTone, + const QString ¢erText) { + QFrame *pad = new QFrame(parent); + pad->setObjectName(QStringLiteral("cartesianDirectionPad")); + pad->setFixedSize(250, 180); + QGridLayout *layout = new QGridLayout(pad); + layout->setContentsMargins(8, 8, 8, 8); + layout->setHorizontalSpacing(2); + layout->setVerticalSpacing(2); + QLabel *center = new QLabel(centerText, pad); + center->setObjectName(QStringLiteral("cartesianPadCenter")); + center->setAlignment(Qt::AlignCenter); + center->setFixedSize(52, 52); + layout->addWidget(createJogButton( + pad, QStringLiteral("▲\n+%1").arg(verticalName), + verticalAxis, 1, verticalTone, QStringLiteral("north"), 86, 48), + 0, 1, Qt::AlignHCenter); + layout->addWidget(createJogButton( + pad, QStringLiteral("◀ −%1").arg(horizontalName), + horizontalAxis, -1, horizontalTone, QStringLiteral("eastTall"), 72, 150), + 0, 0, 3, 1, Qt::AlignVCenter); + layout->addWidget(center, 1, 1, Qt::AlignCenter); + layout->addWidget(createJogButton( + pad, QStringLiteral("+%1 ▶").arg(horizontalName), + horizontalAxis, 1, horizontalTone, QStringLiteral("westTall"), 72, 150), + 0, 2, 3, 1, Qt::AlignVCenter); + layout->addWidget(createJogButton( + pad, QStringLiteral("−%1\n▼").arg(verticalName), + verticalAxis, -1, verticalTone, QStringLiteral("south"), 86, 48), + 2, 1, Qt::AlignHCenter); + return pad; + }; + const auto createVerticalRocker = [&createJogButton]( + QWidget *parent, int axis, + const QString &axisName, + const QString &tone) { + QFrame *rocker = new QFrame(parent); + rocker->setObjectName(QStringLiteral("cartesianAxisRocker")); + rocker->setProperty("orientation", QStringLiteral("vertical")); + rocker->setFixedSize(76, 168); + QVBoxLayout *layout = new QVBoxLayout(rocker); + layout->setContentsMargins(8, 8, 8, 8); + layout->setSpacing(0); + QLabel *center = new QLabel(axisName, rocker); + center->setObjectName(QStringLiteral("cartesianPadCenter")); + center->setAlignment(Qt::AlignCenter); + center->setFixedSize(52, 52); + layout->addWidget(createJogButton( + rocker, QStringLiteral("▲\n+%1").arg(axisName), axis, 1, tone, + QStringLiteral("north"), 60, 50), + 0, Qt::AlignHCenter); + layout->addWidget(center, 0, Qt::AlignHCenter); + layout->addWidget(createJogButton( + rocker, QStringLiteral("−%1\n▼").arg(axisName), axis, -1, tone, + QStringLiteral("south"), 60, 50), + 0, Qt::AlignHCenter); + return rocker; + }; + const auto createHorizontalRocker = [&createJogButton]( + QWidget *parent, int axis, + const QString &axisName, + const QString &tone) { + QFrame *rocker = new QFrame(parent); + rocker->setObjectName(QStringLiteral("cartesianAxisRocker")); + rocker->setProperty("orientation", QStringLiteral("horizontal")); + rocker->setFixedSize(208, 68); + QHBoxLayout *layout = new QHBoxLayout(rocker); + layout->setContentsMargins(8, 8, 8, 8); + layout->setSpacing(0); + QLabel *center = new QLabel(axisName, rocker); + center->setObjectName(QStringLiteral("cartesianPadCenter")); + center->setAlignment(Qt::AlignCenter); + center->setFixedSize(52, 52); + layout->addWidget(createJogButton( + rocker, QStringLiteral("◀ −%1").arg(axisName), axis, -1, tone, + QStringLiteral("west"), 70, 52)); + layout->addWidget(center); + layout->addWidget(createJogButton( + rocker, QStringLiteral("+%1 ▶").arg(axisName), axis, 1, tone, + QStringLiteral("east"), 70, 52)); + return rocker; + }; + + QVBoxLayout *cartesianControls = new QVBoxLayout; + cartesianControls->setSpacing(10); + + QFrame *translationGroup = new QFrame(cartesianJogPage); + translationGroup->setObjectName(QStringLiteral("cartesianControlGroup")); + QVBoxLayout *translationLayout = new QVBoxLayout(translationGroup); + translationLayout->setContentsMargins(10, 8, 10, 10); + translationLayout->setSpacing(7); + QLabel *translationTitle = new QLabel(QStringLiteral("平移控制"), translationGroup); + translationTitle->setObjectName(QStringLiteral("cartesianGroupTitle")); + translationLayout->addWidget(translationTitle); + translationLayout->addWidget(createPoseStrip( + translationGroup, + {QStringLiteral("X"), QStringLiteral("Y"), QStringLiteral("Z")}, + {QStringLiteral("m"), QStringLiteral("m"), QStringLiteral("m")})); + QHBoxLayout *translationPads = new QHBoxLayout; + translationPads->setContentsMargins(0, 0, 0, 0); + translationPads->setSpacing(12); + translationPads->addStretch(1); + translationPads->addWidget(createDirectionPad( + translationGroup, 0, QStringLiteral("X"), QStringLiteral("x"), + 1, QStringLiteral("Y"), QStringLiteral("y"), QStringLiteral("XY"))); + translationPads->addWidget(createVerticalRocker( + translationGroup, 2, QStringLiteral("Z"), QStringLiteral("z")), + 0, Qt::AlignVCenter); + translationPads->addStretch(1); + translationLayout->addLayout(translationPads); + cartesianControls->addWidget(translationGroup, 1); + + QFrame *rotationGroup = new QFrame(cartesianJogPage); + rotationGroup->setObjectName(QStringLiteral("cartesianControlGroup")); + QVBoxLayout *rotationLayout = new QVBoxLayout(rotationGroup); + rotationLayout->setContentsMargins(10, 8, 10, 10); + rotationLayout->setSpacing(7); + QLabel *rotationTitle = new QLabel(QStringLiteral("旋转控制"), rotationGroup); + rotationTitle->setObjectName(QStringLiteral("cartesianGroupTitle")); + rotationLayout->addWidget(rotationTitle); + rotationLayout->addWidget(createPoseStrip( + rotationGroup, + {QStringLiteral("RX"), QStringLiteral("RY"), QStringLiteral("RZ")}, + {QStringLiteral("rad"), QStringLiteral("rad"), QStringLiteral("rad")})); + QHBoxLayout *rotationPads = new QHBoxLayout; + rotationPads->setContentsMargins(0, 0, 0, 0); + rotationPads->setSpacing(8); + rotationPads->addStretch(1); + rotationPads->addWidget(createDirectionPad( + rotationGroup, 3, QStringLiteral("RX"), QStringLiteral("x"), + 4, QStringLiteral("RY"), QStringLiteral("y"), QStringLiteral("RXY"))); + rotationPads->addWidget(createHorizontalRocker( + rotationGroup, 5, QStringLiteral("RZ"), QStringLiteral("z")), + 0, Qt::AlignVCenter); + rotationPads->addStretch(1); + rotationLayout->addLayout(rotationPads); + cartesianControls->addWidget(rotationGroup, 1); + + cartesianLayout->addLayout(cartesianControls, 1); cartesianLayout->addStretch(1); + if (QTabBar *jogTabBar = m_jogModeTabs->findChild()) { + jogTabBar->setExpanding(true); + jogTabBar->setUsesScrollButtons(false); + jogTabBar->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed); + } m_jogModeTabs->addTab(cartesianJogPage, QStringLiteral("笛卡尔速度")); jogLayout->addWidget(m_jogModeTabs, 1); @@ -534,12 +755,7 @@ QWidget *MainWindow::createArmPage() }); connect(m_jogModeTabs, &QTabWidget::currentChanged, this, [this](int index) { stopArmJog(); - const bool jointMode = index == 0; - m_velocitySpin->setSuffix(jointMode ? QStringLiteral(" rad/s") - : QStringLiteral(" m/s")); - m_accelerationSpin->setSuffix(jointMode ? QStringLiteral(" rad/s²") - : QStringLiteral(" m/s²")); - if (!jointMode && m_connected) + if (index == 1 && m_connected) m_client->requestArmFrames(m_armIdEdit->text().trimmed()); }); connect(m_client, &RobotClient::armFramesReceived, this, @@ -615,7 +831,8 @@ QWidget *MainWindow::createArmPage() m_armDefaultBaseFrame.clear(); m_armDefaultTcpFrame.clear(); m_latestArmPose.clear(); - m_poseValue->setText(QStringLiteral("X --\nY --\nZ --\n\nRX --\nRY --\nRZ --")); + for (QLabel *valueLabel : m_poseMetricValues) + valueLabel->setText(QStringLiteral("--")); poseSubtitle->setText(QStringLiteral("等待 SDK 默认坐标系信息")); if (m_connected) { m_client->requestArmInfo(newDeviceId); diff --git a/src/robotclient.cpp b/src/robotclient.cpp index 8e24921..fa9e78c 100644 --- a/src/robotclient.cpp +++ b/src/robotclient.cpp @@ -2009,6 +2009,8 @@ public: armFramesReceived; std::function telemetryReceived; + std::function + armTelemetryHealthChanged; std::function systemStatusReceived; std::function systemInfoReceived; std::function imageReceived; @@ -2950,7 +2952,8 @@ private: void pollGrpc() { ++m_tick; - if (m_armPollingEnabled && !m_armDeviceId.isEmpty()) { + const bool armPollDue = m_consecutivePollErrors < 3 || m_tick % 10 == 0; + if (m_armPollingEnabled && !m_armDeviceId.isEmpty() && armPollDue) { const QString polledDeviceId = m_armDeviceId; cmvr::api::GetRobotState::Request request; fillHeader(request.mutable_header(), polledDeviceId); @@ -2959,6 +2962,7 @@ private: setDeadline(context, 350); const grpc::Status status = m_arm->getRobotState(&context, request, &response); if (status.ok() && response.header().success()) { + const bool recovered = m_consecutivePollErrors >= 3; m_consecutivePollErrors = 0; QVariantList joints; const auto &robotState = response.state(); @@ -2979,7 +2983,8 @@ private: pose.insert(QStringLiteral("rx"), tcp.rx()); pose.insert(QStringLiteral("ry"), tcp.ry()); pose.insert(QStringLiteral("rz"), tcp.rz()); - if (!m_armBaseFrame.isEmpty() && !m_armTcpFrame.isEmpty()) { + if (robotState.connected() + && !m_armBaseFrame.isEmpty() && !m_armTcpFrame.isEmpty()) { cmvr::api::GetPose::Request poseRequest; fillHeader(poseRequest.mutable_header(), polledDeviceId); poseRequest.set_base_link(m_armBaseFrame.toStdString()); @@ -3003,6 +3008,9 @@ private: state.insert(QStringLiteral("robotMode"), static_cast(robotState.robot_mode())); state.insert(QStringLiteral("safetyMode"), static_cast(robotState.safety_mode())); state.insert(QStringLiteral("controlMode"), static_cast(robotState.control_mode())); + state.insert(QStringLiteral("freedrive"), + robotState.control_mode() + == cmvr::api::ARM_CONTROL_MODE_FREEDRIVE); state.insert(QStringLiteral("connected"), robotState.connected()); state.insert(QStringLiteral("poweredOn"), robotState.powered_on()); state.insert(QStringLiteral("brakeReleased"), robotState.brake_released()); @@ -3012,10 +3020,27 @@ private: state.insert(QStringLiteral("emergencyStopped"), robotState.emergency_stopped()); state.insert(QStringLiteral("speedScaling"), robotState.speed_scaling()); emit telemetryReceived(polledDeviceId, joints, pose, state); + if (recovered) { + emit armTelemetryHealthChanged( + polledDeviceId, true, QStringLiteral("机械臂遥测通信已恢复")); + } } else { ++m_consecutivePollErrors; - if (m_consecutivePollErrors == 10) - emit logMessage(QStringLiteral("WARN"), QStringLiteral("ArmService 连续 10 次遥测请求失败")); + QString detail; + if (!status.ok()) { + const QString message = QString::fromStdString(status.error_message()); + detail = QStringLiteral("gRPC code %1:%2") + .arg(static_cast(status.error_code())) + .arg(message.isEmpty() + ? QStringLiteral("服务端未返回错误详情") + : message); + } else { + detail = QString::fromStdString(response.header().error_message()); + if (detail.isEmpty()) + detail = QStringLiteral("边缘端未返回机械臂状态"); + } + if (m_consecutivePollErrors == 3) + emit armTelemetryHealthChanged(polledDeviceId, false, detail); } } @@ -3131,6 +3156,12 @@ RobotClient::RobotClient(QObject *parent) emit telemetryReceived(deviceId, joints, pose, state); }, Qt::QueuedConnection); }; + m_worker->armTelemetryHealthChanged = + [this](const QString &deviceId, bool available, const QString &detail) { + QMetaObject::invokeMethod(this, [this, deviceId, available, detail]() { + emit armTelemetryHealthChanged(deviceId, available, detail); + }, Qt::QueuedConnection); + }; m_worker->systemStatusReceived = [this](double cpu, double memory, double disk, const QVariantList &devices) { QMetaObject::invokeMethod(this, [this, cpu, memory, disk, devices]() { diff --git a/src/robotclient.h b/src/robotclient.h index caeb301..90a7c77 100644 --- a/src/robotclient.h +++ b/src/robotclient.h @@ -152,6 +152,8 @@ signals: const QStringList &tcpFrames); void telemetryReceived(const QString &deviceId, const QVariantList &joints, const QVariantMap &pose, const QVariantMap &state); + void armTelemetryHealthChanged(const QString &deviceId, bool available, + const QString &detail); void systemStatusReceived(double cpu, double memory, double disk, const QVariantList &devices); void systemInfoReceived(const QVariantMap &info); diff --git a/src/urdfviewerwidget.cpp b/src/urdfviewerwidget.cpp index c130dd6..6889282 100644 --- a/src/urdfviewerwidget.cpp +++ b/src/urdfviewerwidget.cpp @@ -4,25 +4,33 @@ #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 #include #include +#include #include #include @@ -40,10 +48,12 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -411,6 +421,14 @@ void UrdfViewerWidget::ensureSceneInitialized() m_camera = m_view->camera(); m_camera->lens()->setPerspectiveProjection(42.0f, 16.0f / 9.0f, 0.01f, 1000.0f); + createOrientationIndicator(); + connect(m_camera, &Qt3DRender::QCamera::positionChanged, + this, [this](const QVector3D &) { updateOrientationIndicator(); }); + connect(m_camera, &Qt3DRender::QCamera::viewCenterChanged, + this, [this](const QVector3D &) { updateOrientationIndicator(); }); + connect(m_camera, &Qt3DRender::QCamera::upVectorChanged, + this, [this](const QVector3D &) { updateOrientationIndicator(); }); + Qt3DCore::QEntity *lightEntity = new Qt3DCore::QEntity(m_rootEntity); Qt3DRender::QPointLight *light = new Qt3DRender::QPointLight(lightEntity); light->setColor(QColor(QStringLiteral("#ffffff"))); @@ -429,6 +447,222 @@ void UrdfViewerWidget::ensureSceneInitialized() resetView(); } +void UrdfViewerWidget::createOrientationIndicator() +{ + if (!m_view || !m_rootEntity || m_orientationCamera) + return; + + constexpr float shaftLength = 0.78f; + constexpr float headLength = 0.24f; + constexpr float labelDistance = 1.12f; + + Qt3DCore::QEntity *root = new Qt3DCore::QEntity(m_rootEntity); + root->setObjectName(QStringLiteral("orientationIndicator")); + + m_orientationLayer = new Qt3DRender::QLayer(root); + m_orientationLayer->setRecursive(true); + root->addComponent(m_orientationLayer); + + const auto createMaterial = [root](const QColor &color) { + Qt3DExtras::QPhongMaterial *material = + new Qt3DExtras::QPhongMaterial(root); + material->setAmbient(color); + material->setDiffuse(color); + material->setSpecular(QColor(QStringLiteral("#ffffff"))); + material->setShininess(36.0f); + return material; + }; + + const auto addAxis = [this, root, &createMaterial]( + const QVector3D &direction, + const QColor &color, + const QString &name) { + Qt3DCore::QEntity *shaft = new Qt3DCore::QEntity(root); + Qt3DExtras::QCylinderMesh *shaftMesh = + new Qt3DExtras::QCylinderMesh(shaft); + shaftMesh->setRadius(0.035f); + shaftMesh->setLength(shaftLength); + shaftMesh->setRings(2); + shaftMesh->setSlices(24); + Qt3DCore::QTransform *shaftTransform = + new Qt3DCore::QTransform(shaft); + shaftTransform->setTranslation(direction * (shaftLength * 0.5f)); + shaftTransform->setRotation(QQuaternion::rotationTo( + QVector3D(0.0f, 1.0f, 0.0f), direction)); + shaft->addComponent(shaftMesh); + shaft->addComponent(createMaterial(color)); + shaft->addComponent(shaftTransform); + + Qt3DCore::QEntity *head = new Qt3DCore::QEntity(root); + Qt3DExtras::QConeMesh *headMesh = new Qt3DExtras::QConeMesh(head); + headMesh->setBottomRadius(0.105f); + headMesh->setTopRadius(0.0f); + headMesh->setLength(headLength); + headMesh->setRings(2); + headMesh->setSlices(24); + Qt3DCore::QTransform *headTransform = new Qt3DCore::QTransform(head); + headTransform->setTranslation( + direction * (shaftLength + headLength * 0.5f)); + headTransform->setRotation(QQuaternion::rotationTo( + QVector3D(0.0f, 1.0f, 0.0f), direction)); + head->addComponent(headMesh); + head->addComponent(createMaterial(color)); + head->addComponent(headTransform); + + Qt3DExtras::QText2DEntity *label = + new Qt3DExtras::QText2DEntity(root); + QFont font; + font.setPointSize(22); + font.setBold(true); + label->setFont(font); + label->setText(name); + label->setColor(color.darker(115)); + label->setWidth(42.0f); + label->setHeight(34.0f); + Qt3DCore::QTransform *labelTransform = + new Qt3DCore::QTransform(label); + labelTransform->setTranslation(direction * labelDistance); + labelTransform->setScale(0.0075f); + label->addComponent(labelTransform); + m_orientationLabelTransforms.append(labelTransform); + }; + + addAxis(QVector3D(1.0f, 0.0f, 0.0f), + QColor(QStringLiteral("#e24848")), QStringLiteral("X")); + addAxis(QVector3D(0.0f, 1.0f, 0.0f), + QColor(QStringLiteral("#25a05a")), QStringLiteral("Y")); + addAxis(QVector3D(0.0f, 0.0f, 1.0f), + QColor(QStringLiteral("#2575d8")), QStringLiteral("Z")); + + Qt3DCore::QEntity *origin = new Qt3DCore::QEntity(root); + Qt3DExtras::QSphereMesh *originMesh = + new Qt3DExtras::QSphereMesh(origin); + originMesh->setRadius(0.085f); + originMesh->setRings(16); + originMesh->setSlices(24); + origin->addComponent(originMesh); + origin->addComponent(createMaterial(QColor(QStringLiteral("#60758a")))); + + m_orientationCamera = new Qt3DRender::QCamera(root); + m_orientationCamera->lens()->setOrthographicProjection( + -1.42f, 1.42f, -1.42f, 1.42f, 0.1f, 20.0f); + m_orientationCamera->setViewCenter(QVector3D(0.0f, 0.0f, 0.0f)); + + Qt3DRender::QRenderSurfaceSelector *surfaceSelector = + new Qt3DRender::QRenderSurfaceSelector; + surfaceSelector->setSurface(m_view); + + Qt3DRender::QViewport *mainViewport = + new Qt3DRender::QViewport(surfaceSelector); + mainViewport->setNormalizedRect(QRectF(0.0, 0.0, 1.0, 1.0)); + Qt3DRender::QClearBuffers *mainClear = + new Qt3DRender::QClearBuffers(mainViewport); + mainClear->setBuffers(Qt3DRender::QClearBuffers::ColorDepthBuffer); + mainClear->setClearColor(QColor(QStringLiteral("#c7ced6"))); + Qt3DRender::QCameraSelector *mainCameraSelector = + new Qt3DRender::QCameraSelector(mainClear); + mainCameraSelector->setCamera(m_camera); + Qt3DRender::QLayerFilter *mainLayerFilter = + new Qt3DRender::QLayerFilter(mainCameraSelector); + mainLayerFilter->setFilterMode( + Qt3DRender::QLayerFilter::DiscardAnyMatchingLayers); + mainLayerFilter->addLayer(m_orientationLayer); + + m_orientationViewport = new Qt3DRender::QViewport(surfaceSelector); + Qt3DRender::QClearBuffers *orientationDepthClear = + new Qt3DRender::QClearBuffers(m_orientationViewport); + orientationDepthClear->setBuffers(Qt3DRender::QClearBuffers::DepthBuffer); + Qt3DRender::QCameraSelector *orientationCameraSelector = + new Qt3DRender::QCameraSelector(orientationDepthClear); + orientationCameraSelector->setCamera(m_orientationCamera); + Qt3DRender::QLayerFilter *orientationLayerFilter = + new Qt3DRender::QLayerFilter(orientationCameraSelector); + orientationLayerFilter->setFilterMode( + Qt3DRender::QLayerFilter::AcceptAnyMatchingLayers); + orientationLayerFilter->addLayer(m_orientationLayer); + + m_view->setActiveFrameGraph(surfaceSelector); + positionOrientationIndicator(); + updateOrientationIndicator(); +} + +void UrdfViewerWidget::updateOrientationIndicator() +{ + if (!m_camera || !m_orientationCamera || !m_view) + return; + + QVector3D forward = m_camera->viewCenter() - m_camera->position(); + if (forward.lengthSquared() < 1.0e-8f) + return; + forward.normalize(); + + QVector3D up = m_camera->upVector(); + if (up.lengthSquared() < 1.0e-8f) + up = QVector3D(0.0f, 0.0f, 1.0f); + else + up.normalize(); + + m_orientationCamera->setPosition(-forward * 4.0f); + m_orientationCamera->setViewCenter(QVector3D(0.0f, 0.0f, 0.0f)); + m_orientationCamera->setUpVector(up); + + const QVector3D cameraPosition = m_orientationCamera->position(); + for (Qt3DCore::QTransform *transform : m_orientationLabelTransforms) { + if (!transform) + continue; + + QVector3D normal = cameraPosition - transform->translation(); + if (normal.lengthSquared() < 1.0e-8f) + continue; + normal.normalize(); + + QVector3D right = QVector3D::crossProduct(up, normal); + if (right.lengthSquared() < 1.0e-8f) + right = QVector3D::crossProduct(QVector3D(1.0f, 0.0f, 0.0f), normal); + right.normalize(); + const QVector3D billboardUp = + QVector3D::crossProduct(normal, right).normalized(); + + QMatrix3x3 rotation; + rotation(0, 0) = right.x(); + rotation(1, 0) = right.y(); + rotation(2, 0) = right.z(); + rotation(0, 1) = billboardUp.x(); + rotation(1, 1) = billboardUp.y(); + rotation(2, 1) = billboardUp.z(); + rotation(0, 2) = normal.x(); + rotation(1, 2) = normal.y(); + rotation(2, 2) = normal.z(); + transform->setRotation(QQuaternion::fromRotationMatrix(rotation)); + } + + m_view->requestUpdate(); +} + +void UrdfViewerWidget::positionOrientationIndicator() +{ + if (!m_view || !m_orientationViewport) + return; + + constexpr int indicatorSize = 168; + constexpr int margin = 14; + const int viewportWidth = m_view->width(); + const int viewportHeight = m_view->height(); + if (viewportWidth <= 0 || viewportHeight <= 0) + return; + + const qreal width = qMin(1.0, static_cast(indicatorSize) + / static_cast(viewportWidth)); + const qreal height = qMin(1.0, static_cast(indicatorSize) + / static_cast(viewportHeight)); + const qreal left = qMax(0.0, 1.0 - width + - static_cast(margin) / viewportWidth); + const qreal top = qMax(0.0, static_cast(margin) / viewportHeight); + m_orientationViewport->setNormalizedRect( + QRectF(left, top, width, height)); + m_view->requestUpdate(); +} + void UrdfViewerWidget::setLoading(bool loading, const QString &message) { if (m_loadingTitle) @@ -468,6 +702,12 @@ bool UrdfViewerWidget::eventFilter(QObject *watched, QEvent *event) return QWidget::eventFilter(watched, event); switch (event->type()) { + case QEvent::Resize: + case QEvent::Show: + QTimer::singleShot(0, this, [this]() { + positionOrientationIndicator(); + }); + break; case QEvent::MouseButtonPress: { QMouseEvent *mouseEvent = static_cast(event); if (mouseEvent->button() != Qt::LeftButton && mouseEvent->button() != Qt::RightButton) @@ -931,6 +1171,15 @@ QStringList UrdfViewerWidget::jointNames() const return m_jointOrder; } +QHash> UrdfViewerWidget::jointLimits() const +{ + QHash> limits; + limits.reserve(m_joints.size()); + for (auto it = m_joints.constBegin(); it != m_joints.constEnd(); ++it) + limits.insert(it.key(), qMakePair(it.value().lower, it.value().upper)); + return limits; +} + QHash UrdfViewerWidget::teachingPositions() const { return m_jointPositions; diff --git a/src/urdfviewerwidget.h b/src/urdfviewerwidget.h index 287fb6f..54ef713 100644 --- a/src/urdfviewerwidget.h +++ b/src/urdfviewerwidget.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -27,7 +28,9 @@ class Qt3DWindow; namespace Qt3DRender { class QCamera; +class QLayer; class QScreenRayCaster; +class QViewport; } class UrdfViewerWidget : public QWidget @@ -45,6 +48,7 @@ public: const QString &subtype, const QString &descriptionId); void setJointPositions(const QHash &positions); QStringList jointNames() const; + QHash> jointLimits() const; QHash teachingPositions() const; QString selectedTeachingJoint() const; void setTeachingEnabled(bool enabled); @@ -95,10 +99,17 @@ private: void refreshBundledModels(); void syncModelSelection(const QString &filePath); QString resolveMeshPath(const QString &uri) const; + void createOrientationIndicator(); + void updateOrientationIndicator(); + void positionOrientationIndicator(); void scheduleViewAll(); Qt3DExtras::Qt3DWindow *m_view = nullptr; Qt3DRender::QCamera *m_camera = nullptr; + Qt3DRender::QCamera *m_orientationCamera = nullptr; + Qt3DRender::QLayer *m_orientationLayer = nullptr; + Qt3DRender::QViewport *m_orientationViewport = nullptr; + QVector m_orientationLabelTransforms; Qt3DRender::QScreenRayCaster *m_teachingRayCaster = nullptr; Qt3DCore::QEntity *m_rootEntity = nullptr; Qt3DCore::QEntity *m_modelRoot = nullptr; diff --git a/styles.qss b/styles.qss index 26e4b60..aa0234d 100644 --- a/styles.qss +++ b/styles.qss @@ -1007,6 +1007,39 @@ QLabel#poseValue { font-family: Consolas; font-size: 13px; } +QLabel#poseFrameBadge { + color: #526b82; + background: #eef5fc; + border: 1px solid #d8e8f7; + border-radius: 4px; + padding: 5px 9px; + font-size: 11px; + font-weight: 600; +} +QFrame#poseMetrics { + background: transparent; + border: none; +} +QFrame#poseMetricCard { + background: #f7f9fc; + border: 1px solid #e0e7ef; + border-radius: 6px; +} +QLabel#poseMetricAxis { + color: #2174d8; + font-size: 11px; + font-weight: 700; +} +QLabel#poseMetricValue { + color: #243648; + font-family: Consolas; + font-size: 16px; + font-weight: 700; +} +QLabel#poseMetricUnit { + color: #8492a0; + font-size: 10px; +} QPushButton { background: white; color: #353b43; @@ -1368,6 +1401,134 @@ QPushButton#jogDirectionButton:pressed { background: #1b6ef3; border-color: #1b6ef3; } +QFrame#cartesianControlGroup { + background: #f8fafc; + border: 1px solid #dfe6ee; + border-radius: 7px; +} +QLabel#cartesianGroupTitle { + color: #34495e; + font-size: 12px; + font-weight: 700; +} +QFrame#cartesianPoseChip { + background: #ffffff; + border: 1px solid #e2e8ef; + border-radius: 4px; +} +QLabel#cartesianPoseAxis { + color: #708090; + font-size: 10px; + font-weight: 700; +} +QLabel#cartesianPoseValue { + color: #2e4357; + font-family: Consolas; + font-size: 11px; + font-weight: 600; +} +QLabel#cartesianPadCenter { + color: #536a7e; + background: #ffffff; + border: 2px solid #b9cadd; + border-radius: 25px; + font-size: 12px; + font-weight: 700; +} +QFrame#cartesianDirectionPad, +QFrame#cartesianAxisRocker { + background: #edf3f8; + border: 1px solid #d5e0ea; +} +QFrame#cartesianDirectionPad { + border-radius: 82px; +} +QFrame#cartesianAxisRocker[orientation="vertical"] { + border-radius: 36px; +} +QFrame#cartesianAxisRocker[orientation="horizontal"] { + border-radius: 32px; +} +QPushButton#cartesianPadButton { + padding: 0; + background: #ffffff; + border: 1px solid #cbd8e6; + border-radius: 10px; + font-size: 11px; + font-weight: 700; +} +QPushButton#cartesianPadButton[padRole="north"] { + border-top-left-radius: 25px; + border-top-right-radius: 25px; + border-bottom-left-radius: 9px; + border-bottom-right-radius: 9px; +} +QPushButton#cartesianPadButton[padRole="south"] { + border-top-left-radius: 9px; + border-top-right-radius: 9px; + border-bottom-left-radius: 25px; + border-bottom-right-radius: 25px; +} +QPushButton#cartesianPadButton[padRole="west"] { + border-top-left-radius: 25px; + border-bottom-left-radius: 25px; + border-top-right-radius: 9px; + border-bottom-right-radius: 9px; +} +QPushButton#cartesianPadButton[padRole="east"] { + border-top-left-radius: 9px; + border-bottom-left-radius: 9px; + border-top-right-radius: 25px; + border-bottom-right-radius: 25px; +} +QPushButton#cartesianPadButton[padRole="westTall"] { + border-top-left-radius: 36px; + border-bottom-left-radius: 36px; + border-top-right-radius: 9px; + border-bottom-right-radius: 9px; +} +QPushButton#cartesianPadButton[padRole="eastTall"] { + border-top-left-radius: 9px; + border-bottom-left-radius: 9px; + border-top-right-radius: 36px; + border-bottom-right-radius: 36px; +} +QPushButton#cartesianPadButton:hover { + background: #f0f6fd; + border-color: #8eb8e8; +} +QPushButton#cartesianPadButton:pressed { + color: #ffffff; + background: #2878d0; + border-color: #2878d0; +} +QPushButton#cartesianPadButton[axisTone="x"] { + color: #d34b53; + border-color: #e6b9bd; +} +QPushButton#cartesianPadButton[axisTone="y"] { + color: #24915b; + border-color: #abd9c3; +} +QPushButton#cartesianPadButton[axisTone="z"] { + color: #287bbd; + border-color: #acd0e9; +} +QPushButton#cartesianPadButton[axisTone="x"]:pressed { + color: #ffffff; + background: #d34b53; + border-color: #d34b53; +} +QPushButton#cartesianPadButton[axisTone="y"]:pressed { + color: #ffffff; + background: #24915b; + border-color: #24915b; +} +QPushButton#cartesianPadButton[axisTone="z"]:pressed { + color: #ffffff; + background: #287bbd; + border-color: #287bbd; +} QSlider#jogDirectionIndicator::groove:horizontal { height: 5px; background: #d7dee6;