From c7260bd02136a16a372ada5ca6c385d948142c64 Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Wed, 22 Jul 2026 17:32:57 +0800 Subject: [PATCH] =?UTF-8?q?=E7=BC=96=E5=86=99=E4=BA=86=E4=B8=80=E4=BA=9B?= =?UTF-8?q?=E8=84=9A=E6=9C=AC=E5=92=8C=E5=B7=A5=E5=85=B7-zbw?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 12 + .../__pycache__/tf_web_bridge.cpython-310.pyc | Bin 0 -> 13073 bytes src/car_usb_cam/image_web_bridge.py | 480 +++++++++++++++++ src/car_usb_cam/image_web_bridge_README.md | 73 +++ src/car_usb_cam/run_tf_viewer.sh | 18 + src/car_usb_cam/tf_viewer.html | 488 ++++++++++++++++++ src/car_usb_cam/tf_web_bridge.py | 385 ++++++++++++++ 调试记录.md | 7 + 8 files changed, 1463 insertions(+) create mode 100644 src/car_usb_cam/__pycache__/tf_web_bridge.cpython-310.pyc create mode 100644 src/car_usb_cam/image_web_bridge.py create mode 100644 src/car_usb_cam/image_web_bridge_README.md create mode 100755 src/car_usb_cam/run_tf_viewer.sh create mode 100644 src/car_usb_cam/tf_viewer.html create mode 100644 src/car_usb_cam/tf_web_bridge.py 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 0000000000000000000000000000000000000000..cdcb62e26efa15165aeef7b1ec2f5712c91798c4 GIT binary patch literal 13073 zcma)Cd5~PkS%3Guc{@8hduSzD*I1H|QM4<|acoC+Y#o+lC-Pd>ZlZV{F`1oyvpcJ~ z)bEYfJ2eYbR*9^D;!waJ5GCw}N0mD>ffHU)xR_8sQ<)dLjAjwuKG_-Ce^<;>EYk1^i)%mDK(Z| z>8++G)78GoK2>%qnQH%Jzrquhf$HGopu*kCP<7knHeK`k{ozG@Y5TG^yJK?av^IH< zNcy{k_oU`0mv%2}lY4|&*eiNamwHkYruz1ZwCKBK_}-GYtclDmdvc%XM~?wKl@fzu z=%#*F6Whe_HBAgJ8jJd5R$LPIh+WsL$$P~Qh(;iyCNaj;mj;(mHYBh4F{8 z6KAqvJv$}qbAd1MUl!ATcCH-GWb2KvT(1=>*&r;0?MSSVHsL69?|;gf~He|BQx{009?(+|RD3pG*kWi&W3BmIIX z*QVJx?n?}cXRY&va0dV8iP}8=+@`DqjY>Ie4OhxR_%eoQ`lBxrz8r+|(xoVI>fCdu z$0o*G{SQ1}Xq*TFKg_nXWA&QJqGVXg$IFgPtUKzB38U(oL6j=iYc;=!$Mc17B`wOukh3|Q#iTDq z&bbp4r^hEIFny;a>n*=lB%#)SOcah&>h1^PYm++U&Vcmkz9|gyG5#(5+xU0zpBUG2 zZsc|sC@Hm()c~m?6E9X13jJD`59b@c9Av9o^}_-;Vk~DxZa!ZtRQ-HD^78p=T{J7i zQ~CUrW}(t)>C5NW?PETFTa)`RkOC_i5M<7u%ExpVZOqGIwC%?)Z~;8cHC#QVtJ=tw zg}FQ@CwHJNa-<(NWvvJWv-%-!;CG5dpoR*ywzjCx>f_pNeJp24jw-hUZ?I(v}r*(W@)^Zma0D zS)Dc6x1bwx&RC?s@^uWMK9N2j>8+e8kDw@FKQ#F;0SSIvmz?O9wOoZ@)Fl_gslFXu z6pN&dw4TUqqZe+%c7u+#na`EuFW~p=hVh%+J8Nr zvoyp)YGtaNTu1fo_U%<8@(WZ^%bc2YnGtZ&;p%T5`Br-V~5& zZ2_D$t39J#I=Eo8jahxcZ0k!#XtvE+3%q8c&c?qJCfc;Tn&LngwaHlz^*stpfe(#a z_Oh;PSM0F2tuN|rSnFE`NMMaAK7%%M8U+c(5ZT3qPqmlovKr*l@?Bh&GXq@>t?d*V4ZkL0!FEfMN~J8Kv>H9%Eh~8R91?2rGa3f~*&`|#rZT*%JS}57G4oDqi31F_OO9Jy0)-#I@0NL5I zXEv=i!vOg$VW};!sl|@t4e?GGk=v*TW!lK7r!LFb6Gi{qR%FkW1r#|*T%Mi*^`#%w zD^1nYLC@)O4I_~xkyWmh>bbs{K5A^LhImuWaxIkSSbUb?If6q3?Xl-jt+A@AZ zU)BMQWqpBlVA^F}?gwTFisxTFx6=(f6n6-qe@$Wh%P^RCd^7xHij%u(_|x8dRStcMGR&Ue)EED_+|~4~IQI zU@TY*+-<1Bw%)eb`$-Kwzy-O)*oxf@OCyidM}&h_rz5$dACMREc4dxN^kYzeN&1-i z(0>JTr3h0g*Ze?UVy{U8u0H3ebww84rPzC6x5!e^yzI}vsin)nuMqj5Ty4OcA|Hep zE64y?pyCkdX6PAx0Cbex^OfPSiJmV*P7h}RE_K}04U{AclA4N=7l6bvBOo)B1h67# zK`nLJmJiTqP>SROp}AyH`3sA^guM)7qG}MZb!$psUOF}~@pq^+U#D!|I6ro_*Sp8N z!)f0k&qwA|c^Z_18p7(P=0lJRv(eS6?| zZHBf;3UATC^DVUZtUir02_8QwQrk7O_HNhKJ#DXRM@#(lx^}1Sv~6w?-fXFPZdhTk zO>JFz>~(U;-7nwa-<>4ET>!=QIpfZkS@9KsmhsfHN(}CNo>gBYAZhQI1e8@UeFJ~- zwsM0b2Wq8R3?rvmYZQu?BO68=5m91ohaGi4Usi0Qz=%>(MspgC>KMZ`sTlq#(PrdaxkjvpCQP>I$5~NSX-ceE+MirrE=u9Xd z2QCHN=s?+`mA-B?+tq_4Zudinc~upB9)-3RSV};FAt3~dvSh^8gLWa<9zxT#%};2b zL^|g~dT6)d%TZ)!9YL?mfF*%5jXDS#f;?S5-F9GM5({qIUUJ)swi_nfcos0RsPBU5 z7uKhBVc)`*>DWy9TH%eN8~eMs%^Gmy+q&>2?@5G5OxLea0nUql%85Gb(@oJFx^ zkCN1AR2;sOJ!r{uy>XVn^IvHo<^8Nji6c!qO{9R=>M zyg@(_fHr8rEc$BCq8>UM`Jh=Um0wXtO$GcpPyQ(Te~jQYf{jz@G6~JVns*xYb~6-l zu3(dFrj>OiNsveK!(YU(!65+4rrMyN&x8Y&1fyibFc@9x)*IH{y)IkRy1a$%k>_7s zO^nb=MHX{IMM`t`Lv^_7sLr6!rpE_oBh+UR7@%ZwgAp1a{-=?>X)Yj0cnvwt5Dq7p z%Df2EYT>(R>RQb`ss#t;zyh&j3idJK{v^PT)>5Jk4S|U5kkaS&*ljbiRMIA%TL|er zbK8(FVHR=<06Bz|UkjSj2OWy#GTay+PGbQfc93(Wve?#afT+S2&1xf%%XlbCb&_KF zb1$5F5B(TOCXGd8Hku*y#V0wudSjwdm?x#9M6tr4B#4|C*<#JRT&n2#7Z^Z=UL<}R zn%h8V?;s@Deb;43a992cdKB^7@S%twr+0l4NH^~#bmW(}3s)qOue8Jt;fWq#w%959 zLYFHC^rud$Oy-fJaLCP4HH-sRt01QX zl@*fvF#$<^zkw4tKUxkinDNY=mrTLuE!uFr4X#R3yH#ICh0Lrv{^{=?m?Ti zj5T!XY}7r7Iad`*5>?3S?B3qnca1_2vx#+FL5ydkB5F{Ot177itK(I%Yf-C<7 z2o4Q$kkq8!AcrFv9P20R!V51C2_mWO$8%3&>#qx29}k%5Q`IpbHrFDyqRYG?2jeqaIa^6Fy3A zl>Z;~khF1afSiRgANHIqhdvTGYot~Nh^l21(dF-;?mzGg(g37!3g00``PQy2G+yHY zrKh$?=_+*vMhhVz6b@?Vn+zZ=m}O{gBo0)5448$WGSQ)>MP0Lsd(`1H{j0Q2bZG4^ z!6jIwbmv++$00?kJ$C|1b++we*Y}-p96>-6~FrO z3#|DQ0)}UC(Dg-Pe@gIY05I~3(6OzUs*Cx}ME*VW`fvOck%zxOB6GSMR1i6tn{(CtGJwsH*>(07;QFL$BT2DY) z;Eut)f;$zamQu@_NZtb1^PNepwXLns_KF^nx~X<~Hoep*5Y2r;6X{QyOBvA@_q?dp zGDs0=v;FfGT4E(X?3>{@al!}^m0lX)eqT9@zJuXV$(Ub=d&fO4!gqh|9y|*#0Lu5r zv3k6J^XjQs*HeRKcmbbUPY>2%tR1UkeeTW2n%gka9LM>>`f-rN>K|Ljd;LoBD&^RT z2o$%qDECEbg2;p1^Slbl$Ch{5_ZuUHN*QjT8`KJoV5T0*pTTxdOatI^@Hmnx33?*+ zX4rC=6gie{8J?$@KIlc4V_A-(2JyVyaAd1+BU2TN(JVt2-Gg043S-f$O)5chXYIjaMcn z)(dOxR-AU(I6^KdcX2e@HMs{MN-JF|^EhA)zyb0%+4B1YRLPPmLy{Eh$g_{dEWxh= z{4IX5)<$N_Gd$DFcu8c}*;d4F!w2d@J5I%s0kXlxLvkZ$M!PQf&7d$<@!#b9cLHaX zn`b{*s8s56d5Nds0t*2%6};(8{|@iliMK1i&BcC7)sM6nf6vWI55fB6LnZAL#&(*qx=d0QZed090RL|*R40p<~Z}4p9#ap z9wy)waQ9VR%4C3d&y(9E{r9j}>bb zI4xW`FNEBn+o3LmRILR|;a0`2Iybax;)YRmZy@+VyIo0Ey&GtU0=^$gwP*fWU0bkq zt#uFbyf$O>1qabF46<-XbIVvn`686Fz!N`cQ)R~3oIVGy{JM5sziwPN zuUpsc>(2E=$rUNld(DOVw-(I>xJFB9wKs`HZNXLJCU6;~Gj1z`_P(|oEAO_ulu>=n zrGEbNU7PEmYWv1i`UNVn_l07yiCDTge-^<7DiBEuXqhPUOj8!7%DAO4f4+_s&3xp& zSRX$x>v4#E!LO7zG~?0a#F^*g8waf+UQ9p|84Ma^O9jN^ND55gboH;X8?qnZjO+p2 zB5B+$eDvTP;HsqcuA`7f8eMlwNd~RgQV6rsq&dPT3m9k)%qD$A3Yc>pVlc_827RsF zL8DyD%X+YO9uY@WT=JTV+Tu1in93mM)B+M}i>vPLt8jMVoFR&N)kILO=S|dDs>VHkbkxX^ieIn-9L)EE z@z<^_+!2@%2yY9&0in9ck_XXi;=Tm0N0e*j5LY7H3#ZRLcjDx^(@~FH+qS50;=+ls zao#gwM1A61N@fX9NPQeF*W&b1a}C&K|sGbO6K$GQcXTD|CN>hLhv<$HFf<>6d$Ha7zUDp|3^og zVa-n#F*?d@on6!EzrZOaB_X`q72avYod`T&p+A|D@0P;|KgIb%%HeS|Gz z5mRDgsd| zEIqvTbSx03UnxSGd8BSPB+tJ|cAk=#Vev`TC?$|?koEaIUjcu2rWqo=T7#G=!BHZl zgbC(Ji>F}vuM{Pn+;}OLnw;Vv@OOs>Zgor_@!}TXn$Co8!lGlmCMxlt3d=n!4 zO5;T?^tVU?T*urvPkK*O@+Ou1XVKb;4VjxT=qQBdBGOVf!3L_vLSi1r`Kx7^ACaWd zSFOnOtt-KluPfmWr^u}K?8g!4+H(UNN`p9;8|uDe$x)Fi3@N%?)o!A7Pq#K4TpC)|ZfN1QrETH1+2QM2Nf&#?zH2JY)~VgDYO~#1jI=}k zlF(`}k+VA&%y4k_o^TiN-T3Wkn?ZW}cm%{AQy<$IIo47`P z5r^&jyN*rnz*x%}8KE!SO0Mz_T-QX0q2MQDjy_TkF!S~_Sq@K zVQS|=l_KQJ*`Mbet$`R<=Sd@-^Q0faG$RvNMdfc3Qy#85HE0cor7WC)&c)VaS-Q-} zjvP^`^$Gc(=-ukU(DA7ZqTJ^x1IM!J4hCSoF@=**2vKGJ3CdtLCgEF)+eTU2AmN~* zI5SRhIx7S$}eM(&f9LUl?Mo~mOQWhh;(HC!!} zYikZ#x650;RU(bi$!6~&#@IpbQvmOXKd^&d^ zauM^W>nlXZtJBd+-hexNCBj@vQ3+~0p>#_sPE}-^4)~-1V_s6v(8*6F2wmKGr>-uuE*yZO~ z=&OiMBM{ei`fjS7ebc^bW^m6au}D9wW5b&q>0<;6QwNA-m3Z`zv!@fESgD!#Y+73t zvnFW&GHX>k1L&CFpu^&DA8-2FYP+)eswa0Yey8fGM(X5WRJ-!I9*q+Dyr>uByVIp6 zt}^59v^4na0dDdS68kK{b%MVlIK&3BWz0f7%%LgY9hYffIkl*VifA;B@+Hl^N=rmI zjSFWg<*E4S*1^Fbt~IH}Mk%N_w2Uh8QP-o%0gNy)o1f$ej}v^3fU&&X3lLf7>cz`( zLQYB35L*}<#OE!Ci9JH_C;=H%-Q$ZddzD#~1lI^YOz=^HTLkpAREi@`b|~po!bG1z zv1sg>%i=Mpv-lD^-F_{Y0YLK2;PuV*U;0pSGA@mz9Q`XE^U~9D^z^sfB(5bm so`s+5C6SUE@X}t7m+*FYmeB*L`L?$w)0=q)JrdrCXSs)Pf#L4|1F=I9$p8QV literal 0 HcmV?d00001 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