diff --git a/README.md b/README.md index 0d163fd..439e85d 100644 --- a/README.md +++ b/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（或任何你想要的数字） diff --git a/src/car_usb_cam/__pycache__/tf_web_bridge.cpython-310.pyc b/src/car_usb_cam/__pycache__/tf_web_bridge.cpython-310.pyc new file mode 100644 index 0000000..cdcb62e Binary files /dev/null and b/src/car_usb_cam/__pycache__/tf_web_bridge.cpython-310.pyc differ diff --git a/src/car_usb_cam/image_web_bridge.py b/src/car_usb_cam/image_web_bridge.py new file mode 100644 index 0000000..2fb8bba --- /dev/null +++ b/src/car_usb_cam/image_web_bridge.py @@ -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 ''}") + + +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 ''}") + 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""" + + + + + ROS Image Bridge + + + +
+
+ ROS Image Bridge + topic {topic} + connecting +
+ ROS image stream +
+ + +""".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() diff --git a/src/car_usb_cam/image_web_bridge_README.md b/src/car_usb_cam/image_web_bridge_README.md new file mode 100644 index 0000000..76046e5 --- /dev/null +++ b/src/car_usb_cam/image_web_bridge_README.md @@ -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. diff --git a/src/car_usb_cam/run_tf_viewer.sh b/src/car_usb_cam/run_tf_viewer.sh new file mode 100755 index 0000000..b6d6136 --- /dev/null +++ b/src/car_usb_cam/run_tf_viewer.sh @@ -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 "$@" + diff --git a/src/car_usb_cam/tf_viewer.html b/src/car_usb_cam/tf_viewer.html new file mode 100644 index 0000000..eeda70f --- /dev/null +++ b/src/car_usb_cam/tf_viewer.html @@ -0,0 +1,488 @@ + + + + + + TF Viewer + + + +
+
+
+

TF Viewer

+
?????? /tf ??/tf_static?????? /map ???????????/div> +
+
+ + + + connecting... +
+
+ +
+
+ + + + + + + + + + + + + + + + + +
+ dynamic TF + static TF + map image + map: none +
+
+ + +
+
+ + + + + diff --git a/src/car_usb_cam/tf_web_bridge.py b/src/car_usb_cam/tf_web_bridge.py new file mode 100644 index 0000000..75cf221 --- /dev/null +++ b/src/car_usb_cam/tf_web_bridge.py @@ -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() + diff --git a/调试记录.md b/调试记录.md index 4f7fac9..553ebeb 100644 --- a/调试记录.md +++ b/调试记录.md @@ -180,3 +180,10 @@ Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub ## 6/20 调试记录 所有工具/辅助类脚本全部放在tools文件夹下 + +## 7/20 调试记录 +启动qr_detect的方式： +```bash +ros2 launch car_usb_cam hobot_usb_cam.launch.py # 终端1 +ros2 launch qr_detection qr_detect.launch.py # 终端2 +``` \ No newline at end of file