Robot Status Monitor

commit d574ba34147b1c946d62b9f3cf7d24e4ed99adb4 Author: Kong Date: Tue Apr 14 17:45:47 2026 +0800 first commit diff --git a/.codex b/.codex new file mode 100644 index 0000000..e69de29 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..5ae1032 --- /dev/null +++ b/.gitignore @@ -0,0 +1,18 @@ +# Python +__pycache__/ +*.py[cod] +*.pyo +*.pyd +*.egg-info/ +dist/ +build/ +.venv/ +venv/ +env/ + +# JetBrains IDE +.idea/ + +# macOS +.DS_Store + diff --git a/README.md b/README.md new file mode 100644 index 0000000..3df6b4f --- /dev/null +++ b/README.md @@ -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://: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://: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 [--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 + ``` + +3. **Open browser**: navigate to `http://: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 | + diff --git a/main_monitor.py b/main_monitor.py new file mode 100644 index 0000000..de68c6c --- /dev/null +++ b/main_monitor.py @@ -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 = """ + + + + + Robot Status Monitor + + + +

🤖 Robot Status Monitor

WebSocket: 连接中...

最近更新: -

+ +

⚡ 主电池电压

- V

🔋 主电池电流

- A

📊 主电池电量

- %

⚡ 副电池电压

- V

🔋 副电池电流

- A

📊 副电池电量

- %

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

ID	Pos (rad)	Speed	Current (A)	Temp (°C)	Motor Temp	MOS Temp	Error
左臂 (11-14)
11	-	-	-	-	-	-	-
12	-	-	-	-	-	-	-
13	-	-	-	-	-	-	-
14	-	-	-	-	-	-	-
右臂 (21-24)
21	-	-	-	-	-	-	-
22	-	-	-	-	-	-	-
23	-	-	-	-	-	-	-
24	-	-	-	-	-	-	-
腰部 (31-33)
31	-	-	-	-	-	-	-
32	-	-	-	-	-	-	-
33	-	-	-	-	-	-	-
左腿 (51-56)
51	-	-	-	-	-	-	-
52	-	-	-	-	-	-	-
53	-	-	-	-	-	-	-
54	-	-	-	-	-	-	-
55	-	-	-	-	-	-	-
56	-	-	-	-	-	-	-
右腿 (61-66)
61	-	-	-	-	-	-	-
62	-	-	-	-	-	-	-
63	-	-	-	-	-	-	-
64	-	-	-	-	-	-	-
65	-	-	-	-	-	-	-
66	-	-	-	-	-	-	-

+ + + + +""" + +@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) diff --git a/monitor_sender.py b/monitor_sender.py new file mode 100644 index 0000000..7268f88 --- /dev/null +++ b/monitor_sender.py @@ -0,0 +1,231 @@ +#!/usr/bin/env python3 +""" +monitor_sender.py +运行在机器人控制器上，订阅 /robot_state ROS2 话题， +并尽量兼容 /power/battery/status 电池话题， +通过 WebSocket 将状态信息发送到监控服务器。 +每 1 秒采样并发送一次，降低通信开销。 + +用法： + ros2 run 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()