diff --git a/ground_station_monitoring_ui_archived/README.md b/ground_station_monitoring_ui_archived/README.md new file mode 100644 index 0000000..542defa --- /dev/null +++ b/ground_station_monitoring_ui_archived/README.md @@ -0,0 +1,78 @@ +# WayBionic Ground Station Monitoring Interface (Archived) + +> **Archived:** This standalone PySide6 prototype has been superseded by the RViz2-native ground station UI in `waybionic_rviz_plugins/`. Kept for reference only. + +This folder contains a first desktop UI proof of concept for the WayBionic engineering and operations ground station. It is a standalone Python + PySide6/Qt dashboard focused on monitoring diagnostic health and live values. + +The prototype uses locally generated mock diagnostic data immediately. It does not depend on robot hardware, sensors, ROS topics, PCBs, cameras, or the doctor/surgeon controller. + +## Languages and Frameworks + +This prototype is built as a standalone desktop app using: + +- **Python 3**: core application language for the entry point, dashboard logic, mock diagnostics, and normalized diagnostic contract. +- **PySide6**: official Qt for Python bindings used to render the desktop UI. +- **Qt Widgets**: window layout, panels, tables, buttons, timers, and Qt Style Sheets for the dark engineering dashboard theme. + +Current Python dependencies are listed in `requirements.txt`. The only required package today is `PySide6`. + +This sprint does not use ROS 2, rclpy, or web frameworks in the UI layer. Future live integration is expected to add a backend diagnostics subscriber (for example via ROS 2), but the dashboard should continue consuming the same normalized `DiagnosticMessage` format rather than depending directly on ROS message types. + +## Scope + +This app is monitoring-only. + +- It does not send motor commands. +- It does not participate in the real-time safety-critical control loop. +- It does not implement video streaming. +- It does not implement robot control or arm simulation. + +The interface is intended to give operators and engineers an early ground station layout while the live robotics integrations are still being built. + +## UI Regions + +- Top bar: demo state controls, current NORMAL/FAULT state, title, and last-updated indicator. +- System Status: diagnostic source, future ROS 2 connection status, backend heartbeat, UI mode, and safety note. +- Telemetry + Live Values: compact diagnostic table for current readings. +- Current Alerts: derived from any diagnostic signal whose status is not `OK`. +- Surgeon Camera View: reserved placeholder; no video streaming in this sprint. +- Robot / Arm Visualization: reserved placeholder; monitoring-only for now. + +## Install + +From this folder: + +```powershell +python -m venv .venv +.\.venv\Scripts\Activate.ps1 +python -m pip install -r requirements.txt +``` + +On Linux/macOS: + +```bash +python -m venv .venv +source .venv/bin/activate +python -m pip install -r requirements.txt +``` + +## Run + +```powershell +python app.py +``` + +## Demo Modes + +Use the top-left buttons to switch states: + +- `Normal Demo`: all mock diagnostic signals report `OK`, and the alerts panel shows `No active alerts`. +- `Fault Demo`: board temperature reports a high-temperature `FAULT`, IMU heartbeat reports `STALE`, and alerts become visually urgent. + +The dashboard refreshes roughly once per second and updates the telemetry table, alert panel, backend heartbeat, state indicator, and last-updated text from the current diagnostic source. + +## Future ROS 2 Integration + +The UI depends on normalized `DiagnosticMessage` objects, not on mock internals. A future `ROS2DiagnosticsSubscriber` should replace `MockDiagnosticsSource` and convert live ROS 2 diagnostics into the same normalized contract before the data reaches the UI. + +See `docs/DIAGNOSTICS_CONTRACT.md` for the expected data shape and backend integration notes. diff --git a/ground_station_monitoring_ui_archived/app.py b/ground_station_monitoring_ui_archived/app.py new file mode 100644 index 0000000..731d6df --- /dev/null +++ b/ground_station_monitoring_ui_archived/app.py @@ -0,0 +1,20 @@ +"""Entry point for the WayBionic ground station monitoring prototype.""" + +from __future__ import annotations + +import sys + +from PySide6.QtWidgets import QApplication + +from src.main_window import GroundStationMainWindow + + +def main() -> int: + app = QApplication(sys.argv) + window = GroundStationMainWindow() + window.show() + return app.exec() + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/ground_station_monitoring_ui_archived/docs/DIAGNOSTICS_CONTRACT.md b/ground_station_monitoring_ui_archived/docs/DIAGNOSTICS_CONTRACT.md new file mode 100644 index 0000000..01e4e22 --- /dev/null +++ b/ground_station_monitoring_ui_archived/docs/DIAGNOSTICS_CONTRACT.md @@ -0,0 +1,89 @@ +# Diagnostics Contract + +The ground station UI consumes normalized diagnostic messages. Mock data uses this contract today, and future ROS 2 data should be converted into the same shape before reaching the UI. + +## Internal Format + +```python +DiagnosticMessage: + signal_name: str + status: "OK" | "WARN" | "FAULT" | "STALE" + timestamp: str + value: float | str | None + unit: str | None + alert_message: str | None +``` + +## Fields + +- `signal_name`: stable name of the monitored topic or signal, such as `board.temperature`, `motor.current`, `imu.roll`, `imu.pitch`, `imu.yaw`, or `imu.heartbeat`. +- `status`: current health state of the signal. It must map cleanly to `OK`, `WARN`, `FAULT`, or `STALE`. +- `timestamp`: time the diagnostic was generated or last updated. ISO-8601 UTC strings are preferred. +- `value`: current reading, if applicable. +- `unit`: unit for the value, such as `°C`, `A`, `deg`, `V`, or empty/null. +- `alert_message`: human-readable message shown in the alerts panel when status is not `OK`. + +## Example Normal Diagnostics + +```python +[ + DiagnosticMessage("board.temperature", "OK", "2026-06-06T18:30:00.400000+00:00", 42, "°C", None), + DiagnosticMessage("motor.current", "OK", "2026-06-06T18:30:00.500000+00:00", 0.8, "A", None), + DiagnosticMessage("imu.roll", "OK", "2026-06-06T18:30:00.200000+00:00", 1.2, "deg", None), + DiagnosticMessage("imu.pitch", "OK", "2026-06-06T18:30:00.200000+00:00", -0.4, "deg", None), + DiagnosticMessage("imu.yaw", "OK", "2026-06-06T18:30:00.200000+00:00", 12.9, "deg", None), +] +``` + +## Example Fault Diagnostics + +```python +[ + DiagnosticMessage( + "board.temperature", + "FAULT", + "2026-06-06T18:30:00.200000+00:00", + 82, + "°C", + "High temperature detected", + ), + DiagnosticMessage("motor.current", "OK", "2026-06-06T18:30:00.500000+00:00", 0.8, "A", None), + DiagnosticMessage("imu.roll", "OK", "2026-06-06T18:30:00.200000+00:00", 1.2, "deg", None), + DiagnosticMessage("imu.pitch", "OK", "2026-06-06T18:30:00.200000+00:00", -0.4, "deg", None), + DiagnosticMessage("imu.yaw", "OK", "2026-06-06T18:30:00.200000+00:00", 12.9, "deg", None), + DiagnosticMessage( + "imu.heartbeat", + "STALE", + "2026-06-06T18:29:55.200000+00:00", + None, + None, + "Sensor timeout", + ), +] +``` + +## Future ROS 2 Integration + +The current flow is: + +```text +MockDiagnosticsSource -> normalized DiagnosticMessage objects -> UI +``` + +The future flow should be: + +```text +ROS2DiagnosticsSubscriber -> normalized DiagnosticMessage objects -> same UI +``` + +The preferred future topic is `/diagnostics`. The backend may publish diagnostics using ROS 2 diagnostic-style messages, such as diagnostic array/status messages, or another agreed format. + +Whatever live ROS 2 source is used, it should be converted into the normalized `DiagnosticMessage` format before reaching the UI. The UI should not directly depend on ROS message internals. + +## Integration Note For Korede / Backend + +Please publish each monitored signal with a stable signal name, status level, timestamp, current value if applicable, unit, and alert message if applicable. + +The UI expects statuses to map cleanly to `OK`, `WARN`, `FAULT`, or `STALE`. Faults and stale signals should generate visible alerts. + +The UI can initially be tested using mock messages, then connected to live ROS 2 diagnostics once available. diff --git a/ground_station_monitoring_ui_archived/requirements.txt b/ground_station_monitoring_ui_archived/requirements.txt new file mode 100644 index 0000000..7fa34f0 --- /dev/null +++ b/ground_station_monitoring_ui_archived/requirements.txt @@ -0,0 +1 @@ +PySide6 diff --git a/ground_station_monitoring_ui_archived/src/__init__.py b/ground_station_monitoring_ui_archived/src/__init__.py new file mode 100644 index 0000000..5ad0570 --- /dev/null +++ b/ground_station_monitoring_ui_archived/src/__init__.py @@ -0,0 +1 @@ +"""WayBionic ground station monitoring prototype package.""" diff --git a/ground_station_monitoring_ui_archived/src/diagnostics_contract.py b/ground_station_monitoring_ui_archived/src/diagnostics_contract.py new file mode 100644 index 0000000..37d9f5b --- /dev/null +++ b/ground_station_monitoring_ui_archived/src/diagnostics_contract.py @@ -0,0 +1,48 @@ +"""Normalized diagnostics contract used by the monitoring UI. + +The UI consumes these objects only. A future ROS 2 subscriber should convert +live diagnostics into this shape before handing them to the dashboard. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from datetime import datetime, timedelta, timezone +from typing import Literal + + +DiagnosticStatus = Literal["OK", "WARN", "FAULT", "STALE"] + + +@dataclass(frozen=True) +class DiagnosticMessage: + signal_name: str + status: DiagnosticStatus + timestamp: str + value: float | str | None + unit: str | None + alert_message: str | None = None + + +def utc_timestamp(seconds_ago: float = 0.0) -> str: + """Return an ISO-8601 UTC timestamp offset from the current time.""" + now = datetime.now(timezone.utc) + if seconds_ago: + now = now - timedelta(seconds=seconds_ago) + return now.isoformat() + + +def seconds_since(timestamp: str) -> float: + """Return age in seconds for an ISO-8601 timestamp.""" + try: + parsed = datetime.fromisoformat(timestamp) + except ValueError: + return 0.0 + + if parsed.tzinfo is None: + parsed = parsed.replace(tzinfo=timezone.utc) + return max(0.0, (datetime.now(timezone.utc) - parsed).total_seconds()) + + +def format_age(timestamp: str) -> str: + return f"{seconds_since(timestamp):.1f}s ago" diff --git a/ground_station_monitoring_ui_archived/src/main_window.py b/ground_station_monitoring_ui_archived/src/main_window.py new file mode 100644 index 0000000..d044215 --- /dev/null +++ b/ground_station_monitoring_ui_archived/src/main_window.py @@ -0,0 +1,348 @@ +"""Main PySide6 dashboard window for the monitoring prototype.""" + +from __future__ import annotations + +from PySide6.QtCore import Qt, QTimer +from PySide6.QtGui import QColor +from PySide6.QtWidgets import ( + QAbstractItemView, + QButtonGroup, + QFrame, + QGridLayout, + QHBoxLayout, + QHeaderView, + QLabel, + QMainWindow, + QPushButton, + QSizePolicy, + QTableWidget, + QTableWidgetItem, + QVBoxLayout, + QWidget, +) + +from src.diagnostics_contract import DiagnosticMessage, format_age, seconds_since +from src.mock_diagnostics import MockDiagnosticsSource +from src.styles import APP_STYLESHEET, COLORS, STATUS_COLORS + + +class GroundStationMainWindow(QMainWindow): + """Standalone monitoring-first ground station dashboard.""" + + def __init__(self) -> None: + super().__init__() + self.setWindowTitle("WayBionic Ground Station") + self.resize(1360, 820) + + self.diagnostics_source = MockDiagnosticsSource() + self.current_messages: list[DiagnosticMessage] = [] + + self.setStyleSheet(APP_STYLESHEET) + self.setCentralWidget(self._build_central_widget()) + + self.timer = QTimer(self) + self.timer.timeout.connect(self.refresh) + self.timer.start(1000) + self.refresh() + + def _build_central_widget(self) -> QWidget: + root = QWidget() + root_layout = QVBoxLayout(root) + root_layout.setContentsMargins(18, 14, 18, 18) + root_layout.setSpacing(14) + + root_layout.addLayout(self._build_top_bar()) + + body = QHBoxLayout() + body.setSpacing(14) + body.addLayout(self._build_left_column(), 5) + body.addLayout(self._build_right_column(), 6) + root_layout.addLayout(body, 1) + + return root + + def _build_top_bar(self) -> QHBoxLayout: + top_bar = QHBoxLayout() + top_bar.setSpacing(12) + + state_controls = QHBoxLayout() + self.normal_button = QPushButton("Normal Demo") + self.normal_button.setCheckable(True) + self.normal_button.setChecked(True) + self.fault_button = QPushButton("Fault Demo") + self.fault_button.setCheckable(True) + + self.demo_buttons = QButtonGroup(self) + self.demo_buttons.setExclusive(True) + self.demo_buttons.addButton(self.normal_button) + self.demo_buttons.addButton(self.fault_button) + self.normal_button.clicked.connect(lambda: self._set_demo_mode("normal")) + self.fault_button.clicked.connect(lambda: self._set_demo_mode("fault")) + + self.state_label = QLabel("Current State: NORMAL") + self.state_label.setObjectName("StateNormal") + state_controls.addWidget(self.normal_button) + state_controls.addWidget(self.fault_button) + state_controls.addWidget(self.state_label) + + title = QLabel("WayBionic Ground Station") + title.setObjectName("Title") + title.setAlignment(Qt.AlignCenter) + + self.last_updated_label = QLabel("Last updated: --") + self.last_updated_label.setObjectName("Muted") + self.last_updated_label.setAlignment(Qt.AlignRight | Qt.AlignVCenter) + + top_bar.addLayout(state_controls, 2) + top_bar.addWidget(title, 3) + top_bar.addWidget(self.last_updated_label, 2) + return top_bar + + def _build_left_column(self) -> QVBoxLayout: + left = QVBoxLayout() + left.setSpacing(14) + left.addWidget(self._build_system_status_panel(), 2) + left.addWidget(self._build_telemetry_panel(), 5) + left.addWidget(self._build_alerts_panel(), 3) + return left + + def _build_right_column(self) -> QVBoxLayout: + right = QVBoxLayout() + right.setSpacing(14) + right.addWidget( + self._build_placeholder_panel( + "Surgeon Camera View", + "Reserved - no video streaming in this sprint", + ), + 1, + ) + right.addWidget( + self._build_placeholder_panel( + "Robot / Arm Visualization", + "Reserved - monitoring only", + ), + 1, + ) + return right + + def _panel(self) -> QFrame: + panel = QFrame() + panel.setObjectName("Panel") + panel.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + return panel + + def _build_system_status_panel(self) -> QFrame: + panel = self._panel() + layout = QGridLayout(panel) + layout.setContentsMargins(18, 16, 18, 16) + layout.setHorizontalSpacing(18) + layout.setVerticalSpacing(10) + + title = QLabel("System Status") + title.setObjectName("PanelTitle") + layout.addWidget(title, 0, 0, 1, 2) + + self.status_fields: dict[str, QLabel] = {} + rows = [ + ("Diagnostic Source", "Mock"), + ("ROS 2 Connection", "Not connected / Future integration"), + ("Backend Heartbeat", "OK"), + ("UI Mode", "Monitoring only"), + ("Safety Note", "No motor commands sent from this interface"), + ] + for row, (label_text, value_text) in enumerate(rows, start=1): + label = QLabel(label_text) + label.setObjectName("Muted") + value = QLabel(value_text) + value.setWordWrap(True) + layout.addWidget(label, row, 0) + layout.addWidget(value, row, 1) + self.status_fields[label_text] = value + + layout.setColumnStretch(1, 1) + return panel + + def _build_telemetry_panel(self) -> QFrame: + panel = self._panel() + layout = QVBoxLayout(panel) + layout.setContentsMargins(18, 16, 18, 18) + layout.setSpacing(12) + + title = QLabel("Telemetry + Live Values") + title.setObjectName("PanelTitle") + layout.addWidget(title) + + self.telemetry_table = QTableWidget(0, 6) + self.telemetry_table.setHorizontalHeaderLabels( + ["Signal", "Status", "Value", "Unit", "Last Updated", "Message"] + ) + self.telemetry_table.setEditTriggers(QAbstractItemView.NoEditTriggers) + self.telemetry_table.setSelectionBehavior(QAbstractItemView.SelectRows) + self.telemetry_table.setFocusPolicy(Qt.NoFocus) + self.telemetry_table.verticalHeader().setVisible(False) + self.telemetry_table.horizontalHeader().setSectionResizeMode(QHeaderView.ResizeMode.Stretch) + self.telemetry_table.horizontalHeader().setMinimumSectionSize(100) + layout.addWidget(self.telemetry_table, 1) + return panel + + def _build_alerts_panel(self) -> QFrame: + self.alerts_panel = self._panel() + layout = QVBoxLayout(self.alerts_panel) + layout.setContentsMargins(18, 16, 18, 18) + layout.setSpacing(12) + + title_row = QHBoxLayout() + title = QLabel("Current Alerts") + title.setObjectName("PanelTitle") + self.alert_icon = QLabel("") + self.alert_icon.setAlignment(Qt.AlignRight | Qt.AlignVCenter) + self.alert_icon.setStyleSheet(f"font-size: 30px; color: {COLORS['fault']}; font-weight: 800;") + title_row.addWidget(title, 1) + title_row.addWidget(self.alert_icon) + layout.addLayout(title_row) + + self.alerts_container = QVBoxLayout() + self.alerts_container.setSpacing(8) + layout.addLayout(self.alerts_container) + layout.addStretch(1) + return self.alerts_panel + + def _build_placeholder_panel(self, title_text: str, body_text: str) -> QFrame: + panel = self._panel() + layout = QVBoxLayout(panel) + layout.setContentsMargins(24, 22, 24, 22) + layout.setSpacing(12) + + title = QLabel(title_text) + title.setObjectName("PanelTitle") + title.setStyleSheet("font-size: 22px;") + + body = QLabel(body_text) + body.setWordWrap(True) + body.setStyleSheet(f"color: {COLORS['blue']}; font-size: 22px; font-weight: 650;") + + scope = QLabel("Prototype scope: monitoring surface only; no control, video, or simulation backend attached.") + scope.setObjectName("Muted") + scope.setWordWrap(True) + + layout.addWidget(title) + layout.addWidget(body) + layout.addStretch(1) + layout.addWidget(scope) + return panel + + def _set_demo_mode(self, mode: str) -> None: + self.diagnostics_source.set_mode(mode) + self.refresh() + + def refresh(self) -> None: + self.current_messages = self.diagnostics_source.get_messages() + self._update_state() + self._update_system_status() + self._update_table() + self._update_alerts() + + def _update_state(self) -> None: + has_fault = any(message.status in {"FAULT", "STALE"} for message in self.current_messages) + state = "FAULT" if has_fault else "NORMAL" + self.state_label.setText(f"Current State: {state}") + self.state_label.setObjectName("StateFault" if has_fault else "StateNormal") + self.state_label.style().unpolish(self.state_label) + self.state_label.style().polish(self.state_label) + + if self.current_messages: + latest_age = min(seconds_since(message.timestamp) for message in self.current_messages) + self.last_updated_label.setText(f"Last updated: {latest_age:.1f}s ago") + else: + self.last_updated_label.setText("Last updated: --") + + def _update_system_status(self) -> None: + has_stale = any(message.status == "STALE" for message in self.current_messages) + has_fault = any(message.status == "FAULT" for message in self.current_messages) + heartbeat = "STALE" if has_stale else "OK" + heartbeat_color = STATUS_COLORS["STALE"] if has_stale else STATUS_COLORS["OK"] + + self.status_fields["Diagnostic Source"].setText(self.diagnostics_source.source_name) + self.status_fields["Backend Heartbeat"].setText(heartbeat) + self.status_fields["Backend Heartbeat"].setStyleSheet(f"color: {heartbeat_color}; font-weight: 800;") + self.status_fields["Safety Note"].setStyleSheet( + f"color: {COLORS['fault'] if has_fault else COLORS['muted']}; font-weight: 700;" + ) + + def _update_table(self) -> None: + self.telemetry_table.setRowCount(len(self.current_messages)) + + for row, message in enumerate(self.current_messages): + values = [ + message.signal_name, + message.status, + self._format_value(message.value), + message.unit or "-", + format_age(message.timestamp), + message.alert_message or "-", + ] + status_color = QColor(STATUS_COLORS[message.status]) + row_background = QColor("#170d12" if message.status == "FAULT" else "#101016" if message.status == "STALE" else "#09131d") + + for column, value in enumerate(values): + item = QTableWidgetItem(value) + item.setForeground(status_color if column == 1 else QColor(COLORS["text"])) + item.setBackground(row_background) + if column in {1, 2, 3, 4}: + item.setTextAlignment(Qt.AlignCenter) + else: + item.setTextAlignment(Qt.AlignLeft | Qt.AlignVCenter) + if message.status != "OK" and column in {0, 1, 5}: + font = item.font() + font.setBold(True) + item.setFont(font) + self.telemetry_table.setItem(row, column, item) + + def _update_alerts(self) -> None: + self._clear_layout(self.alerts_container) + alerts = [message for message in self.current_messages if message.status != "OK"] + + if not alerts: + self.alert_icon.setText("") + no_alerts = QLabel("No active alerts") + no_alerts.setStyleSheet(f"color: {COLORS['ok']}; font-size: 18px; font-weight: 700;") + self.alerts_container.addWidget(no_alerts) + return + + self.alert_icon.setText("!") + for message in alerts: + label = QLabel(self._alert_text(message)) + label.setWordWrap(True) + label.setStyleSheet( + f""" + background: rgba(255, 77, 94, 0.16); + border: 1px solid {STATUS_COLORS[message.status]}; + border-radius: 8px; + color: {COLORS['text']}; + font-size: 16px; + font-weight: 800; + padding: 10px 12px; + """ + ) + self.alerts_container.addWidget(label) + + def _clear_layout(self, layout: QVBoxLayout) -> None: + while layout.count(): + item = layout.takeAt(0) + widget = item.widget() + if widget is not None: + widget.deleteLater() + + def _alert_text(self, message: DiagnosticMessage) -> str: + if message.signal_name == "board.temperature": + return f"FAULT - Board temperature high: {self._format_value(message.value)}{message.unit or ''}" + if message.signal_name == "imu.heartbeat": + return "STALE - IMU heartbeat timeout" + return f"{message.status} - {message.signal_name}: {message.alert_message or 'Attention required'}" + + def _format_value(self, value: float | str | None) -> str: + if value is None: + return "-" + if isinstance(value, float): + return f"{value:g}" + return str(value) diff --git a/ground_station_monitoring_ui_archived/src/mock_diagnostics.py b/ground_station_monitoring_ui_archived/src/mock_diagnostics.py new file mode 100644 index 0000000..f2388fd --- /dev/null +++ b/ground_station_monitoring_ui_archived/src/mock_diagnostics.py @@ -0,0 +1,63 @@ +"""Locally generated diagnostic data for the prototype dashboard.""" + +from __future__ import annotations + +import math +import time +from dataclasses import dataclass +from typing import Protocol + +from src.diagnostics_contract import DiagnosticMessage, utc_timestamp + + +class DiagnosticsSource(Protocol): + source_name: str + + def get_messages(self) -> list[DiagnosticMessage]: + """Return normalized diagnostic messages for the UI.""" + + +@dataclass +class MockDiagnosticsSource: + """Mock source that can be switched between normal and fault demos.""" + + mode: str = "normal" + source_name: str = "Mock" + + def set_mode(self, mode: str) -> None: + if mode not in {"normal", "fault"}: + raise ValueError(f"Unsupported diagnostics mode: {mode}") + self.mode = mode + + def get_messages(self) -> list[DiagnosticMessage]: + if self.mode == "fault": + return self._fault_messages() + return self._normal_messages() + + def _normal_messages(self) -> list[DiagnosticMessage]: + pulse = math.sin(time.monotonic() / 4.0) + return [ + DiagnosticMessage("board.temperature", "OK", utc_timestamp(0.4), round(42.0 + pulse, 1), "°C"), + DiagnosticMessage("motor.current", "OK", utc_timestamp(0.5), round(0.8 + pulse * 0.05, 2), "A"), + DiagnosticMessage("imu.roll", "OK", utc_timestamp(0.2), round(1.2 + pulse * 0.1, 1), "deg"), + DiagnosticMessage("imu.pitch", "OK", utc_timestamp(0.2), round(-0.4 + pulse * 0.1, 1), "deg"), + DiagnosticMessage("imu.yaw", "OK", utc_timestamp(0.2), round(12.9 + pulse * 0.2, 1), "deg"), + ] + + def _fault_messages(self) -> list[DiagnosticMessage]: + pulse = math.sin(time.monotonic() / 3.0) + return [ + DiagnosticMessage( + "board.temperature", + "FAULT", + utc_timestamp(0.2), + round(82.0 + pulse * 0.5, 1), + "°C", + "High temperature detected", + ), + DiagnosticMessage("motor.current", "OK", utc_timestamp(0.5), 0.8, "A"), + DiagnosticMessage("imu.roll", "OK", utc_timestamp(0.2), 1.2, "deg"), + DiagnosticMessage("imu.pitch", "OK", utc_timestamp(0.2), -0.4, "deg"), + DiagnosticMessage("imu.yaw", "OK", utc_timestamp(0.2), 12.9, "deg"), + DiagnosticMessage("imu.heartbeat", "STALE", utc_timestamp(5.2), None, None, "Sensor timeout"), + ] diff --git a/ground_station_monitoring_ui_archived/src/styles.py b/ground_station_monitoring_ui_archived/src/styles.py new file mode 100644 index 0000000..239a713 --- /dev/null +++ b/ground_station_monitoring_ui_archived/src/styles.py @@ -0,0 +1,118 @@ +"""Qt styling constants for the WayBionic monitoring prototype.""" + +from __future__ import annotations + + +COLORS = { + "background": "#071019", + "panel": "#0d1722", + "panel_alt": "#101d2a", + "border": "#284052", + "text": "#e8f1f8", + "muted": "#8ea3b1", + "ok": "#3ddc84", + "warn": "#ffb020", + "fault": "#ff4d5e", + "stale": "#9aa4ad", + "blue": "#43a6ff", +} + + +STATUS_COLORS = { + "OK": COLORS["ok"], + "WARN": COLORS["warn"], + "FAULT": COLORS["fault"], + "STALE": COLORS["stale"], +} + + +APP_STYLESHEET = f""" +QMainWindow {{ + background: {COLORS["background"]}; +}} + +QWidget {{ + color: {COLORS["text"]}; + font-family: "Segoe UI", "Inter", Arial, sans-serif; + font-size: 13px; +}} + +QFrame#Panel {{ + background: {COLORS["panel"]}; + border: 1px solid {COLORS["border"]}; + border-radius: 10px; +}} + +QLabel#Title {{ + font-size: 24px; + font-weight: 700; + letter-spacing: 0.5px; +}} + +QLabel#PanelTitle {{ + font-size: 17px; + font-weight: 700; +}} + +QLabel#Muted {{ + color: {COLORS["muted"]}; +}} + +QLabel#StateNormal {{ + background: rgba(61, 220, 132, 0.16); + border: 1px solid {COLORS["ok"]}; + border-radius: 8px; + color: {COLORS["ok"]}; + font-weight: 700; + padding: 8px 12px; +}} + +QLabel#StateFault {{ + background: rgba(255, 77, 94, 0.18); + border: 1px solid {COLORS["fault"]}; + border-radius: 8px; + color: {COLORS["fault"]}; + font-weight: 800; + padding: 8px 12px; +}} + +QPushButton {{ + background: {COLORS["panel_alt"]}; + border: 1px solid {COLORS["border"]}; + border-radius: 8px; + padding: 8px 12px; + font-weight: 600; +}} + +QPushButton:hover {{ + border-color: {COLORS["blue"]}; +}} + +QPushButton:checked {{ + background: rgba(67, 166, 255, 0.18); + border-color: {COLORS["blue"]}; + color: {COLORS["text"]}; +}} + +QTableWidget {{ + background: #09131d; + border: 1px solid {COLORS["border"]}; + border-radius: 8px; + gridline-color: #1f3343; + selection-background-color: rgba(67, 166, 255, 0.25); +}} + +QHeaderView::section {{ + background: {COLORS["panel_alt"]}; + color: {COLORS["muted"]}; + border: none; + border-right: 1px solid {COLORS["border"]}; + padding: 8px; + font-weight: 700; +}} + +QTableWidget::item {{ + padding: 7px; + border-bottom: 1px solid #142535; +}} +""" diff --git a/waybionic_rviz_plugins/CMakeLists.txt b/waybionic_rviz_plugins/CMakeLists.txt new file mode 100644 index 0000000..2d8ec7f --- /dev/null +++ b/waybionic_rviz_plugins/CMakeLists.txt @@ -0,0 +1,91 @@ +cmake_minimum_required(VERSION 3.8) +project(waybionic_rviz_plugins) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +find_package(ament_cmake REQUIRED) +find_package(diagnostic_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(Qt5 REQUIRED COMPONENTS Widgets) +find_package(rclcpp REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_default_plugins REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + diagnostic_msgs + pluginlib + rclcpp + rviz_common + rviz_default_plugins +) + +add_library(${PROJECT_NAME} SHARED + include/waybionic_rviz_plugins/diagnostics_source.hpp + include/waybionic_rviz_plugins/diagnostics_panel.hpp + include/waybionic_rviz_plugins/ros_diagnostics_source.hpp + src/diagnostics_panel.cpp + src/mock_diagnostics_source.cpp + src/ros_diagnostics_source.cpp +) + +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} Qt5::Widgets) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +pluginlib_export_plugin_description_file(rviz_common plugin_description.xml) + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + DIRECTORY config launch docs + DESTINATION share/${PROJECT_NAME} +) + +install( + FILES plugin_description.xml + DESTINATION share/${PROJECT_NAME} +) + +install( + PROGRAMS scripts/temporary_diagnostics_publisher.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + find_package(ament_cmake_xmllint REQUIRED) + + ament_lint_cmake() + ament_xmllint() + + ament_add_pytest_test( + test_package_metadata + test/test_package_metadata.py + TIMEOUT 60 + ) +endif() + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/waybionic_rviz_plugins/README.md b/waybionic_rviz_plugins/README.md new file mode 100644 index 0000000..eaf0153 --- /dev/null +++ b/waybionic_rviz_plugins/README.md @@ -0,0 +1,169 @@ +# WayBionic RViz2 Diagnostics Plugin + +RViz2-native diagnostics and monitoring UI for WayBionic. This package is diagnostics-only: it provides the engineer monitoring panel, mock/live diagnostics switching, and a temporary `/diagnostics` publisher for local validation. + +Monitoring-only scope: + +- No motor commands from this package. +- No robot control or safety-critical logic. +- No camera or doctor/surgeon UI in this PR (handled separately later). +- Mock diagnostics for validation while backend `/diagnostics` is not ready. + +## Quickstart + +Use this package from any ROS 2 Jazzy colcon workspace: + +```bash +cd +source /opt/ros/jazzy/setup.bash +rosdep install --from-paths src --ignore-src -r -y +colcon build --packages-select waybionic_rviz_plugins --symlink-install +source install/setup.bash +ros2 launch waybionic_rviz_plugins engineer_view.launch.py +``` + +Run package tests: + +```bash +colcon test --packages-select waybionic_rviz_plugins +colcon test-result --verbose +``` + +## Package Layout + +```text +waybionic_rviz_plugins/ + CMakeLists.txt # Builds the shared RViz plugin library + tests + package.xml # Generic deps only; no Annin/AR4 exec deps + plugin_description.xml # Registers DiagnosticsPanel + scripts/ + temporary_diagnostics_publisher.py + include/waybionic_rviz_plugins/ + diagnostics_contract.hpp # Normalized DiagnosticMessage model + diagnostics_source.hpp # DiagnosticsSource interface + diagnostics_panel.hpp # Engineer monitoring panel + mock_diagnostics_source.hpp + ros_diagnostics_source.hpp + src/ + diagnostics_panel.cpp + mock_diagnostics_source.cpp + ros_diagnostics_source.cpp + config/ + engineer_monitoring_view.rviz # Generic engineer layout (default) + engineer_ar4_demo.rviz # Optional AR4 visualization helper + launch/ + engineer_view.launch.py + temporary_diagnostics_publisher.launch.py + engineer_ar4_demo.launch.py # Optional AR4 helper only + test/ + test_package_metadata.py + docs/ + DIAGNOSTICS_CONTRACT.md + GROUND_STATION_RVIZ_UI.md + PR_NOTES.md +``` + +### What each launch/config pair does + +| Launch | RViz config | Purpose | +|--------|-------------|---------| +| `engineer_view.launch.py` | `engineer_monitoring_view.rviz` | Generic engineer monitoring; no AR4 | +| `temporary_diagnostics_publisher.launch.py` | — | Temporary `/diagnostics` demo publisher | +| `engineer_ar4_demo.launch.py` | `engineer_ar4_demo.rviz` | Optional passive AR4 robot viz | + +Core plugin dependencies are ROS/RViz/Qt only (`rclcpp`, `rviz_common`, `diagnostic_msgs`, etc.). AR4/Annin packages are required only for the optional `engineer_ar4_demo` helper. + +## RViz Panel + +`DiagnosticsPanel` appears under **Panels → Add Panel → `waybionic_rviz_plugins`**. + +It provides engineer monitoring: telemetry table, alerts, and mock/live diagnostics switching. + +## Engineer View + +```bash +ros2 launch waybionic_rviz_plugins engineer_view.launch.py +``` + +Opens `config/engineer_monitoring_view.rviz` with the docked `WayBionic Diagnostics` panel. Does not launch Annin/AR4 packages or robot publishers. + +### Mock vs live diagnostics + +```bash +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=true +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=false +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=false diagnostics_topic:=/diagnostics +``` + +| Mode | Behavior | +|------|----------| +| Mock (`use_mock_diagnostics:=true`, default) | Uses `MockDiagnosticsSource`; `Mock Normal` / `Mock Fault` controls enabled | +| Live (`use_mock_diagnostics:=false`) | Uses `RosDiagnosticsSource`; subscribes to `diagnostic_msgs/msg/DiagnosticArray`; mock controls disabled | + +Live mode with no publisher yet shows a stable waiting state (`Waiting for messages`) instead of fake mock data. + +Panel settings are also saved in `engineer_monitoring_view.rviz` as `Use Mock Diagnostics` and `Diagnostics Topic`. + +### Temporary diagnostics publisher + +While Korede/backend publishing is unavailable, use the temporary demo publisher: + +```bash +ros2 launch waybionic_rviz_plugins temporary_diagnostics_publisher.launch.py +ros2 launch waybionic_rviz_plugins temporary_diagnostics_publisher.launch.py mode:=fault +ros2 launch waybionic_rviz_plugins temporary_diagnostics_publisher.launch.py mode:=stale +ros2 launch waybionic_rviz_plugins temporary_diagnostics_publisher.launch.py mode:=cycle +``` + +Then open live diagnostics in the engineer panel: + +```bash +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=false +``` + +Modes: + +| Mode | Behavior | +|------|----------| +| `normal` | OK telemetry for board, motor, and IMU signals | +| `fault` | High board temperature + stale IMU heartbeat | +| `stale` | All sample signals published as STALE | +| `cycle` | Rotates through normal, fault, and stale every 5 seconds | + +### Diagnostics architecture + +```text +DiagnosticsSource + MockDiagnosticsSource -> DiagnosticMessage -> DiagnosticsPanel + RosDiagnosticsSource -> DiagnosticMessage -> DiagnosticsPanel +``` + +`RosDiagnosticsSource` maps ROS diagnostic levels and fields into the internal `DiagnosticMessage` model before the Qt panel renders them. See `docs/DIAGNOSTICS_CONTRACT.md` for the full mapping Korede/backend should follow. + +## Optional AR4 Visualization Helper + +Not part of the default quickstart. Isolated for prototype review only: + +```bash +ros2 launch waybionic_rviz_plugins engineer_ar4_demo.launch.py +ros2 launch waybionic_rviz_plugins engineer_ar4_demo.launch.py ar_model:=mk3 include_gripper:=True +``` + +Requires an AR4/Annin workspace with `annin_ar4_description`, `xacro`, `robot_state_publisher`, and `joint_state_publisher`. These are not core dependencies of `waybionic_rviz_plugins`. + +## Platform Notes + +- Primary validation target is Ubuntu/WSL2 with ROS 2 Jazzy. +- A Mac/RoboStack RViz shutdown crash is not treated as a merge blocker unless it is reproduced on Ubuntu/WSL2. + +## Related Docs + +- `docs/DIAGNOSTICS_CONTRACT.md` — normalized diagnostic model and ROS mapping +- `docs/GROUND_STATION_RVIZ_UI.md` — extended architecture notes +- `docs/PR_NOTES.md` — review summary and PR description source + +## Follow-Ups + +- Test engineer layout against Harold's WayBionic placeholder robot once `origin/rebuild/waybionic-foundation` is merge-ready. +- Validate live `/diagnostics` against Korede/backend once stable publishing is available. +- Camera/doctor low-latency workflow will be handled in a separate PR. diff --git a/waybionic_rviz_plugins/config/engineer_ar4_demo.rviz b/waybionic_rviz_plugins/config/engineer_ar4_demo.rviz new file mode 100644 index 0000000..2debfa5 --- /dev/null +++ b/waybionic_rviz_plugins/config/engineer_ar4_demo.rviz @@ -0,0 +1,115 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + - Class: rviz_common/Views + Name: Views + - Class: waybionic_rviz_plugins/DiagnosticsPanel + Diagnostics Topic: /diagnostics + Name: WayBionic Diagnostics + Use Mock Diagnostics: true +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 0.4 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Name: Optional AR4 Robot Model + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 15; 22; 30 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.8 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0.2 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.55 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.4 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 995 + Hide Left Dock: false + Hide Right Dock: false + Views: + collapsed: true + WayBionic Diagnostics: + collapsed: false + Width: 1920 + X: 0 + Y: 0 diff --git a/waybionic_rviz_plugins/config/engineer_monitoring_view.rviz b/waybionic_rviz_plugins/config/engineer_monitoring_view.rviz new file mode 100644 index 0000000..574c52a --- /dev/null +++ b/waybionic_rviz_plugins/config/engineer_monitoring_view.rviz @@ -0,0 +1,115 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + - Class: rviz_common/Views + Name: Views + - Class: waybionic_rviz_plugins/DiagnosticsPanel + Diagnostics Topic: /diagnostics + Name: WayBionic Diagnostics + Use Mock Diagnostics: true +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 0.4 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Name: Robot Model + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 15; 22; 30 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.8 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0.2 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.55 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.4 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 995 + Hide Left Dock: false + Hide Right Dock: false + Views: + collapsed: true + WayBionic Diagnostics: + collapsed: false + Width: 1920 + X: 0 + Y: 0 diff --git a/waybionic_rviz_plugins/docs/DIAGNOSTICS_CONTRACT.md b/waybionic_rviz_plugins/docs/DIAGNOSTICS_CONTRACT.md new file mode 100644 index 0000000..17d5078 --- /dev/null +++ b/waybionic_rviz_plugins/docs/DIAGNOSTICS_CONTRACT.md @@ -0,0 +1,118 @@ +# Diagnostics Contract + +The RViz2 diagnostics panel consumes a normalized internal model. Mock diagnostics and live ROS 2 diagnostics both convert into this model before updating the UI. + +## Internal C++ Shape + +```cpp +enum class DiagnosticStatus +{ + Ok, + Warn, + Fault, + Stale +}; + +struct DiagnosticMessage +{ + std::string signal_name; + DiagnosticStatus status; + rclcpp::Time timestamp; + std::optional value; + std::optional unit; + std::optional alert_message; +}; +``` + +## Fields + +- `signal_name`: stable name of the monitored signal, such as `board.temperature`, `motor.current`, `imu.roll`, `imu.pitch`, `imu.yaw`, or `imu.heartbeat`. +- `status`: current health state, normalized to `OK`, `WARN`, `FAULT`, or `STALE`. +- `timestamp`: time the diagnostic was generated or last updated. +- `value`: current reading, if applicable. +- `unit`: unit for the value, such as `C`, `A`, `deg`, `V`, or empty/null. +- `alert_message`: human-readable message shown in the alerts panel when status is not `OK`. + +## Example Normal Data + +```text +board.temperature | OK | 42.0 | C | 0.4s ago | - +motor.current | OK | 0.80 | A | 0.5s ago | - +imu.roll | OK | 1.2 | deg | 0.2s ago | - +imu.pitch | OK | -0.4 | deg | 0.2s ago | - +imu.yaw | OK | 12.9 | deg | 0.2s ago | - +``` + +## Example Fault Data + +```text +board.temperature | FAULT | 82.0 | C | 0.2s ago | High temperature detected +imu.heartbeat | STALE | - | - | 5.2s ago | Sensor timeout +``` + +Other rows can remain `OK` while these fault rows generate visible alerts. + +## ROS 2 Diagnostics Mapping + +Live mode subscribes to `/diagnostics` by default. The topic can be overridden at launch: + +```bash +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=false diagnostics_topic:=/diagnostics +``` + +Message type: + +- `diagnostic_msgs/msg/DiagnosticArray` + +Mapping guidance: + +- `DiagnosticStatus.name` maps to `signal_name`. +- ROS diagnostic `OK` maps to `DiagnosticStatus::Ok`. +- ROS diagnostic `WARN` maps to `DiagnosticStatus::Warn`. +- ROS diagnostic `ERROR` maps to `DiagnosticStatus::Fault`. +- ROS diagnostic `STALE` maps to `DiagnosticStatus::Stale`. +- `DiagnosticStatus.message` maps to `alert_message` when the status is not `OK`. +- `DiagnosticStatus.values` may provide `value` and `unit` keys. If those keys are not present, the first non-empty key/value pair is shown as a value with the key as the unit/label. +- `DiagnosticArray.header.stamp` is used as the row timestamp. If it is unset, receive time is used. +- If no live message has arrived, the panel shows `Live diagnostics mode active` and `Waiting for messages` instead of mock data. +- If the latest live message is old, the panel marks rows stale. + +The Qt/RViz UI should depend on `DiagnosticMessage`, not raw ROS message internals. This keeps mock data, future backend diagnostics, and any future simulator diagnostics on the same path. + +## Backend Note + +Please publish each monitored signal with: + +- stable signal name +- status level +- timestamp +- current value, if applicable +- unit, if applicable +- alert message, if applicable + +Faults and stale signals should generate visible alerts in the engineer panel. + +## Source Switching + +Launch with mock diagnostics: + +```bash +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=true +``` + +Launch with live ROS 2 diagnostics: + +```bash +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=false +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=false diagnostics_topic:=/diagnostics +``` + +Mock mode keeps the `Mock Normal` and `Mock Fault` validation controls enabled. Live mode disables those controls and listens to the configured diagnostics topic. + +## Integration Checklist + +1. Publish live diagnostics to `/diagnostics` or pass the agreed topic as `diagnostics_topic:=`. +2. Verify that each `DiagnosticStatus` includes a stable `name`, useful `message`, and value/unit keys where applicable. +3. Launch the engineer view with `use_mock_diagnostics:=false`. +4. Verify mock normal/fault validation states and live backend diagnostics before operator use. +5. Keep robot visualization integration separate; it does not require changes to the diagnostic contract itself. diff --git a/waybionic_rviz_plugins/docs/GROUND_STATION_RVIZ_UI.md b/waybionic_rviz_plugins/docs/GROUND_STATION_RVIZ_UI.md new file mode 100644 index 0000000..5865961 --- /dev/null +++ b/waybionic_rviz_plugins/docs/GROUND_STATION_RVIZ_UI.md @@ -0,0 +1,135 @@ +# WayBionic RViz2 Ground Station UI + +This package contains the WayBionic RViz2-native diagnostics plugin and engineer monitoring layout. It keeps the operator interface inside RViz2 and avoids modifying RViz source code. + +This PR is diagnostics-only. Camera/doctor placeholder work was removed and will be handled later as a separate low-latency/VR/camera workflow. + +## Scope + +This is a monitoring-first UI package. + +- It does not send motor commands. +- It does not implement robot control. +- It does not implement safety-critical logic. +- It does not include camera or doctor/surgeon UI. +- It does not require Annin/AR4 packages for the generic engineer view. + +## Build + +```bash +cd +source /opt/ros/jazzy/setup.bash +rosdep install --from-paths src --ignore-src -r -y +colcon build --packages-select waybionic_rviz_plugins --symlink-install +source install/setup.bash +colcon test --packages-select waybionic_rviz_plugins +colcon test-result --verbose +``` + +## Engineer Monitoring View + +```bash +ros2 launch waybionic_rviz_plugins engineer_view.launch.py +``` + +This opens RViz2 with `config/engineer_monitoring_view.rviz`. The layout includes a docked `WayBionic Diagnostics` panel and generic RViz displays. It does not launch AR4 robot description packages, passive joint publishers, hardware drivers, or motor command nodes. + +Diagnostics source modes: + +```bash +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=true +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=false +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=false diagnostics_topic:=/diagnostics +``` + +- Mock mode uses `MockDiagnosticsSource` and keeps the `Mock Normal` and `Mock Fault` validation controls working. +- Live mode uses `RosDiagnosticsSource`, subscribes to the configured diagnostics topic, and disables mock controls. + +## Temporary Diagnostics Publisher + +A temporary demo publisher is included for local `/diagnostics` validation while Korede/backend publishing is unavailable: + +```bash +ros2 launch waybionic_rviz_plugins temporary_diagnostics_publisher.launch.py +ros2 launch waybionic_rviz_plugins temporary_diagnostics_publisher.launch.py mode:=cycle +``` + +Supported modes: `normal`, `fault`, `stale`, `cycle`. + +Sample signals: `board.temperature`, `motor.current`, `imu.roll`, `imu.pitch`, `imu.yaw`, `imu.heartbeat`. + +## Optional AR4 Visualization Helper + +```bash +ros2 launch waybionic_rviz_plugins engineer_ar4_demo.launch.py +``` + +This launches passive AR4 robot visualization using `annin_ar4_description`, `xacro`, `robot_state_publisher`, and `joint_state_publisher`, then opens `config/engineer_ar4_demo.rviz`. These are optional visualization dependencies, not core plugin dependencies. + +## Architecture + +```text +Generic engineer view: + engineer_view.launch.py -> engineer_monitoring_view.rviz + -> WayBionic Diagnostics panel + +Temporary backend demo: + temporary_diagnostics_publisher.launch.py -> /diagnostics + +Optional AR4 visualization helper: + engineer_ar4_demo.launch.py -> engineer_ar4_demo.rviz + -> AR4 robot_description + passive joint states + +Diagnostics flow: + DiagnosticsSource + -> MockDiagnosticsSource -> DiagnosticMessage -> DiagnosticsPanel + -> RosDiagnosticsSource -> DiagnosticMessage -> DiagnosticsPanel +``` + +RViz panel plugin: + +```text +Panels -> Add Panel -> waybionic_rviz_plugins -> DiagnosticsPanel +``` + +## Live Data Integration + +`RosDiagnosticsSource` subscribes to `/diagnostics` by default, or another topic passed as `diagnostics_topic:=`, with `diagnostic_msgs/msg/DiagnosticArray`. It maps each ROS diagnostic status into `DiagnosticMessage`, including status level, name, timestamp, value/unit fields, and alert message. See `DIAGNOSTICS_CONTRACT.md` for the exact mapping. + +The panel remains monitoring-only in both mock and live modes. + +## Platform Notes + +- Primary validation target is Ubuntu/WSL2 with ROS 2 Jazzy. +- A Mac/RoboStack RViz shutdown crash is not treated as a merge blocker unless it is reproduced on Ubuntu/WSL2. + +## Package Contents + +```text +waybionic_rviz_plugins/ + include/waybionic_rviz_plugins/ + diagnostics_contract.hpp + diagnostics_panel.hpp + diagnostics_source.hpp + mock_diagnostics_source.hpp + ros_diagnostics_source.hpp + src/ + diagnostics_panel.cpp + mock_diagnostics_source.cpp + ros_diagnostics_source.cpp + scripts/ + temporary_diagnostics_publisher.py + config/ + engineer_monitoring_view.rviz + engineer_ar4_demo.rviz + launch/ + engineer_view.launch.py + temporary_diagnostics_publisher.launch.py + engineer_ar4_demo.launch.py + test/ + test_package_metadata.py + docs/ + DIAGNOSTICS_CONTRACT.md + GROUND_STATION_RVIZ_UI.md + PR_NOTES.md +``` diff --git a/waybionic_rviz_plugins/docs/PR_NOTES.md b/waybionic_rviz_plugins/docs/PR_NOTES.md new file mode 100644 index 0000000..a2bfa98 --- /dev/null +++ b/waybionic_rviz_plugins/docs/PR_NOTES.md @@ -0,0 +1,141 @@ +# PR Notes + +## Review Screenshots + +These images are included only to help reviewers see the UI quickly. They are not part of the runtime package behavior. + +| Engineer view — mock normal | Engineer view — mock fault | +| --- | --- | +| ![Engineer mock normal](screenshots/engineer_mock_normal.png) | ![Engineer mock fault](screenshots/engineer_mock_fault.png) | + +## Summary + +This PR makes `waybionic_rviz_plugins` a focused WayBionic RViz2 diagnostics package. It keeps the engineer monitoring panel, mock/live diagnostics switching, and a temporary `/diagnostics` publisher for local validation. + +Camera/doctor placeholder work was removed from this PR and will be handled separately later. + +## Primary Launch Commands + +```bash +colcon build --packages-select waybionic_rviz_plugins --symlink-install +source install/setup.bash +ros2 launch waybionic_rviz_plugins engineer_view.launch.py +``` + +Mock diagnostics (default): + +```bash +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=true +``` + +Live diagnostics: + +```bash +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=false diagnostics_topic:=/diagnostics +``` + +Temporary backend/demo publisher: + +```bash +ros2 launch waybionic_rviz_plugins temporary_diagnostics_publisher.launch.py +ros2 launch waybionic_rviz_plugins temporary_diagnostics_publisher.launch.py mode:=cycle +``` + +## What Works + +- Engineer RViz view with the `WayBionic Diagnostics` panel. +- Mock normal and fault validation states. +- Alert banner/table rendering from normalized `DiagnosticMessage` rows. +- Live diagnostics source path that subscribes to a `diagnostic_msgs/msg/DiagnosticArray` topic. +- Temporary `/diagnostics` publisher with `normal`, `fault`, `stale`, and `cycle` modes. +- Package metadata and lint/test wiring via `colcon test`. +- Only `DiagnosticsPanel` is registered in `plugin_description.xml`. + +## Mock Diagnostics Mode + +Mock mode is enabled by default through `use_mock_diagnostics:=true`. It uses synthetic values and keeps `Mock Normal` and `Mock Fault` controls enabled for UI validation while backend `/diagnostics` publishing is not ready. + +## Live `/diagnostics` Path + +`RosDiagnosticsSource` subscribes to `/diagnostics` by default, or another topic passed as `diagnostics_topic:=`, and converts each status into the internal `DiagnosticMessage` model before the panel renders it. + +Expected mapping: + +- `DiagnosticStatus.name` -> `signal_name` +- `OK` -> `OK` +- `WARN` -> `WARN` +- `ERROR` -> `FAULT` +- `STALE` -> `STALE` +- `DiagnosticStatus.message` -> `alert_message` for non-OK rows +- `DiagnosticStatus.values` -> `value` and `unit` when those keys are present +- `DiagnosticArray.header.stamp`, or receive time when unset, -> `timestamp` + +## Temporary Diagnostics Publisher + +`scripts/temporary_diagnostics_publisher.py` publishes sample `DiagnosticArray` messages for local testing while Korede/backend publishing is unavailable. + +Modes: + +- `normal` — OK telemetry +- `fault` — high board temperature + stale IMU heartbeat +- `stale` — all sample signals STALE +- `cycle` — rotate through normal/fault/stale every 5 seconds + +## Lint / Test Wiring + +```bash +colcon test --packages-select waybionic_rviz_plugins +colcon test-result --verbose +``` + +Tests verify: + +- `DiagnosticsPanel` is registered in `plugin_description.xml` +- `SurgeonCameraPanel` is not registered +- `package.xml` has no Annin/AR4 dependency +- doctor/camera launch/config files are removed +- temporary diagnostics publisher files exist + +## Optional AR4 Visualization Helper + +Core package dependencies do not include Annin/AR4 packages. The generic launch is `engineer_view.launch.py`; it starts RViz only and does not require AR4 descriptions or passive joint publishers. + +Optional AR4 coupling is isolated in: + +- `launch/engineer_ar4_demo.launch.py` +- `config/engineer_ar4_demo.rviz` + +## Testing Performed + +```bash +source /opt/ros/jazzy/setup.bash +colcon build --packages-select waybionic_rviz_plugins --symlink-install +source install/setup.bash +colcon test --packages-select waybionic_rviz_plugins +colcon test-result --verbose +ros2 launch waybionic_rviz_plugins engineer_view.launch.py --show-args +ros2 launch waybionic_rviz_plugins temporary_diagnostics_publisher.launch.py --show-args +``` + +Optional AR4 helper, only in a workspace with AR4 dependencies: + +```bash +ros2 launch waybionic_rviz_plugins engineer_ar4_demo.launch.py --show-args +``` + +## PR Status + +Opened as https://github.com/Waybionic/waybionic_ground_station/pull/2 against `main`. Review screenshots at the top of this file are for reviewer context only. + +## Known Limitations + +- Meaningful live rows still depend on stable backend publishing once Korede is available. +- Stale handling is currently a simple five-second freshness check. +- The UI is monitoring-only and does not send motor commands. +- Camera/doctor workflow is out of scope for this PR. + +## Next Steps + +- Confirm final diagnostic signal names and value/unit conventions with backend. +- Merge diagnostics foundation, then handle camera/doctor low-latency workflow separately. +- Add focused runtime validation against Korede/backend once publishing is available. diff --git a/waybionic_rviz_plugins/docs/screenshots/engineer_mock_fault.png b/waybionic_rviz_plugins/docs/screenshots/engineer_mock_fault.png new file mode 100644 index 0000000..84ec6ea Binary files /dev/null and b/waybionic_rviz_plugins/docs/screenshots/engineer_mock_fault.png differ diff --git a/waybionic_rviz_plugins/docs/screenshots/engineer_mock_normal.png b/waybionic_rviz_plugins/docs/screenshots/engineer_mock_normal.png new file mode 100644 index 0000000..ce6b363 Binary files /dev/null and b/waybionic_rviz_plugins/docs/screenshots/engineer_mock_normal.png differ diff --git a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_contract.hpp b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_contract.hpp new file mode 100644 index 0000000..6f8fd26 --- /dev/null +++ b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_contract.hpp @@ -0,0 +1,52 @@ +#ifndef WAYBIONIC_RVIZ_PLUGINS__DIAGNOSTICS_CONTRACT_HPP_ +#define WAYBIONIC_RVIZ_PLUGINS__DIAGNOSTICS_CONTRACT_HPP_ + +#include +#include + +#include + +namespace waybionic_rviz_plugins +{ + +enum class DiagnosticStatus +{ + Ok, + Warn, + Fault, + Stale +}; + +struct DiagnosticMessage +{ + std::string signal_name; + DiagnosticStatus status; + rclcpp::Time timestamp; + std::optional value; + std::optional unit; + std::optional alert_message; +}; + +inline const char * toString(const DiagnosticStatus status) +{ + switch (status) { + case DiagnosticStatus::Ok: + return "OK"; + case DiagnosticStatus::Warn: + return "WARN"; + case DiagnosticStatus::Fault: + return "FAULT"; + case DiagnosticStatus::Stale: + return "STALE"; + } + return "UNKNOWN"; +} + +inline bool isAlertStatus(const DiagnosticStatus status) +{ + return status != DiagnosticStatus::Ok; +} + +} // namespace waybionic_rviz_plugins + +#endif // WAYBIONIC_RVIZ_PLUGINS__DIAGNOSTICS_CONTRACT_HPP_ diff --git a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_panel.hpp b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_panel.hpp new file mode 100644 index 0000000..4433841 --- /dev/null +++ b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_panel.hpp @@ -0,0 +1,87 @@ +#ifndef WAYBIONIC_RVIZ_PLUGINS__DIAGNOSTICS_PANEL_HPP_ +#define WAYBIONIC_RVIZ_PLUGINS__DIAGNOSTICS_PANEL_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "waybionic_rviz_plugins/diagnostics_contract.hpp" +#include "waybionic_rviz_plugins/diagnostics_source.hpp" +#include "waybionic_rviz_plugins/mock_diagnostics_source.hpp" + +class QButtonGroup; +class QPushButton; + +namespace waybionic_rviz_plugins +{ + +class DiagnosticsPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit DiagnosticsPanel(QWidget * parent = nullptr); + + void onInitialize() override; + void save(rviz_common::Config config) const override; + void load(const rviz_common::Config & config) override; + +private: + void buildUi(); + void configureSource(bool use_mock_diagnostics); + bool readUseMockDiagnosticsParameter(bool default_value); + std::string readDiagnosticsTopicParameter(const std::string & default_value); + void refresh(); + void setMockDiagnosticsState(MockDiagnosticsState mode); + void setUseMockDiagnostics(bool use_mock_diagnostics); + void updateSystemStatus(const std::vector & messages, const rclcpp::Time & now); + void updateTelemetryTable(const std::vector & messages, const rclcpp::Time & now); + void updateAlerts(const std::vector & messages); + void updateSourceControls(); + void clearAlerts(); + + QString statusColor(DiagnosticStatus status) const; + QString rowBackground(DiagnosticStatus status) const; + QString ageText(const rclcpp::Time & timestamp, const rclcpp::Time & now) const; + QString optionalText(const std::optional & value) const; + QString alertText(const DiagnosticMessage & message) const; + + std::unique_ptr diagnostics_source_; + MockDiagnosticsSource * mock_diagnostics_source_{nullptr}; + rclcpp::Node::SharedPtr rviz_node_; + rclcpp::Clock clock_{RCL_SYSTEM_TIME}; + std::string diagnostics_topic_{"/diagnostics"}; + bool use_mock_diagnostics_{true}; + + QTimer * refresh_timer_{nullptr}; + QCheckBox * use_mock_diagnostics_checkbox_{nullptr}; + QLabel * state_label_{nullptr}; + QLabel * last_updated_label_{nullptr}; + QLabel * source_label_{nullptr}; + QLabel * ros_connection_label_{nullptr}; + QLabel * heartbeat_label_{nullptr}; + QLabel * ui_mode_label_{nullptr}; + QLabel * safety_label_{nullptr}; + QLabel * alert_icon_label_{nullptr}; + QTableWidget * telemetry_table_{nullptr}; + QVBoxLayout * alerts_layout_{nullptr}; + QPushButton * normal_button_{nullptr}; + QPushButton * fault_button_{nullptr}; + QButtonGroup * mock_state_button_group_{nullptr}; +}; + +} // namespace waybionic_rviz_plugins + +#endif // WAYBIONIC_RVIZ_PLUGINS__DIAGNOSTICS_PANEL_HPP_ diff --git a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_source.hpp b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_source.hpp new file mode 100644 index 0000000..7114626 --- /dev/null +++ b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_source.hpp @@ -0,0 +1,26 @@ +#ifndef WAYBIONIC_RVIZ_PLUGINS__DIAGNOSTICS_SOURCE_HPP_ +#define WAYBIONIC_RVIZ_PLUGINS__DIAGNOSTICS_SOURCE_HPP_ + +#include +#include + +#include + +#include "waybionic_rviz_plugins/diagnostics_contract.hpp" + +namespace waybionic_rviz_plugins +{ + +class DiagnosticsSource +{ +public: + virtual ~DiagnosticsSource() = default; + + virtual std::string sourceName() const = 0; + virtual std::string connectionStatus(const rclcpp::Time & now) const = 0; + virtual std::vector messages(const rclcpp::Time & now) const = 0; +}; + +} // namespace waybionic_rviz_plugins + +#endif // WAYBIONIC_RVIZ_PLUGINS__DIAGNOSTICS_SOURCE_HPP_ diff --git a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/mock_diagnostics_source.hpp b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/mock_diagnostics_source.hpp new file mode 100644 index 0000000..ac94245 --- /dev/null +++ b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/mock_diagnostics_source.hpp @@ -0,0 +1,40 @@ +#ifndef WAYBIONIC_RVIZ_PLUGINS__MOCK_DIAGNOSTICS_SOURCE_HPP_ +#define WAYBIONIC_RVIZ_PLUGINS__MOCK_DIAGNOSTICS_SOURCE_HPP_ + +#include +#include + +#include + +#include "waybionic_rviz_plugins/diagnostics_contract.hpp" +#include "waybionic_rviz_plugins/diagnostics_source.hpp" + +namespace waybionic_rviz_plugins +{ + +enum class MockDiagnosticsState +{ + Normal, + Fault +}; + +class MockDiagnosticsSource : public DiagnosticsSource +{ +public: + void setMode(MockDiagnosticsState mode); + MockDiagnosticsState mode() const; + std::string sourceName() const override; + std::string connectionStatus(const rclcpp::Time & now) const override; + + std::vector messages(const rclcpp::Time & now) const override; + +private: + std::vector normalMessages(const rclcpp::Time & now) const; + std::vector faultMessages(const rclcpp::Time & now) const; + + MockDiagnosticsState mode_{MockDiagnosticsState::Normal}; +}; + +} // namespace waybionic_rviz_plugins + +#endif // WAYBIONIC_RVIZ_PLUGINS__MOCK_DIAGNOSTICS_SOURCE_HPP_ diff --git a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/ros_diagnostics_source.hpp b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/ros_diagnostics_source.hpp new file mode 100644 index 0000000..5101ba3 --- /dev/null +++ b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/ros_diagnostics_source.hpp @@ -0,0 +1,45 @@ +#ifndef WAYBIONIC_RVIZ_PLUGINS__ROS_DIAGNOSTICS_SOURCE_HPP_ +#define WAYBIONIC_RVIZ_PLUGINS__ROS_DIAGNOSTICS_SOURCE_HPP_ + +#include +#include +#include + +#include +#include + +#include "waybionic_rviz_plugins/diagnostics_source.hpp" + +namespace waybionic_rviz_plugins +{ + +class RosDiagnosticsSource : public DiagnosticsSource +{ +public: + explicit RosDiagnosticsSource( + rclcpp::Node::SharedPtr node, + std::string diagnostics_topic = "/diagnostics"); + + std::string sourceName() const override; + std::string connectionStatus(const rclcpp::Time & now) const override; + std::vector messages(const rclcpp::Time & now) const override; + +private: + void diagnosticsCallback(diagnostic_msgs::msg::DiagnosticArray::SharedPtr message); + DiagnosticMessage toDiagnosticMessage( + const diagnostic_msgs::msg::DiagnosticStatus & status, + const rclcpp::Time & timestamp) const; + + rclcpp::Node::SharedPtr node_; + std::string diagnostics_topic_; + rclcpp::Subscription::SharedPtr subscription_; + + mutable std::mutex mutex_; + std::vector latest_messages_; + rclcpp::Time last_received_time_{0, 0, RCL_SYSTEM_TIME}; + bool has_received_{false}; +}; + +} // namespace waybionic_rviz_plugins + +#endif // WAYBIONIC_RVIZ_PLUGINS__ROS_DIAGNOSTICS_SOURCE_HPP_ diff --git a/waybionic_rviz_plugins/launch/engineer_ar4_demo.launch.py b/waybionic_rviz_plugins/launch/engineer_ar4_demo.launch.py new file mode 100644 index 0000000..5954b18 --- /dev/null +++ b/waybionic_rviz_plugins/launch/engineer_ar4_demo.launch.py @@ -0,0 +1,104 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + ar_model = LaunchConfiguration("ar_model") + include_gripper = LaunchConfiguration("include_gripper") + tf_prefix = LaunchConfiguration("tf_prefix") + use_sim_time = LaunchConfiguration("use_sim_time") + use_mock_diagnostics = LaunchConfiguration("use_mock_diagnostics") + diagnostics_topic = LaunchConfiguration("diagnostics_topic") + + rviz_config = PathJoinSubstitution([ + FindPackageShare("waybionic_rviz_plugins"), + "config", + "engineer_ar4_demo.rviz", + ]) + + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution([ + FindPackageShare("annin_ar4_description"), + "urdf", + "ar.urdf.xacro", + ]), + " ", + "ar_model:=", + ar_model, + " ", + "tf_prefix:=", + tf_prefix, + " ", + "include_gripper:=", + include_gripper, + ]) + robot_description = {"robot_description": robot_description_content} + + return LaunchDescription([ + DeclareLaunchArgument( + "use_mock_diagnostics", + default_value="true", + choices=["true", "false"], + description="Use local mock diagnostics validation states instead of live ROS 2 diagnostics.", + ), + DeclareLaunchArgument( + "diagnostics_topic", + default_value="/diagnostics", + description="ROS 2 diagnostic_msgs/msg/DiagnosticArray topic used when use_mock_diagnostics is false.", + ), + DeclareLaunchArgument( + "ar_model", + default_value="mk3", + choices=["mk1", "mk2", "mk3"], + description="AR4 model to visualize for the optional AR4 helper.", + ), + DeclareLaunchArgument( + "include_gripper", + default_value="True", + choices=["True", "False"], + description="Include the gripper in the optional AR4 visualization.", + ), + DeclareLaunchArgument( + "tf_prefix", + default_value="", + description="Optional TF prefix for the robot tree.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + choices=["true", "false"], + description="Use simulation time for passive visualization.", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="waybionic_engineer_ar4_robot_state_publisher", + output="screen", + parameters=[robot_description, {"use_sim_time": ParameterValue(use_sim_time, value_type=bool)}], + ), + Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="waybionic_engineer_ar4_joint_state_publisher", + output="screen", + parameters=[{"use_sim_time": ParameterValue(use_sim_time, value_type=bool)}], + ), + Node( + package="rviz2", + executable="rviz2", + name="waybionic_engineer_ar4_rviz", + output="screen", + arguments=["-d", rviz_config], + parameters=[robot_description, { + "use_sim_time": ParameterValue(use_sim_time, value_type=bool), + "use_mock_diagnostics": ParameterValue(use_mock_diagnostics, value_type=bool), + "diagnostics_topic": ParameterValue(diagnostics_topic, value_type=str), + }], + ), + ]) diff --git a/waybionic_rviz_plugins/launch/engineer_view.launch.py b/waybionic_rviz_plugins/launch/engineer_view.launch.py new file mode 100644 index 0000000..0a02776 --- /dev/null +++ b/waybionic_rviz_plugins/launch/engineer_view.launch.py @@ -0,0 +1,50 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration("use_sim_time") + use_mock_diagnostics = LaunchConfiguration("use_mock_diagnostics") + diagnostics_topic = LaunchConfiguration("diagnostics_topic") + + rviz_config = PathJoinSubstitution([ + FindPackageShare("waybionic_rviz_plugins"), + "config", + "engineer_monitoring_view.rviz", + ]) + + return LaunchDescription([ + DeclareLaunchArgument( + "use_mock_diagnostics", + default_value="true", + choices=["true", "false"], + description="Use local mock diagnostics validation states instead of live ROS 2 diagnostics.", + ), + DeclareLaunchArgument( + "diagnostics_topic", + default_value="/diagnostics", + description="ROS 2 diagnostic_msgs/msg/DiagnosticArray topic used when use_mock_diagnostics is false.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + choices=["true", "false"], + description="Use simulation time for passive visualization.", + ), + Node( + package="rviz2", + executable="rviz2", + name="waybionic_engineer_rviz", + output="screen", + arguments=["-d", rviz_config], + parameters=[{ + "use_sim_time": ParameterValue(use_sim_time, value_type=bool), + "use_mock_diagnostics": ParameterValue(use_mock_diagnostics, value_type=bool), + "diagnostics_topic": ParameterValue(diagnostics_topic, value_type=str), + }], + ), + ]) diff --git a/waybionic_rviz_plugins/launch/temporary_diagnostics_publisher.launch.py b/waybionic_rviz_plugins/launch/temporary_diagnostics_publisher.launch.py new file mode 100644 index 0000000..97b806a --- /dev/null +++ b/waybionic_rviz_plugins/launch/temporary_diagnostics_publisher.launch.py @@ -0,0 +1,39 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'mode', + default_value='normal', + choices=['normal', 'fault', 'stale', 'cycle'], + description=( + 'Diagnostics profile to publish: normal, fault, stale, or cycle ' + '(rotate through normal/fault/stale every 5 seconds).' + ), + ), + DeclareLaunchArgument( + 'topic', + default_value='/diagnostics', + description='diagnostic_msgs/msg/DiagnosticArray topic to publish.', + ), + DeclareLaunchArgument( + 'publish_rate_hz', + default_value='2.0', + description='Publish rate in Hz.', + ), + Node( + package='waybionic_rviz_plugins', + executable='temporary_diagnostics_publisher.py', + name='temporary_diagnostics_publisher', + output='screen', + parameters=[{ + 'mode': LaunchConfiguration('mode'), + 'topic': LaunchConfiguration('topic'), + 'publish_rate_hz': LaunchConfiguration('publish_rate_hz'), + }], + ), + ]) diff --git a/waybionic_rviz_plugins/package.xml b/waybionic_rviz_plugins/package.xml new file mode 100644 index 0000000..04ed636 --- /dev/null +++ b/waybionic_rviz_plugins/package.xml @@ -0,0 +1,32 @@ + + + waybionic_rviz_plugins + 0.1.0 + RViz2 diagnostics panel and engineer monitoring layouts for WayBionic. + Khuzaymah Bin Haris + MIT + + ament_cmake + + diagnostic_msgs + pluginlib + qtbase5-dev + rclcpp + rviz_common + rviz_default_plugins + + launch + launch_ros + rclpy + rviz2 + + ament_cmake_lint_cmake + ament_cmake_pytest + ament_cmake_xmllint + python3-pytest + + + ament_cmake + + + diff --git a/waybionic_rviz_plugins/plugin_description.xml b/waybionic_rviz_plugins/plugin_description.xml new file mode 100644 index 0000000..faa00a6 --- /dev/null +++ b/waybionic_rviz_plugins/plugin_description.xml @@ -0,0 +1,10 @@ + + + + WayBionic ground station diagnostics, telemetry, and alerts panel for RViz2. + + + diff --git a/waybionic_rviz_plugins/scripts/temporary_diagnostics_publisher.py b/waybionic_rviz_plugins/scripts/temporary_diagnostics_publisher.py new file mode 100644 index 0000000..fdfb433 --- /dev/null +++ b/waybionic_rviz_plugins/scripts/temporary_diagnostics_publisher.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python3 +"""Temporary demo publisher for local /diagnostics validation.""" + +import math + +import rclpy +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue +from rclpy.node import Node + +CYCLE_MODES = ('normal', 'fault', 'stale') +CYCLE_PERIOD_SECONDS = 5.0 + + +def make_status(name, level, message='', value=None, unit=None): + status = DiagnosticStatus() + status.name = name + status.level = level + status.message = message + if value is not None: + status.values.append(KeyValue(key='value', value=str(value))) + if unit is not None: + status.values.append(KeyValue(key='unit', value=unit)) + return status + + +class TemporaryDiagnosticsPublisher(Node): + def __init__(self): + super().__init__('temporary_diagnostics_publisher') + self.declare_parameter('mode', 'normal') + self.declare_parameter('topic', '/diagnostics') + self.declare_parameter('publish_rate_hz', 2.0) + + self.mode = self.get_parameter('mode').get_parameter_value().string_value + topic = self.get_parameter('topic').get_parameter_value().string_value + rate_hz = self.get_parameter('publish_rate_hz').get_parameter_value().double_value + + if self.mode not in {'normal', 'fault', 'stale', 'cycle'}: + raise ValueError( + f"Unsupported mode '{self.mode}'. Use normal, fault, stale, or cycle." + ) + + self.publisher_ = self.create_publisher(DiagnosticArray, topic, 10) + period = 1.0 / rate_hz if rate_hz > 0.0 else 0.5 + self.timer_ = self.create_timer(period, self.publish_diagnostics) + self.cycle_start_ = self.get_clock().now() + + self.get_logger().info( + f"Publishing temporary diagnostics to {topic} in '{self.mode}' mode at {rate_hz:.1f} Hz" + ) + + def active_mode(self): + if self.mode != 'cycle': + return self.mode + + elapsed = (self.get_clock().now() - self.cycle_start_).nanoseconds / 1e9 + index = int(elapsed / CYCLE_PERIOD_SECONDS) % len(CYCLE_MODES) + return CYCLE_MODES[index] + + def publish_diagnostics(self): + message = DiagnosticArray() + message.header.stamp = self.get_clock().now().to_msg() + message.status = self.build_statuses(self.active_mode()) + self.publisher_.publish(message) + + def build_statuses(self, mode): + pulse = math.sin(self.get_clock().now().nanoseconds / 4e9) + + if mode == 'normal': + return [ + make_status( + 'board.temperature', + DiagnosticStatus.OK, + value=f'{42.0 + pulse:.1f}', + unit='C', + ), + make_status( + 'motor.current', + DiagnosticStatus.OK, + value=f'{0.8 + pulse * 0.05:.2f}', + unit='A', + ), + make_status( + 'imu.roll', + DiagnosticStatus.OK, + value=f'{1.2 + pulse * 0.1:.1f}', + unit='deg', + ), + make_status( + 'imu.pitch', + DiagnosticStatus.OK, + value=f'{-0.4 + pulse * 0.1:.1f}', + unit='deg', + ), + make_status( + 'imu.yaw', + DiagnosticStatus.OK, + value=f'{12.9 + pulse * 0.2:.1f}', + unit='deg', + ), + ] + + if mode == 'fault': + return [ + make_status( + 'board.temperature', + DiagnosticStatus.ERROR, + 'High temperature detected', + value=f'{82.0 + pulse * 0.5:.1f}', + unit='C', + ), + make_status('motor.current', DiagnosticStatus.OK, value='0.80', unit='A'), + make_status('imu.roll', DiagnosticStatus.OK, value='1.2', unit='deg'), + make_status('imu.pitch', DiagnosticStatus.OK, value='-0.4', unit='deg'), + make_status('imu.yaw', DiagnosticStatus.OK, value='12.9', unit='deg'), + make_status( + 'imu.heartbeat', + DiagnosticStatus.STALE, + 'Sensor timeout', + ), + ] + + return [ + make_status( + 'board.temperature', + DiagnosticStatus.STALE, + 'No recent board telemetry', + value='--', + unit='C', + ), + make_status( + 'motor.current', + DiagnosticStatus.STALE, + 'No recent motor telemetry', + value='--', + unit='A', + ), + make_status( + 'imu.roll', + DiagnosticStatus.STALE, + 'IMU stream stale', + value='--', + unit='deg', + ), + make_status( + 'imu.pitch', + DiagnosticStatus.STALE, + 'IMU stream stale', + value='--', + unit='deg', + ), + make_status( + 'imu.yaw', + DiagnosticStatus.STALE, + 'IMU stream stale', + value='--', + unit='deg', + ), + make_status( + 'imu.heartbeat', + DiagnosticStatus.STALE, + 'Sensor timeout', + ), + ] + + +def main(args=None): + rclpy.init(args=args) + node = TemporaryDiagnosticsPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/waybionic_rviz_plugins/src/diagnostics_panel.cpp b/waybionic_rviz_plugins/src/diagnostics_panel.cpp new file mode 100644 index 0000000..bc4eefd --- /dev/null +++ b/waybionic_rviz_plugins/src/diagnostics_panel.cpp @@ -0,0 +1,543 @@ +#include "waybionic_rviz_plugins/diagnostics_panel.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "waybionic_rviz_plugins/ros_diagnostics_source.hpp" + +namespace waybionic_rviz_plugins +{ +namespace +{ + +constexpr const char * kPanelStyle = R"( +QWidget { + background-color: #071019; + color: #e8f1f8; + font-family: "Segoe UI", "Ubuntu", sans-serif; + font-size: 12px; +} +QFrame#Card { + background-color: #0d1722; + border: 1px solid #284052; + border-radius: 8px; +} +QLabel#PanelTitle { + font-size: 15px; + font-weight: 700; +} +QLabel#Muted { + color: #8ea3b1; +} +QLabel#StateNormal { + background-color: rgba(61, 220, 132, 0.16); + border: 1px solid #3ddc84; + border-radius: 6px; + color: #3ddc84; + font-weight: 800; + padding: 6px; +} +QLabel#StateFault { + background-color: rgba(255, 77, 94, 0.18); + border: 1px solid #ff4d5e; + border-radius: 6px; + color: #ff4d5e; + font-weight: 800; + padding: 6px; +} +QPushButton { + background-color: #101d2a; + border: 1px solid #284052; + border-radius: 6px; + color: #e8f1f8; + font-weight: 700; + padding: 6px; +} +QPushButton:checked { + background-color: rgba(67, 166, 255, 0.22); + border-color: #43a6ff; +} +QTableWidget { + background-color: #09131d; + border: 1px solid #284052; + gridline-color: #1f3343; +} +QHeaderView::section { + background-color: #101d2a; + color: #8ea3b1; + border: none; + border-right: 1px solid #284052; + font-weight: 700; + padding: 5px; +} +)"; + +QLabel * makeTitle(const QString & text) +{ + auto * label = new QLabel(text); + label->setObjectName("PanelTitle"); + return label; +} + +QLabel * makeMuted(const QString & text) +{ + auto * label = new QLabel(text); + label->setObjectName("Muted"); + return label; +} + +QFrame * makeCard() +{ + auto * card = new QFrame(); + card->setObjectName("Card"); + return card; +} + +} // namespace + +DiagnosticsPanel::DiagnosticsPanel(QWidget * parent) +: rviz_common::Panel(parent) +{ + buildUi(); + configureSource(use_mock_diagnostics_); +} + +void DiagnosticsPanel::onInitialize() +{ + if (auto ros_node_abstraction = getDisplayContext()->getRosNodeAbstraction().lock()) { + rviz_node_ = ros_node_abstraction->get_raw_node(); + } + + diagnostics_topic_ = readDiagnosticsTopicParameter(diagnostics_topic_); + configureSource(readUseMockDiagnosticsParameter(use_mock_diagnostics_)); + refresh_timer_ = new QTimer(this); + connect(refresh_timer_, &QTimer::timeout, this, [this]() { refresh(); }); + refresh_timer_->start(1000); + refresh(); +} + +void DiagnosticsPanel::save(rviz_common::Config config) const +{ + rviz_common::Panel::save(config); + config.mapSetValue("Use Mock Diagnostics", use_mock_diagnostics_); + config.mapSetValue("Diagnostics Topic", QString::fromStdString(diagnostics_topic_)); +} + +void DiagnosticsPanel::load(const rviz_common::Config & config) +{ + rviz_common::Panel::load(config); + + bool use_mock_diagnostics = use_mock_diagnostics_; + QString diagnostics_topic; + if (config.mapGetString("Diagnostics Topic", &diagnostics_topic)) { + diagnostics_topic_ = diagnostics_topic.toStdString(); + } + if (config.mapGetBool("Use Mock Diagnostics", &use_mock_diagnostics)) { + configureSource(use_mock_diagnostics); + } +} + +void DiagnosticsPanel::buildUi() +{ + setStyleSheet(kPanelStyle); + setMinimumWidth(420); + + auto * root_layout = new QVBoxLayout(this); + root_layout->setContentsMargins(10, 10, 10, 10); + root_layout->setSpacing(10); + + auto * header_card = makeCard(); + auto * header_layout = new QVBoxLayout(header_card); + header_layout->setSpacing(8); + + auto * title = makeTitle("WayBionic Engineering Monitor"); + state_label_ = new QLabel("Current State: NORMAL"); + state_label_->setObjectName("StateNormal"); + last_updated_label_ = makeMuted("Last updated: --"); + + use_mock_diagnostics_checkbox_ = new QCheckBox("Use Mock Diagnostics"); + use_mock_diagnostics_checkbox_->setChecked(use_mock_diagnostics_); + use_mock_diagnostics_checkbox_->setToolTip( + "Checked: local mock validation states. Unchecked: subscribe to live ROS 2 diagnostics."); + connect(use_mock_diagnostics_checkbox_, &QCheckBox::toggled, this, [this](const bool checked) { + setUseMockDiagnostics(checked); + }); + + auto * button_row = new QHBoxLayout(); + normal_button_ = new QPushButton("Mock Normal"); + normal_button_->setCheckable(true); + normal_button_->setChecked(true); + fault_button_ = new QPushButton("Mock Fault"); + fault_button_->setCheckable(true); + + mock_state_button_group_ = new QButtonGroup(this); + mock_state_button_group_->setExclusive(true); + mock_state_button_group_->addButton(normal_button_); + mock_state_button_group_->addButton(fault_button_); + connect(normal_button_, &QPushButton::clicked, this, [this]() { + setMockDiagnosticsState(MockDiagnosticsState::Normal); + }); + connect(fault_button_, &QPushButton::clicked, this, [this]() { + setMockDiagnosticsState(MockDiagnosticsState::Fault); + }); + + button_row->addWidget(normal_button_); + button_row->addWidget(fault_button_); + + header_layout->addWidget(title); + header_layout->addWidget(use_mock_diagnostics_checkbox_); + header_layout->addLayout(button_row); + header_layout->addWidget(state_label_); + header_layout->addWidget(last_updated_label_); + root_layout->addWidget(header_card); + + auto * system_card = makeCard(); + auto * system_layout = new QGridLayout(system_card); + system_layout->setVerticalSpacing(6); + system_layout->addWidget(makeTitle("System Status"), 0, 0, 1, 2); + + source_label_ = new QLabel("Mock"); + ros_connection_label_ = new QLabel("Local mock diagnostics"); + heartbeat_label_ = new QLabel("OK"); + ui_mode_label_ = new QLabel("Monitoring only"); + safety_label_ = new QLabel("No motor commands sent from this RViz panel"); + safety_label_->setWordWrap(true); + + const std::vector> rows = { + {"Diagnostic Source", source_label_}, + {"ROS 2 Connection", ros_connection_label_}, + {"Backend Heartbeat", heartbeat_label_}, + {"UI Mode", ui_mode_label_}, + {"Safety Note", safety_label_}, + }; + + int row = 1; + for (const auto & [name, value] : rows) { + system_layout->addWidget(makeMuted(name), row, 0); + system_layout->addWidget(value, row, 1); + ++row; + } + root_layout->addWidget(system_card); + + auto * table_card = makeCard(); + auto * table_layout = new QVBoxLayout(table_card); + table_layout->addWidget(makeTitle("Telemetry + Live Values")); + + telemetry_table_ = new QTableWidget(0, 6); + telemetry_table_->setHorizontalHeaderLabels({"Signal", "Status", "Value", "Unit", "Last Updated", "Message"}); + telemetry_table_->setEditTriggers(QAbstractItemView::NoEditTriggers); + telemetry_table_->setSelectionBehavior(QAbstractItemView::SelectRows); + telemetry_table_->verticalHeader()->setVisible(false); + telemetry_table_->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); + telemetry_table_->horizontalHeader()->setMinimumSectionSize(80); + table_layout->addWidget(telemetry_table_); + root_layout->addWidget(table_card, 1); + + auto * alerts_card = makeCard(); + auto * alerts_root = new QVBoxLayout(alerts_card); + auto * alerts_title_row = new QHBoxLayout(); + alerts_title_row->addWidget(makeTitle("Current Alerts"), 1); + alert_icon_label_ = new QLabel(""); + alert_icon_label_->setStyleSheet("color: #ff4d5e; font-size: 28px; font-weight: 900;"); + alerts_title_row->addWidget(alert_icon_label_); + alerts_root->addLayout(alerts_title_row); + + alerts_layout_ = new QVBoxLayout(); + alerts_layout_->setSpacing(6); + alerts_root->addLayout(alerts_layout_); + alerts_root->addStretch(1); + root_layout->addWidget(alerts_card); +} + +void DiagnosticsPanel::configureSource(const bool use_mock_diagnostics) +{ + use_mock_diagnostics_ = use_mock_diagnostics; + mock_diagnostics_source_ = nullptr; + + if (use_mock_diagnostics_ || !rviz_node_) { + auto mock_source = std::make_unique(); + mock_diagnostics_source_ = mock_source.get(); + diagnostics_source_ = std::move(mock_source); + } else { + diagnostics_source_ = std::make_unique(rviz_node_, diagnostics_topic_); + } + + updateSourceControls(); +} + +bool DiagnosticsPanel::readUseMockDiagnosticsParameter(const bool default_value) +{ + if (!rviz_node_) { + return default_value; + } + + if (!rviz_node_->has_parameter("use_mock_diagnostics")) { + return rviz_node_->declare_parameter("use_mock_diagnostics", default_value); + } + + return rviz_node_->get_parameter("use_mock_diagnostics").as_bool(); +} + +std::string DiagnosticsPanel::readDiagnosticsTopicParameter(const std::string & default_value) +{ + if (!rviz_node_) { + return default_value; + } + + if (!rviz_node_->has_parameter("diagnostics_topic")) { + return rviz_node_->declare_parameter("diagnostics_topic", default_value); + } + + return rviz_node_->get_parameter("diagnostics_topic").as_string(); +} + +void DiagnosticsPanel::refresh() +{ + const auto now = clock_.now(); + const auto messages = diagnostics_source_->messages(now); + updateSystemStatus(messages, now); + updateTelemetryTable(messages, now); + updateAlerts(messages); +} + +void DiagnosticsPanel::setMockDiagnosticsState(const MockDiagnosticsState mode) +{ + if (mock_diagnostics_source_ == nullptr) { + return; + } + + mock_diagnostics_source_->setMode(mode); + refresh(); +} + +void DiagnosticsPanel::setUseMockDiagnostics(const bool use_mock_diagnostics) +{ + configureSource(use_mock_diagnostics); + refresh(); +} + +void DiagnosticsPanel::updateSystemStatus( + const std::vector & messages, + const rclcpp::Time & now) +{ + const bool has_alert = + std::any_of(messages.begin(), messages.end(), [](const auto & message) { + return isAlertStatus(message.status); + }); + const bool has_stale = + std::any_of(messages.begin(), messages.end(), [](const auto & message) { + return message.status == DiagnosticStatus::Stale; + }); + + const bool waiting_for_live_diagnostics = + !use_mock_diagnostics_ && messages.size() == 1 && messages.front().signal_name == "diagnostics.topic"; + + if (waiting_for_live_diagnostics) { + state_label_->setText("Current State: WAITING FOR LIVE DIAGNOSTICS"); + } else { + state_label_->setText(has_alert ? "Current State: FAULT" : "Current State: NORMAL"); + } + state_label_->setObjectName(has_alert ? "StateFault" : "StateNormal"); + state_label_->style()->unpolish(state_label_); + state_label_->style()->polish(state_label_); + + source_label_->setText(QString::fromStdString(diagnostics_source_->sourceName())); + ros_connection_label_->setText(QString::fromStdString(diagnostics_source_->connectionStatus(now))); + heartbeat_label_->setText(has_stale ? "STALE" : "OK"); + heartbeat_label_->setStyleSheet(QString("color: %1; font-weight: 800;").arg(has_stale ? "#9aa4ad" : "#3ddc84")); + safety_label_->setStyleSheet(QString("color: %1; font-weight: 700;").arg(has_alert ? "#ff4d5e" : "#8ea3b1")); + + if (messages.empty()) { + last_updated_label_->setText("Last updated: --"); + return; + } + + double latest_age = std::numeric_limits::max(); + for (const auto & message : messages) { + latest_age = std::min(latest_age, (now - message.timestamp).seconds()); + } + last_updated_label_->setText(QString("Last updated: %1s ago").arg(latest_age, 0, 'f', 1)); +} + +void DiagnosticsPanel::updateSourceControls() +{ + if (use_mock_diagnostics_checkbox_ != nullptr) { + const QSignalBlocker blocker(use_mock_diagnostics_checkbox_); + use_mock_diagnostics_checkbox_->setChecked(use_mock_diagnostics_); + } + + const bool mock_enabled = mock_diagnostics_source_ != nullptr; + if (normal_button_ != nullptr) { + normal_button_->setEnabled(mock_enabled); + } + if (fault_button_ != nullptr) { + fault_button_->setEnabled(mock_enabled); + } +} + +void DiagnosticsPanel::updateTelemetryTable( + const std::vector & messages, + const rclcpp::Time & now) +{ + telemetry_table_->setRowCount(static_cast(messages.size())); + + for (int row = 0; row < static_cast(messages.size()); ++row) { + const auto & message = messages.at(row); + const QStringList values = { + QString::fromStdString(message.signal_name), + toString(message.status), + optionalText(message.value), + optionalText(message.unit), + ageText(message.timestamp, now), + optionalText(message.alert_message), + }; + + for (int column = 0; column < values.size(); ++column) { + auto * item = new QTableWidgetItem(values.at(column)); + item->setBackground(QColor(rowBackground(message.status))); + item->setForeground(QColor(column == 1 ? statusColor(message.status) : "#e8f1f8")); + if (column == 1 || column == 2 || column == 3 || column == 4) { + item->setTextAlignment(Qt::AlignCenter); + } + if (message.status != DiagnosticStatus::Ok && (column == 0 || column == 1 || column == 5)) { + auto font = item->font(); + font.setBold(true); + item->setFont(font); + } + telemetry_table_->setItem(row, column, item); + } + } +} + +void DiagnosticsPanel::updateAlerts(const std::vector & messages) +{ + clearAlerts(); + + bool has_alert = false; + for (const auto & message : messages) { + if (!isAlertStatus(message.status)) { + continue; + } + + has_alert = true; + auto * label = new QLabel(alertText(message)); + label->setWordWrap(true); + label->setStyleSheet(QString( + "background-color: rgba(255, 77, 94, 0.18);" + "border: 1px solid %1;" + "border-radius: 6px;" + "color: #e8f1f8;" + "font-weight: 800;" + "padding: 8px;").arg(statusColor(message.status))); + alerts_layout_->addWidget(label); + } + + if (!has_alert) { + alert_icon_label_->setText(""); + auto * label = new QLabel("No active alerts"); + label->setStyleSheet("color: #3ddc84; font-size: 15px; font-weight: 800;"); + alerts_layout_->addWidget(label); + return; + } + + alert_icon_label_->setText("!"); +} + +void DiagnosticsPanel::clearAlerts() +{ + while (alerts_layout_->count() > 0) { + auto * item = alerts_layout_->takeAt(0); + if (auto * widget = item->widget()) { + widget->deleteLater(); + } + delete item; + } +} + +QString DiagnosticsPanel::statusColor(const DiagnosticStatus status) const +{ + switch (status) { + case DiagnosticStatus::Ok: + return "#3ddc84"; + case DiagnosticStatus::Warn: + return "#ffb020"; + case DiagnosticStatus::Fault: + return "#ff4d5e"; + case DiagnosticStatus::Stale: + return "#9aa4ad"; + } + return "#e8f1f8"; +} + +QString DiagnosticsPanel::rowBackground(const DiagnosticStatus status) const +{ + switch (status) { + case DiagnosticStatus::Fault: + return "#241018"; + case DiagnosticStatus::Warn: + return "#241d0d"; + case DiagnosticStatus::Stale: + return "#151922"; + case DiagnosticStatus::Ok: + return "#09131d"; + } + return "#09131d"; +} + +QString DiagnosticsPanel::ageText(const rclcpp::Time & timestamp, const rclcpp::Time & now) const +{ + const double age_seconds = std::max(0.0, (now - timestamp).seconds()); + return QString("%1s ago").arg(age_seconds, 0, 'f', 1); +} + +QString DiagnosticsPanel::optionalText(const std::optional & value) const +{ + if (!value.has_value() || value->empty()) { + return "-"; + } + return QString::fromStdString(*value); +} + +QString DiagnosticsPanel::alertText(const DiagnosticMessage & message) const +{ + if (message.signal_name == "board.temperature") { + return QString("FAULT - Board temperature high: %1 %2") + .arg(optionalText(message.value), optionalText(message.unit)); + } + + if (message.signal_name == "imu.heartbeat") { + return "STALE - IMU heartbeat timeout"; + } + + return QString("%1 - %2: %3") + .arg(toString(message.status)) + .arg(QString::fromStdString(message.signal_name)) + .arg(optionalText(message.alert_message)); +} + +} // namespace waybionic_rviz_plugins + +PLUGINLIB_EXPORT_CLASS(waybionic_rviz_plugins::DiagnosticsPanel, rviz_common::Panel) diff --git a/waybionic_rviz_plugins/src/mock_diagnostics_source.cpp b/waybionic_rviz_plugins/src/mock_diagnostics_source.cpp new file mode 100644 index 0000000..54b2e58 --- /dev/null +++ b/waybionic_rviz_plugins/src/mock_diagnostics_source.cpp @@ -0,0 +1,90 @@ +#include "waybionic_rviz_plugins/mock_diagnostics_source.hpp" + +#include +#include +#include +#include +#include +#include + +namespace waybionic_rviz_plugins +{ +namespace +{ + +rclcpp::Time secondsAgo(const rclcpp::Time & now, const double seconds) +{ + const auto nanoseconds = static_cast(seconds * 1'000'000'000.0); + return now - rclcpp::Duration::from_nanoseconds(nanoseconds); +} + +std::string formatNumber(const double value, const int precision) +{ + char buffer[32]; + std::snprintf(buffer, sizeof(buffer), "%.*f", precision, value); + return std::string(buffer); +} + +} // namespace + +void MockDiagnosticsSource::setMode(const MockDiagnosticsState mode) +{ + mode_ = mode; +} + +MockDiagnosticsState MockDiagnosticsSource::mode() const +{ + return mode_; +} + +std::string MockDiagnosticsSource::sourceName() const +{ + return "Mock"; +} + +std::string MockDiagnosticsSource::connectionStatus(const rclcpp::Time & /*now*/) const +{ + return "Local mock diagnostics"; +} + +std::vector MockDiagnosticsSource::messages(const rclcpp::Time & now) const +{ + if (mode_ == MockDiagnosticsState::Fault) { + return faultMessages(now); + } + return normalMessages(now); +} + +std::vector MockDiagnosticsSource::normalMessages(const rclcpp::Time & now) const +{ + const double pulse = std::sin(now.seconds() / 4.0); + return { + {"board.temperature", DiagnosticStatus::Ok, secondsAgo(now, 0.4), formatNumber(42.0 + pulse, 1), "C", std::nullopt}, + {"motor.current", DiagnosticStatus::Ok, secondsAgo(now, 0.5), formatNumber(0.8 + pulse * 0.05, 2), "A", std::nullopt}, + {"imu.roll", DiagnosticStatus::Ok, secondsAgo(now, 0.2), formatNumber(1.2 + pulse * 0.1, 1), "deg", std::nullopt}, + {"imu.pitch", DiagnosticStatus::Ok, secondsAgo(now, 0.2), formatNumber(-0.4 + pulse * 0.1, 1), "deg", std::nullopt}, + {"imu.yaw", DiagnosticStatus::Ok, secondsAgo(now, 0.2), formatNumber(12.9 + pulse * 0.2, 1), "deg", std::nullopt}, + }; +} + +std::vector MockDiagnosticsSource::faultMessages(const rclcpp::Time & now) const +{ + const double pulse = std::sin(now.seconds() / 3.0); + return { + { + "board.temperature", + DiagnosticStatus::Fault, + secondsAgo(now, 0.2), + formatNumber(82.0 + pulse * 0.5, 1), + "C", + "High temperature detected", + }, + {"motor.current", DiagnosticStatus::Ok, secondsAgo(now, 0.5), "0.80", "A", std::nullopt}, + {"imu.roll", DiagnosticStatus::Ok, secondsAgo(now, 0.2), "1.2", "deg", std::nullopt}, + {"imu.pitch", DiagnosticStatus::Ok, secondsAgo(now, 0.2), "-0.4", "deg", std::nullopt}, + {"imu.yaw", DiagnosticStatus::Ok, secondsAgo(now, 0.2), "12.9", "deg", std::nullopt}, + {"imu.heartbeat", DiagnosticStatus::Stale, secondsAgo(now, 5.2), std::nullopt, std::nullopt, "Sensor timeout"}, + }; +} + +} // namespace waybionic_rviz_plugins diff --git a/waybionic_rviz_plugins/src/ros_diagnostics_source.cpp b/waybionic_rviz_plugins/src/ros_diagnostics_source.cpp new file mode 100644 index 0000000..a16898a --- /dev/null +++ b/waybionic_rviz_plugins/src/ros_diagnostics_source.cpp @@ -0,0 +1,174 @@ +#include "waybionic_rviz_plugins/ros_diagnostics_source.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +namespace waybionic_rviz_plugins +{ +namespace +{ + +constexpr double kStaleAfterSeconds = 5.0; + +DiagnosticStatus mapLevel(const unsigned char level) +{ + switch (level) { + case diagnostic_msgs::msg::DiagnosticStatus::OK: + return DiagnosticStatus::Ok; + case diagnostic_msgs::msg::DiagnosticStatus::WARN: + return DiagnosticStatus::Warn; + case diagnostic_msgs::msg::DiagnosticStatus::ERROR: + return DiagnosticStatus::Fault; + case diagnostic_msgs::msg::DiagnosticStatus::STALE: + return DiagnosticStatus::Stale; + default: + return DiagnosticStatus::Warn; + } +} + +std::string lowerCopy(std::string value) +{ + std::transform(value.begin(), value.end(), value.begin(), [](const unsigned char character) { + return static_cast(std::tolower(character)); + }); + return value; +} + +bool hasContent(const std::string & value) +{ + return !value.empty(); +} + +} // namespace + +RosDiagnosticsSource::RosDiagnosticsSource( + rclcpp::Node::SharedPtr node, + std::string diagnostics_topic) +: node_(std::move(node)), + diagnostics_topic_(std::move(diagnostics_topic)) +{ + subscription_ = node_->create_subscription( + diagnostics_topic_, + rclcpp::QoS(10), + [this](diagnostic_msgs::msg::DiagnosticArray::SharedPtr message) { + diagnosticsCallback(std::move(message)); + }); +} + +std::string RosDiagnosticsSource::sourceName() const +{ + return "ROS " + diagnostics_topic_; +} + +std::string RosDiagnosticsSource::connectionStatus(const rclcpp::Time & now) const +{ + std::lock_guard lock(mutex_); + if (!has_received_) { + return "Waiting for " + diagnostics_topic_; + } + + const double age_seconds = std::max(0.0, (now - last_received_time_).seconds()); + if (age_seconds > kStaleAfterSeconds) { + return "No recent messages on " + diagnostics_topic_; + } + + return "Connected to " + diagnostics_topic_; +} + +std::vector RosDiagnosticsSource::messages(const rclcpp::Time & now) const +{ + std::lock_guard lock(mutex_); + if (!has_received_) { + return {{ + "diagnostics.topic", + DiagnosticStatus::Stale, + now, + std::nullopt, + std::nullopt, + "Live diagnostics mode active; waiting for " + diagnostics_topic_ + " messages", + }}; + } + + auto messages = latest_messages_; + const double age_seconds = std::max(0.0, (now - last_received_time_).seconds()); + if (age_seconds <= kStaleAfterSeconds) { + return messages; + } + + for (auto & message : messages) { + if (message.status == DiagnosticStatus::Ok || message.status == DiagnosticStatus::Warn) { + message.status = DiagnosticStatus::Stale; + message.alert_message = "No recent update from " + diagnostics_topic_; + } + } + return messages; +} + +void RosDiagnosticsSource::diagnosticsCallback( + diagnostic_msgs::msg::DiagnosticArray::SharedPtr message) +{ + rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + const auto received_at = system_clock.now(); + rclcpp::Time timestamp(message->header.stamp, RCL_SYSTEM_TIME); + if (timestamp.nanoseconds() == 0) { + timestamp = received_at; + } + + std::vector normalized_messages; + normalized_messages.reserve(message->status.size()); + for (const auto & status : message->status) { + normalized_messages.push_back(toDiagnosticMessage(status, timestamp)); + } + + std::lock_guard lock(mutex_); + latest_messages_ = std::move(normalized_messages); + last_received_time_ = received_at; + has_received_ = true; +} + +DiagnosticMessage RosDiagnosticsSource::toDiagnosticMessage( + const diagnostic_msgs::msg::DiagnosticStatus & status, + const rclcpp::Time & timestamp) const +{ + std::optional value; + std::optional unit; + + for (const auto & key_value : status.values) { + const auto key = lowerCopy(key_value.key); + if (key == "value") { + value = key_value.value; + continue; + } + if (key == "unit") { + unit = key_value.value; + continue; + } + if (!value.has_value() && hasContent(key_value.value)) { + value = key_value.value; + unit = key_value.key; + } + } + + const auto normalized_status = mapLevel(status.level); + std::optional alert_message; + if (hasContent(status.message) && normalized_status != DiagnosticStatus::Ok) { + alert_message = status.message; + } + + return { + status.name, + normalized_status, + timestamp, + value, + unit, + alert_message, + }; +} + +} // namespace waybionic_rviz_plugins diff --git a/waybionic_rviz_plugins/test/test_package_metadata.py b/waybionic_rviz_plugins/test/test_package_metadata.py new file mode 100644 index 0000000..4b3b8e4 --- /dev/null +++ b/waybionic_rviz_plugins/test/test_package_metadata.py @@ -0,0 +1,37 @@ +from pathlib import Path + + +PACKAGE_ROOT = Path(__file__).resolve().parent.parent + + +def read_text(relative_path: str) -> str: + return (PACKAGE_ROOT / relative_path).read_text(encoding='utf-8') + + +def test_plugin_xml_registers_diagnostics_panel(): + plugin_xml = read_text('plugin_description.xml') + assert 'waybionic_rviz_plugins/DiagnosticsPanel' in plugin_xml + assert 'DiagnosticsPanel' in plugin_xml + + +def test_plugin_xml_does_not_register_surgeon_panel(): + plugin_xml = read_text('plugin_description.xml') + assert 'SurgeonCameraPanel' not in plugin_xml + + +def test_package_xml_has_no_annin_or_ar4_dependency(): + package_xml = read_text('package.xml').lower() + assert 'annin' not in package_xml + assert 'ar4' not in package_xml + + +def test_doctor_camera_files_removed(): + assert not (PACKAGE_ROOT / 'launch' / 'doctor_view.launch.py').exists() + assert not (PACKAGE_ROOT / 'config' / 'doctor_camera_view.rviz').exists() + assert not (PACKAGE_ROOT / 'include' / 'waybionic_rviz_plugins' / 'surgeon_camera_panel.hpp').exists() + assert not (PACKAGE_ROOT / 'src' / 'surgeon_camera_panel.cpp').exists() + + +def test_temporary_diagnostics_publisher_exists(): + assert (PACKAGE_ROOT / 'scripts' / 'temporary_diagnostics_publisher.py').exists() + assert (PACKAGE_ROOT / 'launch' / 'temporary_diagnostics_publisher.launch.py').exists()