first commit
This commit is contained in:
18
.gitignore
vendored
Normal file
18
.gitignore
vendored
Normal file
@@ -0,0 +1,18 @@
|
|||||||
|
# Python
|
||||||
|
__pycache__/
|
||||||
|
*.py[cod]
|
||||||
|
*.pyo
|
||||||
|
*.pyd
|
||||||
|
*.egg-info/
|
||||||
|
dist/
|
||||||
|
build/
|
||||||
|
.venv/
|
||||||
|
venv/
|
||||||
|
env/
|
||||||
|
|
||||||
|
# JetBrains IDE
|
||||||
|
.idea/
|
||||||
|
|
||||||
|
# macOS
|
||||||
|
.DS_Store
|
||||||
|
|
||||||
201
README.md
Normal file
201
README.md
Normal file
@@ -0,0 +1,201 @@
|
|||||||
|
# xMonitor — Robot Status Monitor
|
||||||
|
|
||||||
|
A real-time web dashboard for monitoring a ROS 2 robot's motor, arm, waist, and power status.
|
||||||
|
Data is streamed from the robot controller over WebSocket and displayed in a browser.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## Architecture
|
||||||
|
|
||||||
|
```
|
||||||
|
Robot Controller (ROS 2) Monitor Server (PC / Server)
|
||||||
|
┌──────────────────────────┐ ┌──────────────────────────┐
|
||||||
|
│ monitor_sender.py │ WebSocket │ main_monitor.py │
|
||||||
|
│ ─────────────────────── │─────────────▶│ (FastAPI + uvicorn) │
|
||||||
|
│ Subscribes: │ /ws/robot │ │
|
||||||
|
│ • /leg/status │ │ Broadcasts to browsers │
|
||||||
|
│ • /arm/status │ │ via /ws │
|
||||||
|
│ • /waist/status │ └────────────┬─────────────┘
|
||||||
|
│ • /power/battery/status │ │ WebSocket /ws
|
||||||
|
│ • /leg/motor_status │ ▼
|
||||||
|
│ • /arm/motor_status │ ┌──────────────────────────┐
|
||||||
|
│ • /waist/motor_status │ │ Browser (any device) │
|
||||||
|
│ │ │ http://<server-ip>:8000 │
|
||||||
|
│ Sends JSON every 1 s │ └──────────────────────────┘
|
||||||
|
└──────────────────────────┘
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## Requirements
|
||||||
|
|
||||||
|
### Monitor Server (PC / any host)
|
||||||
|
|
||||||
|
| Package | Purpose |
|
||||||
|
|---------|---------|
|
||||||
|
| Python ≥ 3.9 | Runtime |
|
||||||
|
| `fastapi` | Web framework |
|
||||||
|
| `uvicorn` | ASGI server |
|
||||||
|
| `websockets` | (used by sender; not needed on server) |
|
||||||
|
|
||||||
|
```bash
|
||||||
|
pip install fastapi uvicorn
|
||||||
|
```
|
||||||
|
|
||||||
|
### Robot Controller (ROS 2 node)
|
||||||
|
|
||||||
|
| Package | Purpose |
|
||||||
|
|---------|---------|
|
||||||
|
| Python ≥ 3.9 | Runtime |
|
||||||
|
| ROS 2 (Humble / Iron / …) | Middleware |
|
||||||
|
| `bodyctrl_msgs` | Custom message package |
|
||||||
|
| `websockets` | WebSocket client |
|
||||||
|
| `rclpy` | ROS 2 Python client |
|
||||||
|
|
||||||
|
```bash
|
||||||
|
pip install websockets
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## Monitor Server — `main_monitor.py`
|
||||||
|
|
||||||
|
### Start
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# Default: listens on all interfaces, port 8000
|
||||||
|
python main_monitor.py
|
||||||
|
|
||||||
|
# Or with uvicorn directly (supports --reload for development)
|
||||||
|
uvicorn main_monitor:app --host 0.0.0.0 --port 8000
|
||||||
|
```
|
||||||
|
|
||||||
|
### WebSocket endpoints
|
||||||
|
|
||||||
|
| Endpoint | Direction | Description |
|
||||||
|
|----------|-----------|-------------|
|
||||||
|
| `GET /` | HTTP | Returns the web dashboard HTML |
|
||||||
|
| `WS /ws` | Server → Browser | Pushes robot status to every connected browser |
|
||||||
|
| `WS /ws/robot` | Robot → Server | Receives JSON from `monitor_sender.py` |
|
||||||
|
|
||||||
|
### Web GUI
|
||||||
|
|
||||||
|
Open `http://<server-ip>:8000` in any browser.
|
||||||
|
|
||||||
|
The dashboard shows:
|
||||||
|
|
||||||
|
- **Connection status** indicator (green / red dot)
|
||||||
|
- **Last update timestamp**
|
||||||
|
- **Power panel** (two rows):
|
||||||
|
- 主电池 (Master battery): voltage (V), current (A), SOC (%)
|
||||||
|
- 副电池 (Secondary battery): voltage (V), current (A), SOC (%)
|
||||||
|
- Cards turn **orange** (warn) or **red** (alert) when voltage / SOC drops low
|
||||||
|
- **Motor status table** with columns:
|
||||||
|
`ID | Pos (rad) | Speed | Current (A) | Temp (°C) | Motor Temp | MOS Temp | Error`
|
||||||
|
- Groups: 左臂 11-14, 右臂 21-24, 腰部 31-33, 左腿 51-56, 右腿 61-66
|
||||||
|
- Temperature cells turn **yellow** when > 100 °C, **red** when > 120 °C
|
||||||
|
- Rows with errors are highlighted red; error column shows `✓` when OK
|
||||||
|
- Browser auto-reconnects every 3 s if the WebSocket drops
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## Robot Sender — `monitor_sender.py`
|
||||||
|
|
||||||
|
Run this on the robot controller where ROS 2 is running.
|
||||||
|
|
||||||
|
### Usage
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python3 monitor_sender.py --ip <server-ip> [--port <port>]
|
||||||
|
```
|
||||||
|
|
||||||
|
| Argument | Default | Description |
|
||||||
|
|----------|---------|-------------|
|
||||||
|
| `--ip` | `10.11.24.86` | IP address of the monitor server |
|
||||||
|
| `--port` | `8000` | TCP port of the monitor server |
|
||||||
|
|
||||||
|
### Examples
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# Connect to server at 192.168.1.100 on default port 8000
|
||||||
|
python3 monitor_sender.py --ip 192.168.1.100
|
||||||
|
|
||||||
|
# Connect to server at 10.0.0.5 on port 9000
|
||||||
|
python3 monitor_sender.py --ip 10.0.0.5 --port 9000
|
||||||
|
```
|
||||||
|
|
||||||
|
### Subscribed ROS 2 topics
|
||||||
|
|
||||||
|
| Topic | Message Type | Description |
|
||||||
|
|-------|-------------|-------------|
|
||||||
|
| `/leg/status` | `bodyctrl_msgs/MotorStatusMsg` | Leg motor status (pos / speed / current / temp / error) |
|
||||||
|
| `/arm/status` | `bodyctrl_msgs/MotorStatusMsg` | Arm motor status |
|
||||||
|
| `/waist/status` | `bodyctrl_msgs/MotorStatusMsg` | Waist motor status |
|
||||||
|
| `/leg/motor_status` | `bodyctrl_msgs/MotorStatusMsg1` | Leg motor & MOS temperatures |
|
||||||
|
| `/arm/motor_status` | `bodyctrl_msgs/MotorStatusMsg1` | Arm motor & MOS temperatures |
|
||||||
|
| `/waist/motor_status` | `bodyctrl_msgs/MotorStatusMsg1` | Waist motor & MOS temperatures |
|
||||||
|
| `/power/battery/status` | `bodyctrl_msgs/PowerBatteryStatus` | Battery voltages, currents, SOC |
|
||||||
|
|
||||||
|
### Behaviour
|
||||||
|
|
||||||
|
- All topics are sampled once per second (1 Hz) via a ROS 2 timer, reducing network overhead.
|
||||||
|
- Data from all topics is merged into a single JSON payload and sent over WebSocket.
|
||||||
|
- Motor temperature (`motortemperature`) and MOS temperature (`mostemperature`) from `MotorStatusMsg1` are merged into the corresponding motor entry by `name` ID.
|
||||||
|
- If the WebSocket connection to the server drops, the sender automatically retries every 3 s and clears the internal queue to avoid stale data.
|
||||||
|
|
||||||
|
### JSON payload format
|
||||||
|
|
||||||
|
```json
|
||||||
|
{
|
||||||
|
"timestamp": "2026-04-05T10:00:00+00:00",
|
||||||
|
"statuses": [
|
||||||
|
{
|
||||||
|
"name": 51,
|
||||||
|
"pos": -0.2508,
|
||||||
|
"speed": -0.0051,
|
||||||
|
"current": -0.0488,
|
||||||
|
"temperature": 30.0,
|
||||||
|
"motor_temp": 28.5,
|
||||||
|
"mos_temp": 31.2,
|
||||||
|
"error": 0
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"power": {
|
||||||
|
"voltage": 52.30,
|
||||||
|
"current": -2.60,
|
||||||
|
"power": 100.0,
|
||||||
|
"little_voltage": 53.10,
|
||||||
|
"little_current": -0.10,
|
||||||
|
"little_power": 94.0
|
||||||
|
}
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## Quick-start (end-to-end)
|
||||||
|
|
||||||
|
1. **On the monitor server** (PC on the same LAN):
|
||||||
|
```bash
|
||||||
|
pip install fastapi uvicorn
|
||||||
|
python main_monitor.py
|
||||||
|
```
|
||||||
|
|
||||||
|
2. **On the robot controller** (source your ROS 2 workspace first):
|
||||||
|
```bash
|
||||||
|
source /home/ubuntu/ros2ws/install/setup.bash
|
||||||
|
python3 monitor_sender.py --ip <your-server-ip>
|
||||||
|
```
|
||||||
|
|
||||||
|
3. **Open browser**: navigate to `http://<your-server-ip>:8000`
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## File Overview
|
||||||
|
|
||||||
|
| File | Description |
|
||||||
|
|------|-------------|
|
||||||
|
| `main_monitor.py` | FastAPI server — hosts the web GUI and relays data to browsers |
|
||||||
|
| `monitor_sender.py` | ROS 2 node — collects robot data and streams it to the server |
|
||||||
|
| `README.md` | This file |
|
||||||
|
|
||||||
305
main_monitor.py
Normal file
305
main_monitor.py
Normal file
@@ -0,0 +1,305 @@
|
|||||||
|
# python
|
||||||
|
import asyncio
|
||||||
|
from datetime import datetime, timezone
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import List
|
||||||
|
|
||||||
|
from fastapi import FastAPI, WebSocket, WebSocketDisconnect
|
||||||
|
from fastapi.responses import HTMLResponse
|
||||||
|
|
||||||
|
app = FastAPI()
|
||||||
|
|
||||||
|
# 简单内存管理：保存浏览器客户端
|
||||||
|
browser_clients: List[WebSocket] = []
|
||||||
|
# 可选：保存最近一条状态
|
||||||
|
last_status = None
|
||||||
|
|
||||||
|
LOG_DIR = Path(__file__).resolve().parent / "logs"
|
||||||
|
PACKET_LOG_PATH = LOG_DIR / "robot_packets.jsonl"
|
||||||
|
|
||||||
|
INDEX_HTML = """
|
||||||
|
<!doctype html>
|
||||||
|
<html>
|
||||||
|
<head>
|
||||||
|
<meta charset="utf-8"/>
|
||||||
|
<title>Robot Status Monitor</title>
|
||||||
|
<style>
|
||||||
|
body { font-family: Arial, sans-serif; padding: 20px; background: #f5f5f5; }
|
||||||
|
h2 { margin-bottom: 5px; }
|
||||||
|
.ts { color: #666; font-size: 0.9em; margin-bottom: 10px; }
|
||||||
|
table { border-collapse: collapse; width: 100%; max-width: 1000px; margin-top: 10px; }
|
||||||
|
th { background: #4a90d9; color: white; padding: 10px 8px; text-align: center; }
|
||||||
|
td { border: 1px solid #ccc; padding: 8px; text-align: center; font-family: monospace; }
|
||||||
|
tr.even { background: #fff; }
|
||||||
|
tr.odd { background: #f0f0f0; }
|
||||||
|
tr.error { background: #ffe0e0 !important; }
|
||||||
|
.group-header { background: #ddd; font-weight: bold; text-align: left; padding-left: 12px; }
|
||||||
|
.group-header.arm { background: #d0e8ff; }
|
||||||
|
.group-header.waist { background: #d6f5d6; }
|
||||||
|
.ok { color: green; }
|
||||||
|
.bad { color: red; font-weight: bold; }
|
||||||
|
.temp-yellow { color: #e6a800; font-weight: bold; }
|
||||||
|
.temp-red { color: red; font-weight: bold; }
|
||||||
|
#status-dot { display: inline-block; width: 10px; height: 10px; border-radius: 50%; margin-right: 6px; }
|
||||||
|
#status-dot.ok { background: #4caf50; }
|
||||||
|
#status-dot.bad { background: #f44336; }
|
||||||
|
.power-panel { display: flex; gap: 24px; margin: 12px 0 18px 0; flex-wrap: wrap; }
|
||||||
|
.power-card { background: #fff; border: 1px solid #ccc; border-radius: 8px; padding: 14px 24px; min-width: 160px; text-align: center; box-shadow: 0 1px 3px rgba(0,0,0,.1); }
|
||||||
|
.power-card .label { font-size: 0.85em; color: #666; margin-bottom: 4px; }
|
||||||
|
.power-card .value { font-size: 1.6em; font-weight: bold; font-family: monospace; color: #333; }
|
||||||
|
.power-card.warn .value { color: #e67e00; }
|
||||||
|
.power-card.alert .value { color: #e00; }
|
||||||
|
</style>
|
||||||
|
</head>
|
||||||
|
<body>
|
||||||
|
<h2>🤖 Robot Status Monitor</h2>
|
||||||
|
<p><span id="status-dot" class="bad"></span>WebSocket: <span id="status">连接中...</span></p>
|
||||||
|
<p class="ts">最近更新: <span id="ts">-</span></p>
|
||||||
|
|
||||||
|
<div class="power-panel">
|
||||||
|
<div class="power-card" id="pwr-voltage">
|
||||||
|
<div class="label">⚡ 主电池电压</div>
|
||||||
|
<div class="value" id="pwr-voltage-val">- V</div>
|
||||||
|
</div>
|
||||||
|
<div class="power-card" id="pwr-current">
|
||||||
|
<div class="label">🔋 主电池电流</div>
|
||||||
|
<div class="value" id="pwr-current-val">- A</div>
|
||||||
|
</div>
|
||||||
|
<div class="power-card" id="pwr-power">
|
||||||
|
<div class="label">📊 主电池电量</div>
|
||||||
|
<div class="value" id="pwr-power-val">- %</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
<div class="power-panel">
|
||||||
|
<div class="power-card" id="pwr-little-voltage">
|
||||||
|
<div class="label">⚡ 副电池电压</div>
|
||||||
|
<div class="value" id="pwr-little-voltage-val">- V</div>
|
||||||
|
</div>
|
||||||
|
<div class="power-card" id="pwr-little-current">
|
||||||
|
<div class="label">🔋 副电池电流</div>
|
||||||
|
<div class="value" id="pwr-little-current-val">- A</div>
|
||||||
|
</div>
|
||||||
|
<div class="power-card" id="pwr-little-power">
|
||||||
|
<div class="label">📊 副电池电量</div>
|
||||||
|
<div class="value" id="pwr-little-power-val">- %</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<table>
|
||||||
|
<thead>
|
||||||
|
<tr>
|
||||||
|
<th>ID</th><th>Pos (rad)</th><th>Speed</th><th>Current (A)</th><th>Temp (°C)</th><th>Motor Temp</th><th>MOS Temp</th><th>Error</th>
|
||||||
|
</tr>
|
||||||
|
</thead>
|
||||||
|
<tbody id="tbody">
|
||||||
|
<!-- 左臂 11-14 -->
|
||||||
|
<tr><td colspan="8" class="group-header arm">左臂 (11-14)</td></tr>
|
||||||
|
<tr id="r11"><td>11</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r12"><td>12</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r13"><td>13</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r14"><td>14</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<!-- 右臂 21-24 -->
|
||||||
|
<tr><td colspan="8" class="group-header arm">右臂 (21-24)</td></tr>
|
||||||
|
<tr id="r21"><td>21</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r22"><td>22</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r23"><td>23</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r24"><td>24</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<!-- 腰部 31-33 -->
|
||||||
|
<tr><td colspan="8" class="group-header waist">腰部 (31-33)</td></tr>
|
||||||
|
<tr id="r31"><td>31</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r32"><td>32</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r33"><td>33</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<!-- 左腿 51-56 -->
|
||||||
|
<tr><td colspan="8" class="group-header">左腿 (51-56)</td></tr>
|
||||||
|
<tr id="r51"><td>51</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r52"><td>52</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r53"><td>53</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r54"><td>54</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r55"><td>55</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r56"><td>56</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<!-- 右腿 61-66 -->
|
||||||
|
<tr><td colspan="8" class="group-header">右腿 (61-66)</td></tr>
|
||||||
|
<tr id="r61"><td>61</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r62"><td>62</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r63"><td>63</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r64"><td>64</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r65"><td>65</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
<tr id="r66"><td>66</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr>
|
||||||
|
</tbody>
|
||||||
|
</table>
|
||||||
|
|
||||||
|
<script>
|
||||||
|
const statusSpan = document.getElementById('status');
|
||||||
|
const statusDot = document.getElementById('status-dot');
|
||||||
|
const tsSpan = document.getElementById('ts');
|
||||||
|
|
||||||
|
const WS_HOST = location.hostname;
|
||||||
|
const WS_PORT = location.port || '8000';
|
||||||
|
let ws;
|
||||||
|
|
||||||
|
function connectWS() {
|
||||||
|
ws = new WebSocket('ws://' + WS_HOST + ':' + WS_PORT + '/ws');
|
||||||
|
|
||||||
|
ws.onopen = () => { statusSpan.textContent = '已连接'; statusDot.className = 'ok'; console.log('[WS] connected'); };
|
||||||
|
ws.onclose = () => { statusSpan.textContent = '已断开，3秒后重连...'; statusDot.className = 'bad'; setTimeout(connectWS, 3000); };
|
||||||
|
ws.onerror = (e) => { statusSpan.textContent = '错误'; statusDot.className = 'bad'; console.error('[WS] error', e); };
|
||||||
|
|
||||||
|
ws.onmessage = (ev) => {
|
||||||
|
try {
|
||||||
|
const msg = JSON.parse(ev.data);
|
||||||
|
if (msg.timestamp) tsSpan.textContent = msg.timestamp;
|
||||||
|
if (msg.statuses && Array.isArray(msg.statuses)) {
|
||||||
|
msg.statuses.forEach(s => updateRow(s));
|
||||||
|
}
|
||||||
|
if (msg.power) updatePower(msg.power);
|
||||||
|
} catch (e) {
|
||||||
|
console.error('parse error', e, ev.data);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
function fmt(v, digits) {
|
||||||
|
if (v === undefined || v === null) return '-';
|
||||||
|
return Number(v).toFixed(digits !== undefined ? digits : 4);
|
||||||
|
}
|
||||||
|
|
||||||
|
function tempClass(v) {
|
||||||
|
if (v === undefined || v === null) return '';
|
||||||
|
if (v > 120) return 'temp-red';
|
||||||
|
if (v > 100) return 'temp-yellow';
|
||||||
|
return '';
|
||||||
|
}
|
||||||
|
|
||||||
|
function updateRow(s) {
|
||||||
|
const tr = document.getElementById('r' + s.name);
|
||||||
|
if (!tr) return;
|
||||||
|
const hasError = s.error && s.error !== 0;
|
||||||
|
tr.className = hasError ? 'error' : '';
|
||||||
|
tr.cells[1].textContent = fmt(s.pos, 4);
|
||||||
|
tr.cells[2].textContent = fmt(s.speed, 4);
|
||||||
|
tr.cells[3].textContent = fmt(s.current, 4);
|
||||||
|
tr.cells[4].textContent = fmt(s.temperature, 1);
|
||||||
|
tr.cells[4].className = tempClass(s.temperature);
|
||||||
|
if (s.motor_temp !== undefined) {
|
||||||
|
tr.cells[5].textContent = fmt(s.motor_temp, 1);
|
||||||
|
tr.cells[5].className = tempClass(s.motor_temp);
|
||||||
|
}
|
||||||
|
if (s.mos_temp !== undefined) {
|
||||||
|
tr.cells[6].textContent = fmt(s.mos_temp, 1);
|
||||||
|
tr.cells[6].className = tempClass(s.mos_temp);
|
||||||
|
}
|
||||||
|
tr.cells[7].textContent = hasError ? s.error : '✓';
|
||||||
|
tr.cells[7].className = hasError ? 'bad' : 'ok';
|
||||||
|
}
|
||||||
|
|
||||||
|
function updatePower(p) {
|
||||||
|
const vCard = document.getElementById('pwr-voltage');
|
||||||
|
const pCard = document.getElementById('pwr-power');
|
||||||
|
const lvCard = document.getElementById('pwr-little-voltage');
|
||||||
|
const lpCard = document.getElementById('pwr-little-power');
|
||||||
|
|
||||||
|
document.getElementById('pwr-voltage-val').textContent = fmt(p.voltage, 2) + ' V';
|
||||||
|
document.getElementById('pwr-current-val').textContent = fmt(p.current, 2) + ' A';
|
||||||
|
document.getElementById('pwr-power-val').textContent = fmt(p.power, 1) + ' %';
|
||||||
|
document.getElementById('pwr-little-voltage-val').textContent = fmt(p.little_voltage, 2) + ' V';
|
||||||
|
document.getElementById('pwr-little-current-val').textContent = fmt(p.little_current, 2) + ' A';
|
||||||
|
document.getElementById('pwr-little-power-val').textContent = fmt(p.little_power, 1) + ' %';
|
||||||
|
|
||||||
|
// colour-code master voltage: warn < 48V, alert < 44V
|
||||||
|
vCard.className = 'power-card' + (p.voltage < 44 ? ' alert' : p.voltage < 48 ? ' warn' : '');
|
||||||
|
// colour-code master SOC: warn < 30%, alert < 15%
|
||||||
|
pCard.className = 'power-card' + (p.power < 15 ? ' alert' : p.power < 30 ? ' warn' : '');
|
||||||
|
// colour-code little voltage: warn < 48V, alert < 44V
|
||||||
|
lvCard.className = 'power-card' + (p.little_voltage < 44 ? ' alert' : p.little_voltage < 48 ? ' warn' : '');
|
||||||
|
// colour-code little SOC: warn < 30%, alert < 15%
|
||||||
|
lpCard.className = 'power-card' + (p.little_power < 15 ? ' alert' : p.little_power < 30 ? ' warn' : '');
|
||||||
|
}
|
||||||
|
|
||||||
|
connectWS();
|
||||||
|
</script>
|
||||||
|
</body>
|
||||||
|
</html>
|
||||||
|
"""
|
||||||
|
|
||||||
|
@app.get("/", response_class=HTMLResponse)
|
||||||
|
async def index():
|
||||||
|
return INDEX_HTML
|
||||||
|
|
||||||
|
|
||||||
|
def append_robot_packet_log(packet: dict):
|
||||||
|
"""将服务器收到的机器人数据按 JSON Lines 追加写入日志文件。"""
|
||||||
|
LOG_DIR.mkdir(parents=True, exist_ok=True)
|
||||||
|
log_record = {
|
||||||
|
"received_at": datetime.now(timezone.utc).isoformat(),
|
||||||
|
"packet_timestamp": packet.get("timestamp"),
|
||||||
|
"packet": packet,
|
||||||
|
}
|
||||||
|
with PACKET_LOG_PATH.open("a", encoding="utf-8") as log_file:
|
||||||
|
log_file.write(json.dumps(log_record, ensure_ascii=False) + "\n")
|
||||||
|
|
||||||
|
# 浏览器客户端 WebSocket（接收广播）
|
||||||
|
@app.websocket("/ws")
|
||||||
|
async def websocket_browser_endpoint(ws: WebSocket):
|
||||||
|
await ws.accept()
|
||||||
|
browser_clients.append(ws)
|
||||||
|
global last_status
|
||||||
|
try:
|
||||||
|
# 可在连接时推送最近一条状态
|
||||||
|
if last_status is not None:
|
||||||
|
await ws.send_text(json.dumps(last_status))
|
||||||
|
while True:
|
||||||
|
# 浏览器无需发送消息，但保持连接活跃
|
||||||
|
msg = await ws.receive_text()
|
||||||
|
# 可以处理浏览器发来的控制命令（未实现），这里忽略
|
||||||
|
except WebSocketDisconnect:
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
if ws in browser_clients:
|
||||||
|
browser_clients.remove(ws)
|
||||||
|
|
||||||
|
# 机器人/ROS2 发送端 WebSocket（推送 JSON）
|
||||||
|
@app.websocket("/ws/robot")
|
||||||
|
async def websocket_robot_endpoint(ws: WebSocket):
|
||||||
|
await ws.accept()
|
||||||
|
print("[WS] 机器人发送端已连接")
|
||||||
|
try:
|
||||||
|
while True:
|
||||||
|
text = await ws.receive_text()
|
||||||
|
try:
|
||||||
|
data = json.loads(text)
|
||||||
|
except Exception:
|
||||||
|
print(f"[WS] JSON 解析失败: {text[:200]}")
|
||||||
|
continue
|
||||||
|
|
||||||
|
print(f"[WS] 收到机器人数据: {len(data.get('statuses', []))} 个电机")
|
||||||
|
append_robot_packet_log(data)
|
||||||
|
|
||||||
|
payload = {
|
||||||
|
"statuses": data.get("statuses", []),
|
||||||
|
"timestamp": data.get("timestamp"),
|
||||||
|
"power": data.get("power"),
|
||||||
|
}
|
||||||
|
|
||||||
|
global last_status
|
||||||
|
last_status = payload
|
||||||
|
await broadcast_to_browsers(payload)
|
||||||
|
except WebSocketDisconnect:
|
||||||
|
print("[WS] 机器人发送端已断开")
|
||||||
|
|
||||||
|
async def broadcast_to_browsers(payload):
|
||||||
|
text = json.dumps(payload)
|
||||||
|
to_remove = []
|
||||||
|
for client in list(browser_clients):
|
||||||
|
try:
|
||||||
|
await client.send_text(text)
|
||||||
|
except Exception:
|
||||||
|
to_remove.append(client)
|
||||||
|
for c in to_remove:
|
||||||
|
if c in browser_clients:
|
||||||
|
browser_clients.remove(c)
|
||||||
|
|
||||||
|
# 可直接运行： uvicorn main_monitor:app --reload --port 8000
|
||||||
|
if __name__ == "__main__":
|
||||||
|
import uvicorn
|
||||||
|
uvicorn.run("main_monitor:app", host="0.0.0.0", port=8000, reload=True)
|
||||||
231
monitor_sender.py
Normal file
231
monitor_sender.py
Normal file
@@ -0,0 +1,231 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
monitor_sender.py
|
||||||
|
运行在机器人控制器上，订阅 /robot_state ROS2 话题，
|
||||||
|
并尽量兼容 /power/battery/status 电池话题，
|
||||||
|
通过 WebSocket 将状态信息发送到监控服务器。
|
||||||
|
每 1 秒采样并发送一次，降低通信开销。
|
||||||
|
|
||||||
|
用法：
|
||||||
|
ros2 run <your_package> monitor_sender
|
||||||
|
或直接： python3 monitor_sender.py --ip 192.168.1.100 --port 8000
|
||||||
|
"""
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import asyncio
|
||||||
|
import json
|
||||||
|
import threading
|
||||||
|
from datetime import datetime, timezone
|
||||||
|
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
import websockets
|
||||||
|
import websockets.exceptions
|
||||||
|
|
||||||
|
try:
|
||||||
|
from ros2_bridge_msgs.msg import RobotState
|
||||||
|
except ImportError as exc:
|
||||||
|
RobotState = None
|
||||||
|
ROBOT_STATE_IMPORT_ERROR = exc
|
||||||
|
else:
|
||||||
|
ROBOT_STATE_IMPORT_ERROR = None
|
||||||
|
|
||||||
|
try:
|
||||||
|
from bodyctrl_msgs.msg import PowerBatteryStatus
|
||||||
|
except ImportError:
|
||||||
|
PowerBatteryStatus = None
|
||||||
|
|
||||||
|
|
||||||
|
class RobotStatusBridge(Node):
|
||||||
|
def __init__(self, msg_queue: asyncio.Queue, loop: asyncio.AbstractEventLoop):
|
||||||
|
super().__init__("monitor_sender")
|
||||||
|
self._queue = msg_queue
|
||||||
|
self._loop = loop
|
||||||
|
self._lock = threading.Lock()
|
||||||
|
self._latest_snapshot = None
|
||||||
|
self._latest_power = None # 电源状态单独保存
|
||||||
|
|
||||||
|
if RobotState is None:
|
||||||
|
raise RuntimeError(
|
||||||
|
"无法导入 ros2_bridge_msgs/msg/RobotState，请确认已 source 对应 ROS 2 工作空间。"
|
||||||
|
) from ROBOT_STATE_IMPORT_ERROR
|
||||||
|
|
||||||
|
self.create_subscription(RobotState, "/robot_state", self._robot_state_cb, 10)
|
||||||
|
subscribed_topics = ["/robot_state"]
|
||||||
|
|
||||||
|
if PowerBatteryStatus is not None:
|
||||||
|
self.create_subscription(
|
||||||
|
PowerBatteryStatus,
|
||||||
|
"/power/battery/status",
|
||||||
|
self._power_cb,
|
||||||
|
10,
|
||||||
|
)
|
||||||
|
subscribed_topics.append("/power/battery/status")
|
||||||
|
else:
|
||||||
|
self.get_logger().warn(
|
||||||
|
"未找到 bodyctrl_msgs/PowerBatteryStatus，将跳过 /power/battery/status 订阅"
|
||||||
|
)
|
||||||
|
|
||||||
|
self.get_logger().info(f"已订阅 {', '.join(subscribed_topics)}")
|
||||||
|
|
||||||
|
# 创建 1 秒定时器，每秒将最新状态放入队列
|
||||||
|
self._timer = self.create_timer(1.0, self._timer_cb)
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _stamp_to_iso(stamp) -> str:
|
||||||
|
return datetime.fromtimestamp(
|
||||||
|
stamp.sec + stamp.nanosec * 1e-9, tz=timezone.utc
|
||||||
|
).isoformat()
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _extract_temp_field(status, snake_name: str, legacy_name: str):
|
||||||
|
value = getattr(status, snake_name, None)
|
||||||
|
if value is None:
|
||||||
|
value = getattr(status, legacy_name, None)
|
||||||
|
return value
|
||||||
|
|
||||||
|
def _collect_statuses(self, status_group) -> list[dict]:
|
||||||
|
statuses = []
|
||||||
|
for s in getattr(status_group, "status", []):
|
||||||
|
entry = {
|
||||||
|
"name": int(s.name),
|
||||||
|
"pos": float(s.pos),
|
||||||
|
"speed": float(s.speed),
|
||||||
|
"current": float(s.current),
|
||||||
|
"temperature": float(s.temperature),
|
||||||
|
"error": int(s.error),
|
||||||
|
}
|
||||||
|
motor_temp = self._extract_temp_field(s, "motor_temperature", "motortemperature")
|
||||||
|
mos_temp = self._extract_temp_field(s, "mos_temperature", "mostemperature")
|
||||||
|
if motor_temp is not None:
|
||||||
|
entry["motor_temp"] = round(float(motor_temp), 1)
|
||||||
|
if mos_temp is not None:
|
||||||
|
entry["mos_temp"] = round(float(mos_temp), 1)
|
||||||
|
statuses.append(entry)
|
||||||
|
return statuses
|
||||||
|
|
||||||
|
def _robot_state_cb(self, msg):
|
||||||
|
try:
|
||||||
|
statuses = []
|
||||||
|
for key in ("head", "arm", "waist", "leg"):
|
||||||
|
section = getattr(msg, key, None)
|
||||||
|
if section is not None:
|
||||||
|
statuses.extend(self._collect_statuses(section))
|
||||||
|
|
||||||
|
timestamp = None
|
||||||
|
header = getattr(msg, "header", None)
|
||||||
|
if header is not None and hasattr(header, "stamp"):
|
||||||
|
timestamp = self._stamp_to_iso(header.stamp)
|
||||||
|
|
||||||
|
with self._lock:
|
||||||
|
self._latest_snapshot = {"statuses": statuses, "timestamp": timestamp}
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().warn(f"解析 /robot_state 消息失败: {e}")
|
||||||
|
|
||||||
|
def _power_cb(self, msg):
|
||||||
|
"""电源状态回调：保存最新电源数据"""
|
||||||
|
try:
|
||||||
|
with self._lock:
|
||||||
|
self._latest_power = {
|
||||||
|
"voltage": round(float(msg.master_battery_voltage), 3),
|
||||||
|
"current": round(float(msg.master_battery_current), 3),
|
||||||
|
"power": round(float(msg.master_battery_power), 1),
|
||||||
|
"little_voltage": round(float(msg.little_battery_voltage), 3),
|
||||||
|
"little_current": round(float(msg.little_battery_current), 3),
|
||||||
|
"little_power": round(float(msg.little_battery_power), 1),
|
||||||
|
}
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().warn(f"解析 power 消息失败: {e}")
|
||||||
|
|
||||||
|
def _timer_cb(self):
|
||||||
|
"""1 秒定时器回调：合并所有话题的最新状态，放入异步队列"""
|
||||||
|
with self._lock:
|
||||||
|
snapshot = self._latest_snapshot
|
||||||
|
power = self._latest_power
|
||||||
|
# 清空，避免重复发送
|
||||||
|
self._latest_snapshot = None
|
||||||
|
self._latest_power = None
|
||||||
|
|
||||||
|
if snapshot is None and power is None:
|
||||||
|
return
|
||||||
|
|
||||||
|
payload = {
|
||||||
|
"statuses": snapshot["statuses"] if snapshot else [],
|
||||||
|
"timestamp": snapshot["timestamp"] if snapshot else None,
|
||||||
|
"power": power,
|
||||||
|
}
|
||||||
|
try:
|
||||||
|
self._loop.call_soon_threadsafe(self._queue.put_nowait, payload)
|
||||||
|
except asyncio.QueueFull:
|
||||||
|
self.get_logger().warn("发送队列已满，丢弃本次数据")
|
||||||
|
|
||||||
|
|
||||||
|
async def ws_sender(server_url: str, msg_queue: asyncio.Queue):
|
||||||
|
"""异步 WebSocket 发送循环，带自动重连"""
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
async with websockets.connect(server_url) as ws:
|
||||||
|
print(f"[WS] 已连接到 {server_url}")
|
||||||
|
while True:
|
||||||
|
payload = await msg_queue.get()
|
||||||
|
text = json.dumps(payload)
|
||||||
|
await ws.send(text)
|
||||||
|
except (
|
||||||
|
OSError,
|
||||||
|
websockets.exceptions.ConnectionClosed,
|
||||||
|
websockets.exceptions.InvalidURI,
|
||||||
|
ConnectionRefusedError,
|
||||||
|
) as e:
|
||||||
|
print(f"[WS] 连接断开/失败: {e}，3 秒后重连...")
|
||||||
|
while not msg_queue.empty():
|
||||||
|
try:
|
||||||
|
msg_queue.get_nowait()
|
||||||
|
except asyncio.QueueEmpty:
|
||||||
|
break
|
||||||
|
await asyncio.sleep(3)
|
||||||
|
|
||||||
|
|
||||||
|
def spin_ros(node: Node):
|
||||||
|
"""在独立线程中运行 ROS2 spin"""
|
||||||
|
rclpy.spin(node)
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
parser = argparse.ArgumentParser(description="ROS2 Robot Status → WebSocket 发送端")
|
||||||
|
parser.add_argument(
|
||||||
|
"--ip",
|
||||||
|
type=str,
|
||||||
|
default="10.11.24.86",
|
||||||
|
help="监控服务器 IP 地址，例如 192.168.1.100",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--port",
|
||||||
|
type=int,
|
||||||
|
default=8000,
|
||||||
|
help="监控服务器端口（默认 8000）",
|
||||||
|
)
|
||||||
|
args = parser.parse_args()
|
||||||
|
server_url = f"ws://{args.ip}:{args.port}/ws/robot"
|
||||||
|
|
||||||
|
rclpy.init()
|
||||||
|
|
||||||
|
loop = asyncio.new_event_loop()
|
||||||
|
msg_queue: asyncio.Queue = asyncio.Queue(maxsize=100)
|
||||||
|
|
||||||
|
node = RobotStatusBridge(msg_queue, loop)
|
||||||
|
|
||||||
|
ros_thread = threading.Thread(target=spin_ros, args=(node,), daemon=True)
|
||||||
|
ros_thread.start()
|
||||||
|
|
||||||
|
try:
|
||||||
|
loop.run_until_complete(ws_sender(server_url, msg_queue))
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
print("\n[INFO] 正在关闭...")
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
loop.close()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
Reference in New Issue
Block a user