编写了一些脚本和工具-zbw

This commit is contained in:
2026-07-22 17:32:57 +08:00
parent 691e9c80bd
commit c7260bd021
8 changed files with 1463 additions and 0 deletions

View File

@@ -0,0 +1,480 @@
#!/usr/bin/env python3
"""ROS2 image topic to MJPEG web bridge.
Supports the image publishing modes used by car_usb_cam:
- sensor_msgs/msg/Image on /image
- sensor_msgs/msg/CompressedImage style JPEG topics
- hbm_img_msgs/msg/HbmMsg1080P/540P/480P zero-copy payload messages
"""
from __future__ import annotations
import argparse
import importlib
import json
import signal
import threading
import time
from collections import deque
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
from pathlib import Path
from typing import Any
from urllib.parse import parse_qs, urlsplit
import cv2
import numpy as np
ROOT = Path(__file__).resolve().parent
JPEG_LOCK = threading.Condition()
LATEST_JPEG: bytes | None = None
LATEST_META: dict[str, Any] = {
"ok": False,
"message": "ROS thread not started",
"topic": None,
"topic_type": None,
"frames": 0,
"fps": 0.0,
"last_frame_time": None,
"compress": False,
"quality": None,
}
def normalize_encoding(value: Any) -> str:
if isinstance(value, str):
return value.strip("\x00").lower()
if isinstance(value, (bytes, bytearray)):
return bytes(value).split(b"\x00", 1)[0].decode("ascii", errors="ignore").lower()
if isinstance(value, (list, tuple)):
return bytes(int(item) & 0xFF for item in value).split(b"\x00", 1)[0].decode(
"ascii", errors="ignore"
).lower()
return str(value).strip("\x00").lower()
def _valid_bytes(data: Any, size: int | None = None) -> bytes:
if isinstance(data, bytes):
raw = data
elif isinstance(data, bytearray):
raw = bytes(data)
else:
raw = bytes(int(item) & 0xFF for item in data)
return raw[:size] if size is not None else raw
def _rows_from_buffer(raw: bytes, height: int, step: int, row_bytes: int) -> np.ndarray:
expected = height * step
if len(raw) < expected:
raise ValueError(f"image data too short: got {len(raw)} bytes, need {expected}")
rows = np.frombuffer(raw[:expected], dtype=np.uint8).reshape((height, step))
return rows[:, :row_bytes]
def image_message_to_bgr(msg: Any) -> np.ndarray:
height = int(msg.height)
width = int(msg.width)
step = int(msg.step)
encoding = normalize_encoding(msg.encoding)
raw = _valid_bytes(msg.data)
if encoding in ("rgb8", "bgr8"):
rows = _rows_from_buffer(raw, height, step, width * 3)
image = rows.reshape((height, width, 3))
return cv2.cvtColor(image, cv2.COLOR_RGB2BGR) if encoding == "rgb8" else image
if encoding in ("mono8", "8uc1"):
rows = _rows_from_buffer(raw, height, step, width)
return cv2.cvtColor(rows.reshape((height, width)), cv2.COLOR_GRAY2BGR)
if encoding in ("rgba8", "bgra8"):
rows = _rows_from_buffer(raw, height, step, width * 4)
image = rows.reshape((height, width, 4))
code = cv2.COLOR_RGBA2BGR if encoding == "rgba8" else cv2.COLOR_BGRA2BGR
return cv2.cvtColor(image, code)
if encoding in ("yuyv", "yuyv2rgb", "yuv422_yuy2"):
rows = _rows_from_buffer(raw, height, step, width * 2)
image = rows.reshape((height, width, 2))
return cv2.cvtColor(image, cv2.COLOR_YUV2BGR_YUY2)
if encoding == "nv12":
expected = height * width * 3 // 2
if len(raw) < expected:
raise ValueError(f"nv12 data too short: got {len(raw)} bytes, need {expected}")
image = np.frombuffer(raw[:expected], dtype=np.uint8).reshape((height * 3 // 2, width))
return cv2.cvtColor(image, cv2.COLOR_YUV2BGR_NV12)
raise ValueError(f"unsupported image encoding: {encoding or '<empty>'}")
def hbm_message_to_bgr(msg: Any) -> np.ndarray:
data_size = int(getattr(msg, "data_size", 0)) or None
image_like = type(
"ImageLike",
(),
{
"height": int(msg.height),
"width": int(msg.width),
"step": int(msg.step),
"encoding": normalize_encoding(msg.encoding),
"data": _valid_bytes(msg.data, data_size),
},
)()
return image_message_to_bgr(image_like)
def encode_bgr_to_jpeg(image: np.ndarray, quality: int = 75) -> bytes:
quality = max(1, min(100, int(quality)))
ok, encoded = cv2.imencode(".jpg", image, [int(cv2.IMWRITE_JPEG_QUALITY), quality])
if not ok:
raise ValueError("cv2 failed to encode jpeg")
return encoded.tobytes()
def compressed_message_to_jpeg(msg: Any, quality: int = 75, force_compress: bool = False) -> bytes:
data = _valid_bytes(msg.data)
fmt = str(getattr(msg, "format", "")).lower()
is_jpeg = "jpeg" in fmt or "jpg" in fmt or data.startswith(b"\xff\xd8")
if is_jpeg and not force_compress:
return data
array = np.frombuffer(data, dtype=np.uint8)
decoded = cv2.imdecode(array, cv2.IMREAD_COLOR)
if decoded is None:
raise ValueError(f"unsupported compressed image format: {fmt or '<empty>'}")
return encode_bgr_to_jpeg(decoded, quality)
def message_to_jpeg(
msg: Any,
topic_type: str,
quality: int = 75,
force_compress: bool = False,
) -> bytes:
if topic_type == "sensor_msgs/msg/CompressedImage":
return compressed_message_to_jpeg(msg, quality, force_compress)
if topic_type == "sensor_msgs/msg/Image":
return encode_bgr_to_jpeg(image_message_to_bgr(msg), quality)
if topic_type.startswith("hbm_img_msgs/msg/HbmMsg"):
return encode_bgr_to_jpeg(hbm_message_to_bgr(msg), quality)
raise ValueError(f"unsupported topic type: {topic_type}")
class FrameRateTracker:
def __init__(self, window_seconds: float = 2.0) -> None:
self.window_seconds = float(window_seconds)
self.samples: deque[float] = deque()
def record(self, timestamp: float | None = None) -> float:
now = time.time() if timestamp is None else float(timestamp)
self.samples.append(now)
oldest = now - self.window_seconds
while self.samples and self.samples[0] < oldest:
self.samples.popleft()
if len(self.samples) < 2:
return 0.0
elapsed = self.samples[-1] - self.samples[0]
if elapsed <= 0.0:
return 0.0
return round((len(self.samples) - 1) / elapsed, 1)
def publish_frame(jpeg: bytes, topic: str, topic_type: str, fps: float) -> None:
with JPEG_LOCK:
global LATEST_JPEG
LATEST_JPEG = jpeg
LATEST_META.update(
{
"ok": True,
"message": "streaming",
"topic": topic,
"topic_type": topic_type,
"frames": int(LATEST_META.get("frames") or 0) + 1,
"fps": float(fps),
"last_frame_time": time.time(),
}
)
JPEG_LOCK.notify_all()
def update_status(ok: bool, message: str, **extra: Any) -> None:
with JPEG_LOCK:
LATEST_META.update({"ok": ok, "message": message, **extra})
JPEG_LOCK.notify_all()
def request_path(raw_path: str) -> str:
return urlsplit(raw_path).path
def request_query(raw_path: str) -> dict[str, list[str]]:
return parse_qs(urlsplit(raw_path).query)
class ImageRequestHandler(BaseHTTPRequestHandler):
server_version = "ImageWebBridge/1.0"
def log_message(self, fmt: str, *args: Any) -> None:
return
def do_GET(self) -> None:
path = request_path(self.path)
if path in ("/", "/index.html"):
self.serve_index()
elif path == "/stream.mjpg":
self.serve_stream()
elif path == "/snapshot.jpg":
self.serve_snapshot()
elif path == "/status":
self.serve_json(LATEST_META)
else:
self.send_error(404)
def serve_index(self) -> None:
topic = LATEST_META.get("topic") or ""
body = f"""<!doctype html>
<html lang="en">
<head>
<meta charset="utf-8">
<meta name="viewport" content="width=device-width, initial-scale=1">
<title>ROS Image Bridge</title>
<style>
html, body {{ margin: 0; height: 100%; background: #111; color: #eee; font-family: Arial, sans-serif; }}
main {{ height: 100%; display: grid; grid-template-rows: auto 1fr; }}
header {{ display: flex; gap: 18px; align-items: center; padding: 10px 14px; background: #1d1f23; font-size: 14px; }}
img {{ width: 100%; height: 100%; object-fit: contain; background: #050505; }}
code {{ color: #9ad; }}
</style>
</head>
<body>
<main>
<header>
<strong>ROS Image Bridge</strong>
<span>topic <code id="topic">{topic}</code></span>
<span id="status">connecting</span>
</header>
<img src="/stream.mjpg" alt="ROS image stream">
</main>
<script>
async function tick() {{
const res = await fetch('/status', {{ cache: 'no-store' }});
const data = await res.json();
document.getElementById('topic').textContent = data.topic || '';
const fps = Number(data.fps || 0).toFixed(1);
document.getElementById('status').textContent =
(data.ok ? 'online' : 'waiting') + ' | fps ' + fps +
' | frames ' + (data.frames || 0) + ' | ' + data.message;
}}
setInterval(tick, 1000);
tick();
</script>
</body>
</html>""".encode("utf-8")
self.send_response(200)
self.send_header("Content-Type", "text/html; charset=utf-8")
self.send_header("Content-Length", str(len(body)))
self.end_headers()
self.wfile.write(body)
def serve_json(self, data: Any) -> None:
body = json.dumps(data, ensure_ascii=False, separators=(",", ":")).encode("utf-8")
self.send_response(200)
self.send_header("Content-Type", "application/json; charset=utf-8")
self.send_header("Content-Length", str(len(body)))
self.end_headers()
self.wfile.write(body)
def serve_snapshot(self) -> None:
with JPEG_LOCK:
if LATEST_JPEG is None:
self.send_error(503, "no frame yet")
return
jpeg = LATEST_JPEG
self.send_response(200)
self.send_header("Content-Type", "image/jpeg")
self.send_header("Cache-Control", "no-store")
self.send_header("Content-Length", str(len(jpeg)))
self.end_headers()
self.wfile.write(jpeg)
def serve_stream(self) -> None:
boundary = "frame"
self.send_response(200)
self.send_header("Content-Type", f"multipart/x-mixed-replace; boundary={boundary}")
self.send_header("Cache-Control", "no-store")
self.end_headers()
last_frame: bytes | None = None
try:
while True:
with JPEG_LOCK:
JPEG_LOCK.wait_for(
lambda: LATEST_JPEG is not None and LATEST_JPEG is not last_frame,
timeout=2.0,
)
jpeg = LATEST_JPEG
if jpeg is None or jpeg is last_frame:
self.wfile.write(b"\r\n")
self.wfile.flush()
continue
last_frame = jpeg
header = (
f"--{boundary}\r\n"
"Content-Type: image/jpeg\r\n"
f"Content-Length: {len(jpeg)}\r\n\r\n"
).encode("ascii")
self.wfile.write(header)
self.wfile.write(jpeg)
self.wfile.write(b"\r\n")
self.wfile.flush()
except (BrokenPipeError, ConnectionResetError):
return
class ReusableThreadingHTTPServer(ThreadingHTTPServer):
allow_reuse_address = True
def start_http(host: str, port: int) -> ThreadingHTTPServer:
server = ReusableThreadingHTTPServer((host, port), ImageRequestHandler)
thread = threading.Thread(target=server.serve_forever, daemon=True)
thread.start()
return server
def import_message_class(topic_type: str) -> Any:
parts = topic_type.split("/")
if len(parts) != 3 or parts[1] != "msg":
raise ValueError(f"expected ROS2 message type like pkg/msg/Name, got {topic_type}")
module = importlib.import_module(f"{parts[0]}.msg")
return getattr(module, parts[2])
def shutdown_rclpy_once(rclpy_module: Any) -> None:
if rclpy_module.ok():
rclpy_module.shutdown()
def discover_topic_type(node: Any, topic: str, timeout: float) -> str:
deadline = time.monotonic() + timeout
while time.monotonic() < deadline:
for name, types in node.get_topic_names_and_types():
if name == topic and types:
return types[0]
time.sleep(0.2)
raise TimeoutError(f"topic {topic} not found within {timeout:.1f}s")
def spin_ros(
topic: str,
topic_type: str,
quality: int,
max_fps: float,
discovery_timeout: float,
force_compress: bool,
) -> None:
try:
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
except Exception as exc:
update_status(False, f"ROS imports failed: {exc}", topic=topic, topic_type=topic_type)
return
class BridgeNode(Node):
def __init__(self) -> None:
super().__init__("image_web_bridge")
actual_type = topic_type
if actual_type == "auto":
actual_type = discover_topic_type(self, topic, discovery_timeout)
msg_class = import_message_class(actual_type)
self.actual_type = actual_type
self.last_emit = 0.0
self.fps = FrameRateTracker()
self.create_subscription(msg_class, topic, self.on_image, 10)
update_status(
False,
"subscribed, waiting for first frame",
topic=topic,
topic_type=actual_type,
compress=force_compress,
quality=quality,
)
def on_image(self, msg: Any) -> None:
now = time.monotonic()
if max_fps > 0 and now - self.last_emit < 1.0 / max_fps:
return
try:
jpeg = message_to_jpeg(msg, self.actual_type, quality, force_compress)
except Exception as exc:
update_status(False, f"frame conversion failed: {exc}", topic=topic, topic_type=self.actual_type)
return
self.last_emit = now
publish_frame(jpeg, topic, self.actual_type, self.fps.record())
try:
rclpy.init()
node = BridgeNode()
except Exception as exc:
update_status(False, f"ROS setup failed: {exc}", topic=topic, topic_type=topic_type)
return
try:
rclpy.spin(node)
except ExternalShutdownException:
pass
finally:
node.destroy_node()
shutdown_rclpy_once(rclpy)
def main() -> None:
parser = argparse.ArgumentParser()
parser.add_argument("--host", default="127.0.0.1")
parser.add_argument("--port", type=int, default=8080)
parser.add_argument("--topic", default="/image_mjpeg")
parser.add_argument("--topic-type", default="auto")
parser.add_argument("--quality", type=int, default=75)
parser.add_argument("--compress", action="store_true")
parser.add_argument("--max-fps", type=float, default=15.0)
parser.add_argument("--discovery-timeout", type=float, default=10.0)
args = parser.parse_args()
update_status(
False,
"HTTP started, waiting for ROS",
topic=args.topic,
topic_type=args.topic_type,
compress=args.compress,
quality=args.quality,
)
server = start_http(args.host, args.port)
print(f"image web bridge: http://{args.host}:{args.port}", flush=True)
print(f"topic: {args.topic} type: {args.topic_type}", flush=True)
stop = threading.Event()
signal.signal(signal.SIGINT, lambda *_: stop.set())
signal.signal(signal.SIGTERM, lambda *_: stop.set())
ros_thread = threading.Thread(
target=spin_ros,
args=(
args.topic,
args.topic_type,
args.quality,
args.max_fps,
args.discovery_timeout,
args.compress,
),
daemon=True,
)
ros_thread.start()
while not stop.is_set():
time.sleep(0.2)
server.shutdown()
if __name__ == "__main__":
main()

View File

@@ -0,0 +1,73 @@
# Scanner Debug Web
Minimal local web debug view for `obstacle_scanner`.
Run from WSL after the machine can see the ROS2 graph:
```bash
cd /mnt/d/Programme/agent/projects/yiliao/scanner_debug_web
./run.sh
```
Open:
```text
http://127.0.0.1:8767
```
Topics used:
- `/obstacles` (`obstacle_scanner/msg/ObstacleArray`)
- `/obstacle_scanner/debug_info` (`std_msgs/msg/String` JSON)
- `/scan` (`sensor_msgs/msg/LaserScan`)
If the `obstacle_scanner` message package is installed in a non-default workspace, set:
```bash
export YILIAO_WS_SETUP=/path/to/yiliao_ws/install/setup.bash
./run.sh
```
## Image Web Bridge
`image_web_bridge.py` forwards ROS2 image topics to a browser MJPEG stream.
It supports the image publishing modes used by `car_usb_cam`:
- `/image` (`sensor_msgs/msg/Image`) from `hobot_usb_cam` when `usb_zero_copy:=False`
- `/image_mjpeg` or `/image_jpeg` (`sensor_msgs/msg/CompressedImage`) from `hobot_codec`
- `/hbmem_img` (`hbm_img_msgs/msg/HbmMsg1080P`, `HbmMsg540P`, or `HbmMsg480P`) from zero-copy camera paths
Run on RDKx5:
```bash
cd /home/sunrise/yiliao_ws/src/car_usb_cam
source /opt/ros/humble/setup.bash
source /opt/tros/humble/setup.bash
source /home/sunrise/yiliao_ws/install/setup.bash
python3 image_web_bridge.py --host 192.168.10.210 --port 8080 --topic /image_mjpeg
```
Open from another device on the same network:
```text
http://192.168.10.210:8080
```
Use explicit topic types when the publisher is not running yet:
```bash
python3 image_web_bridge.py --host 192.168.10.210 --port 8080 --topic /image --topic-type sensor_msgs/msg/Image
python3 image_web_bridge.py --host 192.168.10.210 --port 8080 --topic /image_mjpeg --topic-type sensor_msgs/msg/CompressedImage
python3 image_web_bridge.py --host 192.168.10.210 --port 8080 --topic /hbmem_img --topic-type hbm_img_msgs/msg/HbmMsg1080P
```
By default, JPEG `CompressedImage` topics are passed through without re-encoding.
Add `--compress` to force every frame sent to the browser through JPEG encoding,
using `--quality` as the output quality:
```bash
python3 image_web_bridge.py --host 192.168.10.164 --port 8080 --topic /image_mjpeg --compress --quality 45
```
The page header shows the current output FPS and total forwarded frame count.

View File

@@ -0,0 +1,18 @@
#!/usr/bin/env bash
set -e
source /opt/ros/humble/setup.bash
if [ -f /opt/tros/humble/setup.bash ]; then
source /opt/tros/humble/setup.bash
fi
if [ -n "${YILIAO_WS_SETUP:-}" ]; then
source "$YILIAO_WS_SETUP"
elif [ -f "$HOME/yiliao_ws/install/setup.bash" ]; then
source "$HOME/yiliao_ws/install/setup.bash"
fi
cd "$(dirname "$0")"
python3 tf_web_bridge.py "$@"

View File

@@ -0,0 +1,488 @@
<!doctype html>
<html lang="zh-CN">
<head>
<meta charset="utf-8">
<meta name="viewport" content="width=device-width, initial-scale=1">
<title>TF Viewer</title>
<style>
:root {
color-scheme: dark;
--bg: #101216;
--panel: #181b21;
--soft: #20242c;
--canvas: #0d0f13;
--line: #303642;
--grid: #242933;
--text: #eef2f7;
--muted: #9aa4b2;
--live: #2e9cff;
--static: #2fbf71;
--warn: #f2a23c;
--map: #6f7f95;
}
* { box-sizing: border-box; }
body {
margin: 0;
padding: 14px;
background: var(--bg);
color: var(--text);
font: 14px/1.45 system-ui, -apple-system, "Segoe UI", sans-serif;
}
.app {
max-width: 1360px;
margin: 0 auto;
display: grid;
gap: 12px;
}
header, .toolbar, .legend, .row {
display: flex;
align-items: center;
gap: 10px;
flex-wrap: wrap;
}
header { justify-content: space-between; align-items: end; }
h1 { margin: 0; font-size: 18px; font-weight: 600; }
.muted, label, .status { color: var(--muted); }
.status { font-size: 13px; }
main {
display: grid;
grid-template-columns: minmax(0, 1.35fr) minmax(340px, 0.65fr);
gap: 12px;
align-items: start;
}
.panel {
background: var(--panel);
border: 1px solid var(--line);
border-radius: 8px;
padding: 12px;
}
button, select, input {
border: 1px solid var(--line);
border-radius: 6px;
background: var(--soft);
color: var(--text);
font: inherit;
padding: 6px 9px;
}
button { cursor: pointer; }
button[aria-pressed="true"] { border-color: var(--live); color: var(--live); }
input[type="range"] { width: 130px; padding: 0; }
svg {
display: block;
width: 100%;
height: auto;
background: var(--canvas);
border: 1px solid var(--line);
}
.legend { margin-top: 10px; color: var(--muted); font-size: 13px; }
.dot {
width: 10px;
height: 10px;
border-radius: 50%;
display: inline-block;
background: var(--c);
}
.stats {
display: grid;
grid-template-columns: repeat(2, minmax(0, 1fr));
gap: 8px;
margin-bottom: 12px;
}
.stat {
border: 1px solid var(--line);
border-radius: 6px;
padding: 8px;
}
.stat strong {
display: block;
font-size: 18px;
font-weight: 600;
font-variant-numeric: tabular-nums;
}
.table-wrap { max-height: 420px; overflow: auto; border-top: 1px solid var(--line); }
table {
width: 100%;
border-collapse: collapse;
font-variant-numeric: tabular-nums;
}
th, td {
padding: 7px 4px;
border-bottom: 1px solid var(--line);
text-align: left;
vertical-align: top;
}
th { color: var(--muted); font-weight: 500; position: sticky; top: 0; background: var(--panel); }
code { color: var(--text); }
@media (max-width: 900px) {
main { grid-template-columns: 1fr; }
}
</style>
</head>
<body>
<div class="app">
<header>
<div>
<h1>TF Viewer</h1>
<div class="muted">?????? /tf ??/tf_static?????? /map ???????????/div>
</div>
<div class="toolbar">
<button id="pause" type="button" aria-pressed="false">Pause</button>
<label>follow <select id="focus"></select></label>
<label>scale <input id="scale" type="range" min="40" max="260" value="120"> <span id="scaleText">120</span> px/m</label>
<span id="status" class="status">connecting...</span>
</div>
</header>
<main>
<section class="panel">
<svg id="mapView" viewBox="0 0 940 620" role="img" aria-label="TF map view">
<defs>
<pattern id="grid" width="60" height="60" patternUnits="userSpaceOnUse">
<path d="M 60 0 L 0 0 0 60" fill="none" stroke="var(--grid)" stroke-width="1"/>
</pattern>
<marker id="arrow-live" markerWidth="8" markerHeight="8" refX="7" refY="4" orient="auto">
<path d="M 0 0 L 8 4 L 0 8 z" fill="var(--live)"/>
</marker>
<marker id="arrow-static" markerWidth="8" markerHeight="8" refX="7" refY="4" orient="auto">
<path d="M 0 0 L 8 4 L 0 8 z" fill="var(--static)"/>
</marker>
</defs>
<rect width="940" height="620" fill="url(#grid)"/>
<g id="mapLayer"></g>
<g id="edgeLayer"></g>
<g id="frameLayer"></g>
</svg>
<div class="legend">
<span><i class="dot" style="--c:var(--live)"></i>dynamic TF</span>
<span><i class="dot" style="--c:var(--static)"></i>static TF</span>
<span><i class="dot" style="--c:var(--map)"></i>map image</span>
<span id="mapInfo">map: none</span>
</div>
</section>
<aside class="panel">
<div class="stats">
<div class="stat"><span class="muted">frames</span><strong id="frameCount">0</strong></div>
<div class="stat"><span class="muted">transforms</span><strong id="edgeCount">0</strong></div>
<div class="stat"><span class="muted">roots</span><strong id="rootCount">0</strong></div>
<div class="stat"><span class="muted">last update</span><strong id="lastUpdate">-</strong></div>
</div>
<div class="table-wrap">
<table>
<thead>
<tr><th>parent</th><th>child</th><th>xyz</th><th>type</th></tr>
</thead>
<tbody id="tfRows"></tbody>
</table>
</div>
</aside>
</main>
</div>
<script>
const state = {
edges: new Map(),
map: null,
mapAsset: null,
bridgeStatus: null,
lastEvent: 0,
};
const mapImage = new Image();
let paused = false;
let mapImageLoaded = false;
const svg = document.getElementById('mapView');
const mapLayer = document.getElementById('mapLayer');
const edgeLayer = document.getElementById('edgeLayer');
const frameLayer = document.getElementById('frameLayer');
const focusSelect = document.getElementById('focus');
const scaleInput = document.getElementById('scale');
const statusEl = document.getElementById('status');
function setText(id, value) {
document.getElementById(id).textContent = value;
}
function edgeKey(edge) {
return `${edge.parent}->${edge.child}`;
}
function escapeHtml(value) {
return String(value).replace(/[&<>"']/g, c => ({'&':'&amp;','<':'&lt;','>':'&gt;','"':'&quot;',"'":'&#39;'}[c]));
}
function receiveTf(payload, kind) {
if (!payload || !Array.isArray(payload.transforms)) return;
payload.transforms.forEach(edge => {
state.edges.set(edgeKey(edge), {...edge, kind, receivedAt: Date.now()});
});
state.lastEvent = Date.now();
render();
}
function originFromMap() {
if (state.map && state.map.info && state.map.info.origin) {
const pos = state.map.info.origin.position;
return [Number(pos.x || 0), Number(pos.y || 0), 0];
}
if (state.mapAsset && Array.isArray(state.mapAsset.origin)) {
return state.mapAsset.origin.map(Number);
}
return [0, 0, 0];
}
function resolutionFromMap() {
if (state.map && state.map.info && Number(state.map.info.resolution) > 0) {
return Number(state.map.info.resolution);
}
if (state.mapAsset && Number(state.mapAsset.resolution) > 0) {
return Number(state.mapAsset.resolution);
}
return null;
}
function imageSizeFromMap() {
if (state.map && state.map.info && state.map.info.width && state.map.info.height) {
return [Number(state.map.info.width), Number(state.map.info.height)];
}
if (state.mapAsset && state.mapAsset.width && state.mapAsset.height) {
return [Number(state.mapAsset.width), Number(state.mapAsset.height)];
}
if (mapImageLoaded) return [mapImage.naturalWidth, mapImage.naturalHeight];
return null;
}
function worldToSvg(x, y, center) {
const scale = Number(scaleInput.value);
return {
x: 470 + (x - center.x) * scale,
y: 310 - (y - center.y) * scale,
};
}
function yawFromQuat(q) {
const x = Number(q.x || 0), y = Number(q.y || 0), z = Number(q.z || 0), w = Number(q.w || 1);
return Math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));
}
function computePositions() {
const edges = [...state.edges.values()];
const byChild = new Map();
const children = new Set();
const frames = new Set();
edges.forEach(edge => {
byChild.set(edge.child, edge);
children.add(edge.child);
frames.add(edge.parent);
frames.add(edge.child);
});
const roots = [...frames].filter(frame => !children.has(frame)).sort();
const positions = new Map();
roots.forEach(root => positions.set(root, {x: 0, y: 0, yaw: 0, depth: 0}));
if (frames.has('map') && !positions.has('map')) positions.set('map', {x: 0, y: 0, yaw: 0, depth: 0});
if (frames.has('odom') && !positions.has('odom')) positions.set('odom', {x: 0, y: 0, yaw: 0, depth: 0});
let changed = true;
for (let pass = 0; pass < frames.size + 2 && changed; pass++) {
changed = false;
edges.forEach(edge => {
const parent = positions.get(edge.parent);
if (!parent || positions.has(edge.child)) return;
const t = edge.translation || {};
const yaw = parent.yaw + yawFromQuat(edge.rotation || {});
const cos = Math.cos(parent.yaw);
const sin = Math.sin(parent.yaw);
positions.set(edge.child, {
x: parent.x + Number(t.x || 0) * cos - Number(t.y || 0) * sin,
y: parent.y + Number(t.x || 0) * sin + Number(t.y || 0) * cos,
yaw,
depth: parent.depth + 1,
});
changed = true;
});
}
let index = 0;
frames.forEach(frame => {
if (positions.has(frame)) return;
positions.set(frame, {x: -2 + (index % 5), y: 2 + Math.floor(index / 5) * 0.5, yaw: 0, depth: 0});
index += 1;
});
return {positions, roots};
}
function selectedCenter(positions) {
const selected = focusSelect.value;
if (selected && positions.has(selected)) return positions.get(selected);
if (positions.has('base_footprint')) return positions.get('base_footprint');
if (positions.has('base_link')) return positions.get('base_link');
if (positions.has('odom')) return positions.get('odom');
return {x: 0, y: 0};
}
function renderMap(center) {
mapLayer.innerHTML = '';
const size = imageSizeFromMap();
const resolution = resolutionFromMap();
const url = state.mapAsset && state.mapAsset.url;
if (!url || !size || !resolution) {
setText('mapInfo', state.map ? 'map: /map metadata only' : 'map: none');
return;
}
const [width, height] = size;
const [originX, originY] = originFromMap();
const topLeft = worldToSvg(originX, originY + height * resolution, center);
const pxWidth = width * resolution * Number(scaleInput.value);
const pxHeight = height * resolution * Number(scaleInput.value);
const image = document.createElementNS('http://www.w3.org/2000/svg', 'image');
image.setAttribute('href', url);
image.setAttribute('x', topLeft.x);
image.setAttribute('y', topLeft.y);
image.setAttribute('width', pxWidth);
image.setAttribute('height', pxHeight);
image.setAttribute('opacity', '0.44');
image.setAttribute('preserveAspectRatio', 'none');
mapLayer.appendChild(image);
const rect = document.createElementNS('http://www.w3.org/2000/svg', 'rect');
rect.setAttribute('x', topLeft.x);
rect.setAttribute('y', topLeft.y);
rect.setAttribute('width', pxWidth);
rect.setAttribute('height', pxHeight);
rect.setAttribute('fill', 'none');
rect.setAttribute('stroke', 'var(--map)');
rect.setAttribute('stroke-width', '2');
mapLayer.appendChild(rect);
setText('mapInfo', `map: ${width}x${height}, res ${resolution.toFixed(4)} m/px, origin ${originX.toFixed(2)}, ${originY.toFixed(2)}`);
}
function renderGraph(positions, center) {
edgeLayer.innerHTML = '';
frameLayer.innerHTML = '';
[...state.edges.values()].forEach(edge => {
const a = positions.get(edge.parent);
const b = positions.get(edge.child);
if (!a || !b) return;
const p1 = worldToSvg(a.x, a.y, center);
const p2 = worldToSvg(b.x, b.y, center);
const line = document.createElementNS('http://www.w3.org/2000/svg', 'line');
line.setAttribute('x1', p1.x);
line.setAttribute('y1', p1.y);
line.setAttribute('x2', p2.x);
line.setAttribute('y2', p2.y);
line.setAttribute('stroke', edge.kind === 'static' ? 'var(--static)' : 'var(--live)');
line.setAttribute('stroke-width', edge.kind === 'static' ? '2' : '2.5');
line.setAttribute('marker-end', edge.kind === 'static' ? 'url(#arrow-static)' : 'url(#arrow-live)');
if (edge.kind === 'static') line.setAttribute('stroke-dasharray', '5 5');
edgeLayer.appendChild(line);
});
[...positions.entries()].forEach(([frame, pos]) => {
const p = worldToSvg(pos.x, pos.y, center);
const group = document.createElementNS('http://www.w3.org/2000/svg', 'g');
const circle = document.createElementNS('http://www.w3.org/2000/svg', 'circle');
circle.setAttribute('cx', p.x);
circle.setAttribute('cy', p.y);
circle.setAttribute('r', frame === focusSelect.value ? '7' : '5');
circle.setAttribute('fill', frame === 'map' ? 'var(--map)' : 'var(--text)');
const text = document.createElementNS('http://www.w3.org/2000/svg', 'text');
text.setAttribute('x', p.x + 8);
text.setAttribute('y', p.y - 8);
text.setAttribute('fill', 'var(--text)');
text.setAttribute('font-size', '13');
text.textContent = frame;
group.appendChild(circle);
group.appendChild(text);
frameLayer.appendChild(group);
});
}
function renderRows() {
const rows = [...state.edges.values()]
.sort((a, b) => edgeKey(a).localeCompare(edgeKey(b)))
.map(edge => {
const t = edge.translation || {};
return `<tr><td><code>${escapeHtml(edge.parent)}</code></td><td><code>${escapeHtml(edge.child)}</code></td><td>${Number(t.x || 0).toFixed(3)}, ${Number(t.y || 0).toFixed(3)}, ${Number(t.z || 0).toFixed(3)}</td><td>${edge.kind}</td></tr>`;
});
document.getElementById('tfRows').innerHTML = rows.join('');
}
function refreshFocusOptions(frames) {
const current = focusSelect.value;
const options = [''].concat([...frames].sort());
focusSelect.innerHTML = options.map(frame => `<option value="${escapeHtml(frame)}">${frame ? escapeHtml(frame) : 'auto'}</option>`).join('');
if (options.includes(current)) focusSelect.value = current;
}
function render() {
const {positions, roots} = computePositions();
refreshFocusOptions(new Set(positions.keys()));
const center = selectedCenter(positions);
renderMap(center);
renderGraph(positions, center);
renderRows();
setText('frameCount', positions.size);
setText('edgeCount', state.edges.size);
setText('rootCount', roots.length);
setText('lastUpdate', state.lastEvent ? `${((Date.now() - state.lastEvent) / 1000).toFixed(1)}s` : '-');
const status = state.bridgeStatus && state.bridgeStatus.message ? state.bridgeStatus.message : 'waiting';
statusEl.textContent = `${status}${paused ? ', paused' : ''}`;
document.getElementById('scaleText').textContent = scaleInput.value;
}
document.getElementById('pause').addEventListener('click', event => {
paused = !paused;
event.currentTarget.setAttribute('aria-pressed', paused ? 'true' : 'false');
event.currentTarget.textContent = paused ? 'Resume' : 'Pause';
render();
});
focusSelect.addEventListener('change', render);
scaleInput.addEventListener('input', render);
function receiveMessage(message) {
if (paused && message.type !== 'bridge_status') return;
if (message.type === 'snapshot') {
const data = message.data || {};
state.bridgeStatus = data.bridge_status;
state.map = data.map;
state.mapAsset = data.map_asset;
if (data.tf_static) receiveTf(data.tf_static, 'static');
if (data.tf) receiveTf(data.tf, 'dynamic');
} else if (message.type === 'tf') {
receiveTf(message.data, 'dynamic');
} else if (message.type === 'tf_static') {
receiveTf(message.data, 'static');
} else if (message.type === 'map') {
state.map = message.data;
state.lastEvent = Date.now();
} else if (message.type === 'map_asset') {
state.mapAsset = message.data;
} else if (message.type === 'bridge_status') {
state.bridgeStatus = message.data;
}
render();
}
fetch('/latest', {cache: 'no-store'})
.then(res => res.json())
.then(data => receiveMessage({type: 'snapshot', data}))
.catch(() => {});
const events = new EventSource('/events');
events.onopen = () => { statusEl.textContent = 'connected'; };
events.onerror = () => { statusEl.textContent = 'disconnected'; };
events.onmessage = event => receiveMessage(JSON.parse(event.data));
fetch('/latest', {cache: 'no-store'}).then(res => res.json()).then(data => {
if (data.map_asset && data.map_asset.url) {
state.mapAsset = data.map_asset;
mapImage.onload = () => { mapImageLoaded = true; render(); };
mapImage.src = data.map_asset.url;
}
render();
}).catch(render);
window.setInterval(render, 1000);
render();
</script>
</body>
</html>

View File

@@ -0,0 +1,385 @@
#!/usr/bin/env python3
"""ROS2 TF to browser bridge with optional static map image overlay."""
from __future__ import annotations
import argparse
import struct
import json
import mimetypes
import queue
import signal
import threading
import time
from dataclasses import dataclass
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
from pathlib import Path
from typing import Any
from urllib.parse import urlsplit
ROOT = Path(__file__).resolve().parent
CLIENTS: list[queue.Queue[str]] = []
CLIENTS_LOCK = threading.Lock()
MAP_ASSET: "MapAsset | None" = None
LATEST: dict[str, Any] = {
"tf": None,
"tf_static": None,
"map": None,
"bridge_status": {"ok": False, "message": "ROS thread not started"},
"connected_at": time.time(),
}
@dataclass(frozen=True)
class MapAsset:
path: Path
content_type: str
metadata: dict[str, Any]
def request_path(raw_path: str) -> str:
return urlsplit(raw_path).path
def stamp_to_float(stamp: Any) -> float | None:
if stamp is None:
return None
return float(getattr(stamp, "sec", 0)) + float(getattr(stamp, "nanosec", 0)) / 1_000_000_000.0
def vector_to_dict(value: Any) -> dict[str, float]:
return {
"x": float(getattr(value, "x", 0.0)),
"y": float(getattr(value, "y", 0.0)),
"z": float(getattr(value, "z", 0.0)),
}
def quaternion_to_dict(value: Any) -> dict[str, float]:
return {
"x": float(getattr(value, "x", 0.0)),
"y": float(getattr(value, "y", 0.0)),
"z": float(getattr(value, "z", 0.0)),
"w": float(getattr(value, "w", 1.0)),
}
def tf_message_to_dict(msg: Any) -> dict[str, Any]:
transforms = []
for transform in msg.transforms:
header = transform.header
transforms.append(
{
"parent": str(header.frame_id),
"child": str(transform.child_frame_id),
"stamp": stamp_to_float(getattr(header, "stamp", None)),
"translation": vector_to_dict(transform.transform.translation),
"rotation": quaternion_to_dict(transform.transform.rotation),
}
)
return {"count": len(transforms), "transforms": transforms}
def map_message_to_dict(msg: Any) -> dict[str, Any]:
return {
"header": {
"frame_id": msg.header.frame_id,
"stamp": stamp_to_float(msg.header.stamp),
},
"info": {
"width": int(msg.info.width),
"height": int(msg.info.height),
"resolution": float(msg.info.resolution),
"origin": {
"position": vector_to_dict(msg.info.origin.position),
"orientation": quaternion_to_dict(msg.info.origin.orientation),
},
},
}
def parse_yaml_scalar(value: str) -> Any:
value = value.strip().strip('"').strip("'")
if value.startswith("[") and value.endswith("]"):
return [parse_yaml_scalar(part) for part in value[1:-1].split(",")]
try:
return int(value)
except ValueError:
pass
try:
return float(value)
except ValueError:
return value
def load_simple_yaml(path: Path) -> dict[str, Any]:
data: dict[str, Any] = {}
for raw_line in path.read_text(encoding="utf-8").splitlines():
line = raw_line.split("#", 1)[0].strip()
if not line or ":" not in line:
continue
key, value = line.split(":", 1)
data[key.strip()] = parse_yaml_scalar(value)
return data
def read_png_size(path: Path) -> tuple[int, int] | None:
data = path.read_bytes()[:24]
if len(data) >= 24 and data.startswith(b"\x89PNG\r\n\x1a\n"):
return int.from_bytes(data[16:20], "big"), int.from_bytes(data[20:24], "big")
return None
def read_jpeg_size(path: Path) -> tuple[int, int] | None:
data = path.read_bytes()
if not data.startswith(b"\xff\xd8"):
return None
index = 2
while index + 9 < len(data):
if data[index] != 0xFF:
index += 1
continue
marker = data[index + 1]
index += 2
if marker in (0xD8, 0xD9):
continue
if index + 2 > len(data):
return None
length = struct.unpack(">H", data[index : index + 2])[0]
if marker in range(0xC0, 0xC4) and index + 7 < len(data):
height = struct.unpack(">H", data[index + 3 : index + 5])[0]
width = struct.unpack(">H", data[index + 5 : index + 7])[0]
return width, height
index += length
return None
def read_image_size(path: Path) -> tuple[int, int] | None:
return read_png_size(path) or read_jpeg_size(path)
def detect_content_type(path: Path) -> str:
header = path.read_bytes()[:12]
if header.startswith(b"\x89PNG\r\n\x1a\n"):
return "image/png"
if header.startswith(b"\xff\xd8"):
return "image/jpeg"
return mimetypes.guess_type(path.name)[0] or "application/octet-stream"
def load_map_asset(
image_path: str | None,
yaml_path: str | None = None,
map_width_meters: float = 5.0,
) -> MapAsset | None:
if not image_path:
return None
path = Path(image_path).expanduser().resolve()
if not path.is_file():
raise FileNotFoundError(f"map image not found: {path}")
content_type = detect_content_type(path)
metadata: dict[str, Any] = {"image": path.name, "placement": "publish_map_windows"}
image_size = read_image_size(path)
if image_size is not None:
width, height = image_size
metadata.update(
{
"width": width,
"height": height,
"resolution": float(map_width_meters) / float(width),
"origin": [0.0, 0.0, 0.0],
}
)
metadata_path = Path(yaml_path).expanduser().resolve() if yaml_path else path.with_suffix(".yaml")
if metadata_path.is_file():
metadata.update(load_simple_yaml(metadata_path))
return MapAsset(path=path, content_type=content_type, metadata=metadata)
def publish_event(event_type: str, data: Any) -> None:
LATEST[event_type] = data
payload = json.dumps(
{"type": event_type, "data": data, "time": time.time()},
ensure_ascii=False,
separators=(",", ":"),
)
with CLIENTS_LOCK:
clients = list(CLIENTS)
for client in clients:
client.put(payload)
class TfRequestHandler(BaseHTTPRequestHandler):
server_version = "TfWebBridge/1.0"
def log_message(self, fmt: str, *args: Any) -> None:
return
def do_GET(self) -> None:
path = request_path(self.path)
if path in ("/", "/index.html"):
self.serve_file(ROOT / "tf_viewer.html", "text/html; charset=utf-8")
return
if path == "/events":
self.serve_events()
return
if path == "/latest":
self.serve_json(LATEST)
return
if path == "/map-image" and MAP_ASSET is not None:
self.serve_file(MAP_ASSET.path, MAP_ASSET.content_type)
return
self.send_error(404)
def serve_file(self, path: Path, content_type: str) -> None:
data = path.read_bytes()
self.send_response(200)
self.send_header("Content-Type", content_type)
self.send_header("Cache-Control", "no-store")
self.send_header("Content-Length", str(len(data)))
self.end_headers()
self.wfile.write(data)
def serve_json(self, data: Any) -> None:
body = json.dumps(data, ensure_ascii=False, separators=(",", ":")).encode("utf-8")
self.send_response(200)
self.send_header("Content-Type", "application/json; charset=utf-8")
self.send_header("Cache-Control", "no-store")
self.send_header("Content-Length", str(len(body)))
self.end_headers()
self.wfile.write(body)
def serve_events(self) -> None:
client: queue.Queue[str] = queue.Queue()
with CLIENTS_LOCK:
CLIENTS.append(client)
self.send_response(200)
self.send_header("Content-Type", "text/event-stream")
self.send_header("Cache-Control", "no-cache")
self.send_header("Connection", "keep-alive")
self.end_headers()
try:
client.put(json.dumps({"type": "snapshot", "data": LATEST, "time": time.time()}))
while True:
try:
payload = client.get(timeout=10.0)
self.wfile.write(f"data: {payload}\n\n".encode("utf-8"))
except queue.Empty:
self.wfile.write(b": keepalive\n\n")
self.wfile.flush()
except (BrokenPipeError, ConnectionResetError):
pass
finally:
with CLIENTS_LOCK:
if client in CLIENTS:
CLIENTS.remove(client)
class ReusableThreadingHTTPServer(ThreadingHTTPServer):
allow_reuse_address = True
def start_http(host: str, port: int) -> ThreadingHTTPServer:
server = ReusableThreadingHTTPServer((host, port), TfRequestHandler)
thread = threading.Thread(target=server.serve_forever, daemon=True)
thread.start()
return server
def spin_ros(tf_topic: str, tf_static_topic: str, map_topic: str | None) -> None:
try:
import rclpy
from nav_msgs.msg import OccupancyGrid
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
from tf2_msgs.msg import TFMessage
except Exception as exc:
message = f"ROS imports failed: {exc}"
print(message, flush=True)
publish_event("bridge_status", {"ok": False, "message": message})
return
class BridgeNode(Node):
def __init__(self) -> None:
super().__init__("tf_web_bridge")
tf_qos = QoSProfile(depth=100)
tf_qos.reliability = ReliabilityPolicy.RELIABLE
static_qos = QoSProfile(depth=1)
static_qos.reliability = ReliabilityPolicy.RELIABLE
static_qos.durability = DurabilityPolicy.TRANSIENT_LOCAL
self.create_subscription(TFMessage, tf_topic, self.on_tf, tf_qos)
self.create_subscription(TFMessage, tf_static_topic, self.on_tf_static, static_qos)
if map_topic:
self.create_subscription(OccupancyGrid, map_topic, self.on_map, static_qos)
def on_tf(self, msg: Any) -> None:
publish_event("tf", tf_message_to_dict(msg))
def on_tf_static(self, msg: Any) -> None:
publish_event("tf_static", tf_message_to_dict(msg))
def on_map(self, msg: Any) -> None:
publish_event("map", map_message_to_dict(msg))
rclpy.init()
node = BridgeNode()
publish_event("bridge_status", {"ok": True, "message": "subscribed"})
try:
rclpy.spin(node)
finally:
node.destroy_node()
rclpy.shutdown()
def bind_help() -> str:
return "bind address, default 127.0.0.1; use 192.168.10.164 to expose on that interface"
def main() -> None:
parser = argparse.ArgumentParser()
parser.add_argument("--host", default="127.0.0.1", help=bind_help())
parser.add_argument("--port", type=int, default=8770)
parser.add_argument("--tf-topic", default="/tf")
parser.add_argument("--tf-static-topic", default="/tf_static")
parser.add_argument("--map-topic", default="/map")
parser.add_argument("--no-map-topic", action="store_true")
parser.add_argument("--map-image", default=None)
parser.add_argument("--map-yaml", default=None)
parser.add_argument("--map-width-meters", type=float, default=5.0)
args = parser.parse_args()
global MAP_ASSET
MAP_ASSET = load_map_asset(args.map_image, args.map_yaml, args.map_width_meters)
if MAP_ASSET:
LATEST["map_asset"] = {**MAP_ASSET.metadata, "url": "/map-image"}
server = start_http(args.host, args.port)
print(f"tf web bridge: http://{args.host}:{args.port}", flush=True)
if MAP_ASSET:
print(f"map image: {MAP_ASSET.path}", flush=True)
if args.host == "127.0.0.1":
print("bind: local loopback only; add --host 192.168.10.164 to expose that interface", flush=True)
stop = threading.Event()
signal.signal(signal.SIGINT, lambda *_: stop.set())
signal.signal(signal.SIGTERM, lambda *_: stop.set())
map_topic = None if args.no_map_topic else args.map_topic
ros_thread = threading.Thread(
target=spin_ros,
args=(args.tf_topic, args.tf_static_topic, map_topic),
daemon=True,
)
ros_thread.start()
while not stop.is_set():
time.sleep(0.2)
server.shutdown()
if __name__ == "__main__":
main()