编写了一些脚本和工具-zbw
This commit is contained in:
12
README.md
12
README.md
@@ -88,8 +88,18 @@
|
||||
`ros2 launch lslidar_driver lsn10_launch.py `
|
||||
open new terminal
|
||||
```bash
|
||||
# 简洁前：
|
||||
ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 1\ # (open radar)
|
||||
ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 0\ # (close radar)
|
||||
|
||||
# 简洁后：
|
||||
lidar
|
||||
```
|
||||
|
||||
启动qr_detect的方式：
|
||||
```bash
|
||||
ros2 launch car_usb_cam hobot_usb_cam.launch.py # 终端1
|
||||
ros2 launch qr_detection qr_detect.launch.py # 终端2
|
||||
```
|
||||
|
||||
# 四、其它说明
|
||||
@@ -105,6 +115,8 @@ foxglove # 启动foxbridge
|
||||
运行二维码检测（无缓存版）：
|
||||
`ros2 run qr_detection qr_dete_depth_node --ros-args -p use_buffer:=false`
|
||||
|
||||
二维码识别出来是一个四位数，偶数为逆，奇数为顺
|
||||
|
||||
# 五、tools 工具使用
|
||||
`measure_turning_radius.py`：功能为测试实际转向角度与输入的关系。启动后，会在/turning_radius_data文件夹下生成测试数据
|
||||
`set_volume.py`：功能为设置USB声卡的音量，修改第七行或直接`python3 set_volume.py 80`即可更改为80（或任何你想要的数字）
|
||||
|
||||
BIN
src/car_usb_cam/__pycache__/tf_web_bridge.cpython-310.pyc
Normal file
BIN
src/car_usb_cam/__pycache__/tf_web_bridge.cpython-310.pyc
Normal file
Binary file not shown.
480
src/car_usb_cam/image_web_bridge.py
Normal file
480
src/car_usb_cam/image_web_bridge.py
Normal 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()
|
||||
73
src/car_usb_cam/image_web_bridge_README.md
Normal file
73
src/car_usb_cam/image_web_bridge_README.md
Normal 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.
|
||||
18
src/car_usb_cam/run_tf_viewer.sh
Executable file
18
src/car_usb_cam/run_tf_viewer.sh
Executable 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 "$@"
|
||||
|
||||
488
src/car_usb_cam/tf_viewer.html
Normal file
488
src/car_usb_cam/tf_viewer.html
Normal 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 => ({'&':'&','<':'<','>':'>','"':'"',"'":'''}[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>
|
||||
|
||||
385
src/car_usb_cam/tf_web_bridge.py
Normal file
385
src/car_usb_cam/tf_web_bridge.py
Normal 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()
|
||||
|
||||
Reference in New Issue
Block a user