From f5f3d53ec089339ba73ddd77bce562df7db9dfec Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Tue, 4 Aug 2026 14:31:45 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B8=85=E7=90=86=E4=BA=86=E4=B8=80=E4=BA=9B?= =?UTF-8?q?=E6=B2=A1=E6=9C=89=E7=94=A8=E5=88=B0=E7=9A=84vlm=5Fdetect?= =?UTF-8?q?=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- bashes/radar-driver-switch.sh | 249 +++++++++++------- src/car_usb_cam/image_web_bridge_README.md | 18 ++ .../launch/hobot_usb_cam.launch.py | 48 ++++ .../launch/hobot_usb_cam_websocket.launch.py | 28 +- ...launch_config.cpython-310-pytest-6.2.5.pyc | Bin 0 -> 5881 bytes .../test/test_unified_launch_config.py | 36 ++- src/mtran/README.md | 2 +- .../obstacle_nav2/config/nav2_params.yaml | 22 +- .../launch/obstacle_nav2.launch.py | 2 +- .../launch/wall_localization.launch.py | 2 +- src/origincar_base/src/origincar_base.cpp | 2 +- .../launch/local_vlm_adapter.launch.py | 145 ++++++++++ src/vlm_detect/package.xml | 7 + src/vlm_detect/setup.py | 1 + ...adapter_utils.cpython-310-pytest-6.2.5.pyc | Bin 0 -> 2120 bytes .../test/test_local_adapter_utils.py | 25 ++ .../vlm_backup/local_adapter_utils.py | 13 + .../vlm_backup/local_vlm_adapter_node.py | 212 +++++++++++++++ .../qr_tts_bridge.py | 0 .../{vlm_detect => vlm_backup}/tts_node.py | 0 .../local_adapter_utils.cpython-310.pyc | Bin 0 -> 568 bytes 21 files changed, 701 insertions(+), 111 deletions(-) create mode 100644 src/car_usb_cam/test/__pycache__/test_unified_launch_config.cpython-310-pytest-6.2.5.pyc create mode 100644 src/vlm_detect/launch/local_vlm_adapter.launch.py create mode 100644 src/vlm_detect/test/__pycache__/test_local_adapter_utils.cpython-310-pytest-6.2.5.pyc create mode 100644 src/vlm_detect/test/test_local_adapter_utils.py create mode 100644 src/vlm_detect/vlm_backup/local_adapter_utils.py create mode 100644 src/vlm_detect/vlm_backup/local_vlm_adapter_node.py rename src/vlm_detect/{vlm_detect => vlm_backup}/qr_tts_bridge.py (100%) rename src/vlm_detect/{vlm_detect => vlm_backup}/tts_node.py (100%) create mode 100644 src/vlm_detect/vlm_detect/__pycache__/local_adapter_utils.cpython-310.pyc diff --git a/bashes/radar-driver-switch.sh b/bashes/radar-driver-switch.sh index c64b9ce..34413ab 100644 --- a/bashes/radar-driver-switch.sh +++ b/bashes/radar-driver-switch.sh @@ -1,101 +1,172 @@ -#!/bin/bash +#!/usr/bin/env bash +set -euo pipefail -# 退出码说明 -# 0 - 成功 -# 1 - 错误：等待TTY设备就绪超时 -# 2 - 错误：未找到CH343驱动 -# 3 - 错误：雷达端口上未找到CH34x设备 -# 4 - 错误：未从cdc_acm找到雷达接口 -# 5 - 错误：解绑cdc_acm驱动失败 -# 6 - 错误：绑定usb_ch343驱动失败 +# Physical USB ports on the RDKx5 carrier. +RADAR_PORT="1-1.4" +IMU_PORT="1-1.1" -# ── 配置 ──────────────────────────────────────────── -TTY_DEVICE="/dev/ttyACM*" -CDC_ACM_PATH="/sys/bus/usb/drivers/cdc_acm" -USB_CH343_PATH="/sys/bus/usb/drivers/usb_ch343" -RADAR_USB_PORT="1-1.4" # 雷达 CH34x 的 USB 物理端口（固定不变） -CHECK_INTERVAL=1 -MAX_WAIT_SECONDS=30 +# RADAR -> usb_ch343, IMU -> cdc_acm. +RADAR_DRIVER="usb_ch343" +IMU_DRIVER="cdc_acm" -# ── 初始化 ────────────────────────────────────────── -start_time=$(cut -d. -f1 /proc/uptime) +DRIVER_ROOT="/sys/bus/usb/drivers" +MAX_WAIT_SECONDS="${MAX_WAIT_SECONDS:-30}" +WAIT_INTERVAL_SECONDS="${WAIT_INTERVAL_SECONDS:-1}" +INTERFACES=() -# ── 1. 等待 TTY 设备就绪 ──────────────────────────── -while true; do - for dev in /dev/ttyACM*; do - [ -e "$dev" ] || continue - if [ -c "$dev" ] && [ -r "$dev" ] && [ -w "$dev" ]; then - echo "[雷达驱动自动切换程序] 设备 $dev 已就绪！" > /dev/kmsg - TTY_DEVICE="$dev" - break 2 +log() { + local message="[radar-driver-switch] $*" + if [ -w /dev/kmsg ]; then + printf '%s\n' "$message" > /dev/kmsg + else + printf '%s\n' "$message" + fi +} + +load_driver() { + local driver="$1" + case "$driver" in + cdc_acm) + modprobe cdc_acm 2>/dev/null || true + ;; + usb_ch343) + modprobe ch343 2>/dev/null || true + ;; + esac +} + +driver_path() { + printf '%s/%s\n' "$DRIVER_ROOT" "$1" +} + +require_driver() { + local driver="$1" + local path + path="$(driver_path "$driver")" + if [ ! -d "$path" ]; then + log "error: driver path missing: $path" + exit 2 + fi +} + +collect_interfaces() { + local port="$1" + shopt -s nullglob + INTERFACES=(/sys/bus/usb/devices/"${port}":*) + shopt -u nullglob + [ "${#INTERFACES[@]}" -gt 0 ] +} + +wait_for_interfaces() { + local label="$1" + local port="$2" + local attempt=0 + while [ "$attempt" -le "$MAX_WAIT_SECONDS" ]; do + if collect_interfaces "$port"; then + return 0 + fi + log "waiting for $label port $port interfaces (${attempt}s)" + sleep "$WAIT_INTERVAL_SECONDS" + attempt=$((attempt + WAIT_INTERVAL_SECONDS)) + done + log "error: timed out waiting for $label port $port interfaces" + exit 1 +} + +current_driver() { + local iface="$1" + local target + target="$(readlink -f "$iface/driver" 2>/dev/null || true)" + if [ -n "$target" ]; then + basename "$target" + fi +} + +unbind_from_current_driver() { + local iface="$1" + local iface_name + local driver + iface_name="$(basename "$iface")" + driver="$(current_driver "$iface")" + if [ -z "$driver" ]; then + return 0 + fi + if [ ! -w "$(driver_path "$driver")/unbind" ]; then + log "error: cannot unbind $iface_name from $driver" + exit 5 + fi + printf '%s\n' "$iface_name" > "$(driver_path "$driver")/unbind" + log "unbound $iface_name from $driver" +} + +bind_to_driver() { + local iface="$1" + local driver="$2" + local iface_name + local bind_path + iface_name="$(basename "$iface")" + bind_path="$(driver_path "$driver")/bind" + if [ ! -w "$bind_path" ]; then + log "error: cannot bind $iface_name to $driver" + exit 6 + fi + if printf '%s\n' "$iface_name" > "$bind_path" 2>/dev/null; then + log "bound $iface_name to $driver" + else + log "warning: $iface_name did not bind to $driver" + fi +} + +ensure_port_bound() { + local label="$1" + local port="$2" + local desired_driver="$3" + local iface + local iface_name + local driver + local bound_count=0 + + wait_for_interfaces "$label" "$port" + + for iface in "${INTERFACES[@]}"; do + iface_name="$(basename "$iface")" + driver="$(current_driver "$iface")" + if [ "$driver" = "$desired_driver" ]; then + log "$label $iface_name already uses $desired_driver" + bound_count=$((bound_count + 1)) + continue + fi + + if [ -n "$driver" ]; then + unbind_from_current_driver "$iface" + sleep 0.2 + fi + + bind_to_driver "$iface" "$desired_driver" + sleep 0.2 + driver="$(current_driver "$iface")" + if [ "$driver" = "$desired_driver" ]; then + bound_count=$((bound_count + 1)) fi done - if [ ${MAX_WAIT_SECONDS} -gt 0 ]; then - current_time=$(cut -d. -f1 /proc/uptime) - elapsed_seconds=$((current_time - start_time)) - if [ ${elapsed_seconds} -ge ${MAX_WAIT_SECONDS} ]; then - echo "[雷达驱动自动切换程序] 错误：等待 ${TTY_DEVICE} 超时（${MAX_WAIT_SECONDS}秒）！" > /dev/kmsg - exit 1 - fi - echo "[雷达驱动自动切换程序] 仍在等待${TTY_DEVICE}（已等待${elapsed_seconds}秒）..." > /dev/kmsg + if [ "$bound_count" -eq 0 ]; then + log "error: no $label interface on $port is bound to $desired_driver" + exit 7 fi - sleep ${CHECK_INTERVAL} -done +} -# ── 2. 确认 CH343 驱动已加载 ──────────────────────── -if [ -d "${USB_CH343_PATH}" ]; then - echo "[雷达驱动自动切换程序] CH343驱动已加载，继续..." > /dev/kmsg -else - echo "[雷达驱动自动切换程序] 错误：未找到CH343驱动！" > /dev/kmsg - exit 2 -fi +main() { + load_driver "$RADAR_DRIVER" + load_driver "$IMU_DRIVER" + require_driver "$RADAR_DRIVER" + require_driver "$IMU_DRIVER" -# ── 3. 通过物理端口定位雷达 CH34x ─────────────────── -radar_sysfs="/sys/bus/usb/devices/${RADAR_USB_PORT}" + ensure_port_bound "RADAR" "$RADAR_PORT" "$RADAR_DRIVER" + ensure_port_bound "IMU" "$IMU_PORT" "$IMU_DRIVER" -if [ ! -d "${radar_sysfs}" ]; then - echo "[雷达驱动自动切换程序] 错误：端口 ${RADAR_USB_PORT} 上无设备！" > /dev/kmsg - exit 3 -fi + udevadm settle --timeout=5 2>/dev/null || true + log "done; serial devices: $(ls /dev/ttyACM* /dev/ttyCH343USB* 2>/dev/null | tr '\n' ' ')" +} -vendor_id=$(cat "${radar_sysfs}/idVendor" 2>/dev/null) -product_id=$(cat "${radar_sysfs}/idProduct" 2>/dev/null) - -if [ "${vendor_id}" != "1a86" ] || [ "${product_id}" != "55d4" ]; then - echo "[雷达驱动自动切换程序] 错误：端口 ${RADAR_USB_PORT} 上 VID:PID=${vendor_id}:${product_id}，非雷达CH34x！" > /dev/kmsg - exit 3 -fi - -echo "[雷达驱动自动切换程序] 雷达确认位于端口 ${RADAR_USB_PORT}，VID:PID=${vendor_id}:${product_id}" > /dev/kmsg - -# ── 4. 查找 cdc_acm 下雷达的接口 ──────────────────── -cdc_acm_sub_addr=$(ls "${CDC_ACM_PATH}" 2>/dev/null | grep "^${RADAR_USB_PORT}:" | head -n 1) - -if [ -z "${cdc_acm_sub_addr}" ]; then - echo "[雷达驱动自动切换程序] 错误：cdc_acm驱动中未找到端口 ${RADAR_USB_PORT} 的接口" > /dev/kmsg - exit 4 -fi - -echo "[雷达驱动自动切换程序] 雷达 cdc_acm 接口：${cdc_acm_sub_addr}" > /dev/kmsg - -# ── 5. 解绑 cdc_acm ───────────────────────────────── -echo "${cdc_acm_sub_addr}" | sudo tee "${CDC_ACM_PATH}/unbind" > /dev/null 2>&1 -if [ $? -eq 0 ]; then - echo "[雷达驱动自动切换程序] 已解绑 cdc_acm：${cdc_acm_sub_addr}" > /dev/kmsg -else - echo "[雷达驱动自动切换程序] 错误：解绑 cdc_acm 失败" > /dev/kmsg - exit 5 -fi - -# ── 6. 绑定 ch343 ─────────────────────────────────── -echo "${cdc_acm_sub_addr}" | sudo tee "${USB_CH343_PATH}/bind" > /dev/null 2>&1 -if [ $? -eq 0 ]; then - echo "[雷达驱动自动切换程序] 已绑定 usb_ch343：${cdc_acm_sub_addr}" > /dev/kmsg -else - echo "[雷达驱动自动切换程序] 错误：绑定 usb_ch343 失败" > /dev/kmsg - exit 6 -fi - -echo "[雷达驱动自动切换程序] 雷达驱动切换完毕！" > /dev/kmsg -exit 0 +main "$@" diff --git a/src/car_usb_cam/image_web_bridge_README.md b/src/car_usb_cam/image_web_bridge_README.md index 76046e5..f03738f 100644 --- a/src/car_usb_cam/image_web_bridge_README.md +++ b/src/car_usb_cam/image_web_bridge_README.md @@ -38,6 +38,24 @@ It supports the image publishing modes used by `car_usb_cam`: - `/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 +`car_usb_cam` can switch camera output modes using launch parameters only: + +```bash +# Native MJPEG output from the USB camera. +ros2 launch car_usb_cam hobot_usb_cam.launch.py \ + usb_zero_copy:=false \ + usb_pixel_format:=mjpeg +# Publishes /image as sensor_msgs/msg/CompressedImage. +``` + +```bash +# Zero-copy hbmem output for consumers such as hobot_llamacpp shared-memory mode. +ros2 launch car_usb_cam hobot_usb_cam.launch.py \ + usb_zero_copy:=true \ + usb_pixel_format:=mjpeg +# Publishes /hbmem_img as hbm_img_msgs/msg/HbmMsg1080P. +``` + Run on RDKx5: ```bash diff --git a/src/car_usb_cam/launch/hobot_usb_cam.launch.py b/src/car_usb_cam/launch/hobot_usb_cam.launch.py index 2588b88..b3d056c 100644 --- a/src/car_usb_cam/launch/hobot_usb_cam.launch.py +++ b/src/car_usb_cam/launch/hobot_usb_cam.launch.py @@ -35,6 +35,46 @@ def generate_launch_description(): default_value=camera_config, description="ROS parameter file for the USB camera", ), + DeclareLaunchArgument( + "usb_frame_id", + default_value="default_usb_cam", + description="Image message frame_id", + ), + DeclareLaunchArgument( + "usb_framerate", + default_value="30", + description="USB camera frame rate", + ), + DeclareLaunchArgument( + "usb_image_height", + default_value="720", + description="USB camera image height", + ), + DeclareLaunchArgument( + "usb_image_width", + default_value="1280", + description="USB camera image width", + ), + DeclareLaunchArgument( + "usb_io_method", + default_value="mmap", + description="USB camera io_method: mmap/read/userptr", + ), + DeclareLaunchArgument( + "usb_pixel_format", + default_value="mjpeg", + description="USB camera pixel format, such as mjpeg or yuyv2rgb", + ), + DeclareLaunchArgument( + "usb_video_device", + default_value="/dev/video0", + description="USB camera device", + ), + DeclareLaunchArgument( + "usb_zero_copy", + default_value="false", + description="Publish zero-copy hbmem image messages when true", + ), IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( @@ -51,6 +91,14 @@ def generate_launch_description(): parameters=[ LaunchConfiguration("camera_config"), {"camera_calibration_file_path": calibration_file}, + {"frame_id": LaunchConfiguration("usb_frame_id")}, + {"framerate": LaunchConfiguration("usb_framerate")}, + {"image_height": LaunchConfiguration("usb_image_height")}, + {"image_width": LaunchConfiguration("usb_image_width")}, + {"io_method": LaunchConfiguration("usb_io_method")}, + {"pixel_format": LaunchConfiguration("usb_pixel_format")}, + {"video_device": LaunchConfiguration("usb_video_device")}, + {"zero_copy": LaunchConfiguration("usb_zero_copy")}, ], arguments=["--ros-args", "--log-level", "warn"], ), diff --git a/src/car_usb_cam/launch/hobot_usb_cam_websocket.launch.py b/src/car_usb_cam/launch/hobot_usb_cam_websocket.launch.py index 8f76bd2..29dda79 100644 --- a/src/car_usb_cam/launch/hobot_usb_cam_websocket.launch.py +++ b/src/car_usb_cam/launch/hobot_usb_cam_websocket.launch.py @@ -16,8 +16,9 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration def generate_launch_description(): @@ -28,7 +29,17 @@ def generate_launch_description(): "launch", "hobot_usb_cam.launch.py", ) - ) + ), + launch_arguments={ + "usb_frame_id": LaunchConfiguration("usb_frame_id"), + "usb_framerate": LaunchConfiguration("usb_framerate"), + "usb_image_height": LaunchConfiguration("usb_image_height"), + "usb_image_width": LaunchConfiguration("usb_image_width"), + "usb_io_method": LaunchConfiguration("usb_io_method"), + "usb_pixel_format": LaunchConfiguration("usb_pixel_format"), + "usb_video_device": LaunchConfiguration("usb_video_device"), + "usb_zero_copy": LaunchConfiguration("usb_zero_copy"), + }.items(), ) websocket_launch = IncludeLaunchDescription( @@ -47,4 +58,15 @@ def generate_launch_description(): }.items(), ) - return LaunchDescription([camera_launch, websocket_launch]) + return LaunchDescription([ + DeclareLaunchArgument("usb_frame_id", default_value="default_usb_cam"), + DeclareLaunchArgument("usb_framerate", default_value="30"), + DeclareLaunchArgument("usb_image_height", default_value="720"), + DeclareLaunchArgument("usb_image_width", default_value="1280"), + DeclareLaunchArgument("usb_io_method", default_value="mmap"), + DeclareLaunchArgument("usb_pixel_format", default_value="mjpeg"), + DeclareLaunchArgument("usb_video_device", default_value="/dev/video0"), + DeclareLaunchArgument("usb_zero_copy", default_value="false"), + camera_launch, + websocket_launch, + ]) diff --git a/src/car_usb_cam/test/__pycache__/test_unified_launch_config.cpython-310-pytest-6.2.5.pyc b/src/car_usb_cam/test/__pycache__/test_unified_launch_config.cpython-310-pytest-6.2.5.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0eaa20e0363e0870870326620bcc0c788945db49 GIT binary patch literal 5881 zcmb_gTW=f372Z3S%Zp`Nv8^~4CoYmQX{6Kvf&wwr$cmCUv1vC}E@0|zSDcZw6uC>y zE+vblfVMKwytF9#Ro7{3ap6tc~)X&yyw^{HiP%$Z)xl_tK8FAg}coQ2WI^V{vp6cv?|rIEmq(45h0(@|U%pLW zegR@r>uYyT@9U}7*HgV|92ml&bkj^NN>l1L#|$q7|GoB z4CR^*@;U6#xBJGSOZjI0NY}NAS`W3aj%o{8ZJ&11tce^(7BtgpQ7J&oQo&;+KzvOU zAf&Y%v|7?y2L+^63));dL8rqbCXJBE{89@2u60u`tpG`=@*H(NPhIT@%@c8&3PuRX zwB}(8TE40$?Ibi_MYr^_Uepc989Zg(_^G9fOQ>oP8-_mziIMjD$b3P22DArVL3;-A z;?9fv7W8F}wa2u5n@UC+e4f%xci)Z*qw+j-cUhHt`*z>%>o66s_07XV-x}!Ayse4H zF@ZK;LSQJ)jKd}b1f;>k%ICQogbQ}-9zdS;mj zk$JL82fA1wbdt%xt)Y#_>`%1cX@4*d^q=chX99^+G@(tW$-P1oqs9G<3{`p3P{n03 zPM>I7#s~QLw!tt!Rj&bt1l|nQBfVD*LFAN98i);^!a$aG0ucslEk2=J#p?W(4Ic5k z9kKWt4NmBV@?czMA9@da7lR}THzL?1(*A@+8FuQ;*5Zp)H)?q?jW1RUUrsY;Nqth3 zY1)JtnAT`d=CY|KvW$8z2nH)c#3P=O`BspbU|E&Q)GA6-^L!QM2~oTPKs*kb6?MyS zKld!-KV@6IjEV-HsUPjY>v)pyKyc;f>&gMapPHHbsawou4)Qk3F&A%#<*^5zJokQP zx34Sj2^RtXw+f>^wZ(+F5to0D3}G)dn&#Nc^?{~2gaC7}h<)wQ>Kn{P-Z?UKz(M76 z$h#ByJo4T|zJPpjB40wjJdr!UpdZ7LZs_MK8-j*oMpZcSDwLhO~ZJ%t|(SxUrdre}&#< zew{ugf|TDUD#=AD@U-n_hi}My82cD+GiK6iw}Xz%_qd4R?cg4tK$L?1 zgwb#f-ysvS9s=v+%&IwQQ2-+INwCiSRva){T3))b^4^lae6#lEwYQWbkOe>pxLeuo z2CZS{O}I}!%+%uaT2wWqmGIWObXnZ!wt19FN2ww?Zm%wrU-xNbXl6Z(c!D`a0r`S7 z+~K}j*1*Sivq`^!)emr6gku>0$xZa$`4X7TfBk``kU8aL{;ZIh!jkPH20H`=5OHPr zGZ^BdQJNw#LW>2j<0`xw{XzZq`P58x2LHWp5^htwZyx9wj#DQiPv2y~ZZnrzO?WN$ zFi+kF{)a~O`u1p~LY6)@lB(anq0m~a30g$&!Z_xf#Da=H#bvapy0S=Z6ySV#_0lLu z@RKu?odl>=$D-Kc=e1e}4>VQ!i5>?qqv53*Do{Ab};p)=u z761Cu`?ck_rJHf_TBLlqxQcetZ1D(@0|l$@U|EUsGSXt*b_cP=3N_1*;$P_v<_WDx zkLE?7K(O&1wL3Ldoqtla`+{QCS6c2Zd@p$x!iA%uzpIx$50phLLT<+%mRbMoUH#_T zok9)ao}Oiz!=E5h?SGON0XDTcw3&INGiyLJGj-BjM%xitUE~pVXY^)ncJAyOS+GO> zQV&5QLeG7Z6_g>ch`xP<>{PqG$cn0zNL{4BQk7m)q%GA1g*?48C!8Y14^Wdq=KQs& z(dsguIoCIM(hy;XEc|^Uj2t2=+iE#8-g0G}lPP6U526^I2me8FgE|k|8O5@5pJGZ? z7u4j_K@??$DQ_RA&>1w?r|-~5D_XDi$c@DsN30>T1dJsU$LAAHYPKL)iR&cZB~c@B z69O)%ka=Vrd3mE(T*&NG<|o~~?%vL2v9VSyWyJD6T)8^nmH3dpah1d+2x&LsZj_3T zsDOfH6&i^;rRbO_J|;mbFJfK$&k8Silgvs9hodV!yS!zq~QVrR}uGc zQE+GxXUJ(OPS~4pfPd5v9pV55Qiu?T?VLe7a)CLP%NU{3I4%$m6btgTscB3iRB%i~ zDGu?wMW=K@6Fa6#K|uU9m2X1ye)LF%v;#z{6qW!ttw10o8S*8fdjM5ICtlp9g#4rn z1X~?4)gl=lL4=a|43NoYTjoc69vNT+jLQAW3VcMA`XNTR>v#4L%|%~f`C^;K z`LebDmMUrOvM{lN;ZTisu!T5qkE(jbY{*w$qNoh9fU&ZwZUSWC*3$C3OK+|CAKkbv zb2u@HI8J2_@fw52kY47FRt0W++%QSHYq*Y4WWw=Fc^yl2m=&N?*yaQMWLlJ`C#IDZ z9os_rifZxoHs@DZvAPH<>$Bj5le7Lc>r&L%d-wkcff}M`*H>po-zzG+l$#su<$Sg!@B{OL?+RFGnI9F&+_20;XQkXgWU)eyjs!R*NVgk> zDOJhGa3ML5{p5UprNs%Lt4K>*70JRxnds3Pz7HX-TXau`8LM-KPa`IAYllk=An*yy zdx)v`Zr)rKA5hON5=$h0LW1stlp!7)U+J!5O3MhcluE=sG{JcXS~*Fb(!G-H$09{n h$+sY$C7tRO1Rk^IFc@)7H}96*nM%q17$@O}{{=8l5S;)3 literal 0 HcmV?d00001 diff --git a/src/car_usb_cam/test/test_unified_launch_config.py b/src/car_usb_cam/test/test_unified_launch_config.py index 998311a..93068a8 100644 --- a/src/car_usb_cam/test/test_unified_launch_config.py +++ b/src/car_usb_cam/test/test_unified_launch_config.py @@ -82,6 +82,17 @@ class UnifiedLaunchConfigTest(unittest.TestCase): self.assertIn("usb_camera.yaml", constants) self.assertIn("usb_camera_calibration.yaml", constants) self.assertIn("hobot_usb_cam", constants) + for launch_arg in ( + "usb_frame_id", + "usb_framerate", + "usb_image_height", + "usb_image_width", + "usb_io_method", + "usb_pixel_format", + "usb_video_device", + "usb_zero_copy", + ): + self.assertIn(launch_arg, constants) node_calls = calls_named(BASE_LAUNCH, "Node") self.assertEqual(len(node_calls), 1) parameters = next( @@ -92,6 +103,17 @@ class UnifiedLaunchConfigTest(unittest.TestCase): parameters_tree = ast.dump(parameters) self.assertIn("camera_config", parameters_tree) self.assertIn("camera_calibration_file_path", parameters_tree) + for launch_arg in ( + "usb_frame_id", + "usb_framerate", + "usb_image_height", + "usb_image_width", + "usb_io_method", + "usb_pixel_format", + "usb_video_device", + "usb_zero_copy", + ): + self.assertIn(launch_arg, parameters_tree) def test_web_launch_only_adds_websocket(self): constants = string_constants(WEB_LAUNCH) @@ -100,14 +122,20 @@ class UnifiedLaunchConfigTest(unittest.TestCase): self.assertEqual(call_names(WEB_LAUNCH).count("Node"), 0) self.assertEqual(len(calls_named(WEB_LAUNCH, "IncludeLaunchDescription")), 2) camera_include = assigned_call(WEB_LAUNCH, "camera_launch") - self.assertNotIn("launch_arguments", {item.arg for item in camera_include.keywords}) + self.assertIn("launch_arguments", {item.arg for item in camera_include.keywords}) + camera_include_tree = ast.dump(camera_include) + for launch_arg in ( + "usb_zero_copy", + "usb_image_width", + "usb_image_height", + "usb_pixel_format", + "usb_video_device", + ): + self.assertIn(launch_arg, camera_include_tree) for forbidden in ( "hobot_codec", "/image_mjpeg", "yuyv2rgb", - "usb_image_width", - "usb_image_height", - "usb_pixel_format", ): self.assertNotIn(forbidden, constants) diff --git a/src/mtran/README.md b/src/mtran/README.md index 7eed681..5568887 100644 --- a/src/mtran/README.md +++ b/src/mtran/README.md @@ -56,7 +56,7 @@ cd ~/smart-healthcare-2026/src/mtran/dependencies ./auto_install.sh ``` -脚本会请求 `sudo` 安装非 ROS apt 包，在 `dependencies/tools` 安装固定版本 Bun， +不要使用 `sudo` 权限执行脚本，脚本会请求 `sudo` 安装非 ROS apt 包，在 `dependencies/tools` 安装固定版本 Bun， 从 `vendor/mtranserver` 构建当前 CPU 可运行的 sidecar，并只准备 `en -> zh-Hans` 模型。成功结尾类似： diff --git a/src/navigation/obstacle_nav2/config/nav2_params.yaml b/src/navigation/obstacle_nav2/config/nav2_params.yaml index 6033640..0c195b1 100755 --- a/src/navigation/obstacle_nav2/config/nav2_params.yaml +++ b/src/navigation/obstacle_nav2/config/nav2_params.yaml @@ -78,8 +78,8 @@ controller_server: batch_size: 1000 vx_std: 0.22 vy_std: 0.0 - wz_std: 0.4 - vx_max: 0.75 + wz_std: 0.5 + vx_max: 0.40 vx_min: -0.75 vy_max: 0.0 wz_max: 2.0 @@ -92,16 +92,16 @@ controller_server: trajectory_step: 5 time_step: 3 AckermannConstraints: - min_turning_r: 0.4 + min_turning_r: 0.6 critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] - ConstraintCritic: + ConstraintCritic: enabled: true cost_power: 1 cost_weight: 4.0 GoalCritic: enabled: true cost_power: 1 - cost_weight: 5.0 + cost_weight: 3.0 threshold_to_consider: 1.4 GoalAngleCritic: enabled: true @@ -111,7 +111,7 @@ controller_server: PreferForwardCritic: enabled: true cost_power: 1 - cost_weight: 3.0 + cost_weight: 6.0 threshold_to_consider: 0.5 CostCritic: enabled: true @@ -140,7 +140,7 @@ controller_server: PathAngleCritic: enabled: true cost_power: 1 - cost_weight: 2.0 + cost_weight: 6.0 offset_from_furthest: 4 threshold_to_consider: 0.5 max_angle_to_furthest: 1.0 @@ -175,12 +175,12 @@ local_costmap: transform_tolerance: 0.2 default_obstacle_radius: 0.05 minimum_obstacle_radius: 0.02 - maximum_obstacle_radius: 0.50 + maximum_obstacle_radius: 0.06 extra_inflation: 0.02 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.2 always_send_full_costmap: True local_costmap_client: ros__parameters: @@ -245,7 +245,7 @@ planner_server: angle_quantization_bins: 72 analytic_expansion_ratio: 3.5 analytic_expansion_max_length: 3.0 - minimum_turning_radius: 0.40 + minimum_turning_radius: 0.60 reverse_penalty: 1.9 change_penalty: 1.0 non_straight_penalty: 1.2 @@ -324,7 +324,7 @@ velocity_smoother: smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [0.75, 0.0, 2.5] + max_velocity: [0.40, 0.0, 2.5] min_velocity: [-0.75, 0.0, -2.5] max_accel: [2.5, 0.0, 3.2] max_decel: [-0.5, 0.0, -0.5] diff --git a/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py b/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py index 391609d..0bec55e 100755 --- a/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py +++ b/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py @@ -123,7 +123,7 @@ def generate_launch_description(): description='Reserved: path to map YAML file'), DeclareLaunchArgument( 'enable_motion', - default_value='false', + default_value='true', description='Route Nav2 cmd_vel to the real base topic'), DeclareLaunchArgument( 'start_base', diff --git a/src/origincar_base/launch/wall_localization.launch.py b/src/origincar_base/launch/wall_localization.launch.py index 2f7ce7b..cd5e79c 100644 --- a/src/origincar_base/launch/wall_localization.launch.py +++ b/src/origincar_base/launch/wall_localization.launch.py @@ -15,7 +15,7 @@ def generate_launch_description(): DeclareLaunchArgument('combined_odom_topic', default_value='/odom_combined'), DeclareLaunchArgument( 'wall_config', - default_value='/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json'), + default_value='/home/sunrise/yiliao_ws/src/origincar_base/config/wall_map_calibration (2).json'), DeclareLaunchArgument('port', default_value='8772'), Node( diff --git a/src/origincar_base/src/origincar_base.cpp b/src/origincar_base/src/origincar_base.cpp index 8613cc6..ea906de 100644 --- a/src/origincar_base/src/origincar_base.cpp +++ b/src/origincar_base/src/origincar_base.cpp @@ -126,7 +126,7 @@ void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDr void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux) { - RCLCPP_INFO(this->get_logger(), "linarx: %.2f, angularz: %.2f ", twist_aux->linear.x, twist_aux->angular.z); +// RCLCPP_INFO(this->get_logger(), "linarx: %.2f, angularz: %.2f ", twist_aux->linear.x, twist_aux->angular.z); std::cout << "linerx" << twist_aux->linear.x << std::endl; std::cout << "angular" << twist_aux->angular.z << std::endl; short transition; diff --git a/src/vlm_detect/launch/local_vlm_adapter.launch.py b/src/vlm_detect/launch/local_vlm_adapter.launch.py new file mode 100644 index 0000000..4347570 --- /dev/null +++ b/src/vlm_detect/launch/local_vlm_adapter.launch.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Launch local VLM adapter backed by hobot_llamacpp. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + use_tts = LaunchConfiguration("use_tts") + use_hobot_llamacpp = LaunchConfiguration("use_hobot_llamacpp") + config_file = LaunchConfiguration("config_file") + + image_topic = LaunchConfiguration("image_topic") + trigger_topic = LaunchConfiguration("trigger_topic") + trigger_sign = LaunchConfiguration("trigger_sign") + prompt_text = LaunchConfiguration("prompt_text") + result_topic = LaunchConfiguration("result_topic") + + llamacpp_prompt_topic = LaunchConfiguration("llamacpp_prompt_topic") + llamacpp_image_topic = LaunchConfiguration("llamacpp_image_topic") + llamacpp_result_topic = LaunchConfiguration("llamacpp_result_topic") + llamacpp_text_topic = LaunchConfiguration("llamacpp_text_topic") + llamacpp_vit_model_file_name = LaunchConfiguration("llamacpp_vit_model_file_name") + llamacpp_gguf_model_file_name = LaunchConfiguration("llamacpp_gguf_model_file_name") + llamacpp_model_type = LaunchConfiguration("llamacpp_model_type") + llamacpp_threads = LaunchConfiguration("llamacpp_threads") + + audio_sink = LaunchConfiguration("audio_sink") + tts_speed = LaunchConfiguration("tts_speed") + + local_adapter = Node( + package="vlm_detect", + executable="local_vlm_adapter", + name="local_vlm_adapter", + output="screen", + parameters=[ + config_file, + { + "image_topic": image_topic, + "trigger_topic": trigger_topic, + "trigger_sign": trigger_sign, + "prompt_text": prompt_text, + "result_topic": result_topic, + "llamacpp_prompt_topic": llamacpp_prompt_topic, + "llamacpp_image_topic": llamacpp_image_topic, + "llamacpp_result_topic": llamacpp_result_topic, + }, + ], + ) + + hobot_llamacpp = Node( + package="hobot_llamacpp", + executable="hobot_llamacpp", + name="hobot_llamacpp", + output="screen", + condition=IfCondition(use_hobot_llamacpp), + parameters=[ + { + "feed_type": 1, + "is_shared_mem_sub": 0, + "pre_infer": 0, + "llm_threads": llamacpp_threads, + "model_type": llamacpp_model_type, + "user_prompt": "", + "system_prompt": "You are a helpful assistant.", + "ros_img_sub_topic_name": llamacpp_image_topic, + "ros_string_sub_topic_name": llamacpp_prompt_topic, + "ai_msg_pub_topic_name": llamacpp_result_topic, + "text_msg_pub_topic_name": llamacpp_text_topic, + "model_file_name": llamacpp_vit_model_file_name, + "llm_model_name": llamacpp_gguf_model_file_name, + } + ], + arguments=["--ros-args", "--log-level", "warn"], + ) + + tts_server = Node( + package="vlm_detect", + executable="tts_server", + name="tts_server", + output="screen", + condition=IfCondition(use_tts), + parameters=[ + config_file, + { + "audio_sink": audio_sink, + "tts_speed": tts_speed, + }, + ], + ) + + return LaunchDescription( + [ + DeclareLaunchArgument("use_tts", default_value="true"), + DeclareLaunchArgument("use_hobot_llamacpp", default_value="true"), + DeclareLaunchArgument( + "config_file", + default_value=PathJoinSubstitution( + [ + get_package_share_directory("vlm_detect"), + "config", + "vlm_detect.yaml", + ] + ), + ), + DeclareLaunchArgument("image_topic", default_value="/image_mjpeg"), + DeclareLaunchArgument("trigger_topic", default_value="/sign4return"), + DeclareLaunchArgument("trigger_sign", default_value="9"), + DeclareLaunchArgument( + "prompt_text", + default_value="描述图片中有一个病人的特征，字数控制在20字以内。", + ), + DeclareLaunchArgument("result_topic", default_value="/vlm_result"), + DeclareLaunchArgument("llamacpp_prompt_topic", default_value="/prompt_text"), + DeclareLaunchArgument("llamacpp_image_topic", default_value="/llamacpp/image"), + DeclareLaunchArgument("llamacpp_result_topic", default_value="/llama_cpp_node"), + DeclareLaunchArgument("llamacpp_text_topic", default_value="/tts_text"), + DeclareLaunchArgument( + "llamacpp_vit_model_file_name", + default_value="/home/sunrise/hobot_llamacpp/src/models/vit_model_int16_v2.bin", + ), + DeclareLaunchArgument( + "llamacpp_gguf_model_file_name", + default_value="/home/sunrise/hobot_llamacpp/src/models/Qwen2.5-0.5B-Instruct-Q4_0.gguf", + ), + DeclareLaunchArgument("llamacpp_model_type", default_value="0"), + DeclareLaunchArgument("llamacpp_threads", default_value="6"), + DeclareLaunchArgument( + "audio_sink", + default_value="alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo", + ), + DeclareLaunchArgument("tts_speed", default_value="1.5"), + LogInfo(msg=["Local VLM backend: hobot_llamacpp"]), + local_adapter, + hobot_llamacpp, + tts_server, + ] + ) diff --git a/src/vlm_detect/package.xml b/src/vlm_detect/package.xml index 3f6c53c..5d47caa 100644 --- a/src/vlm_detect/package.xml +++ b/src/vlm_detect/package.xml @@ -7,6 +7,13 @@ root TODO: License declaration + ai_msgs + cv_bridge + origincar_msg + rclpy + sensor_msgs + std_msgs + ament_copyright ament_flake8 ament_pep257 diff --git a/src/vlm_detect/setup.py b/src/vlm_detect/setup.py index b4b075d..4673135 100644 --- a/src/vlm_detect/setup.py +++ b/src/vlm_detect/setup.py @@ -25,6 +25,7 @@ setup( entry_points={ 'console_scripts': [ 'vlm_node = vlm_detect.vlm_node:main', + 'local_vlm_adapter = vlm_detect.local_vlm_adapter_node:main', 'test_publisher = vlm_detect.test_publisher:main', 'tts_node = vlm_detect.tts_node:main', 'tts_server = vlm_detect.tts_server:main', diff --git a/src/vlm_detect/test/__pycache__/test_local_adapter_utils.cpython-310-pytest-6.2.5.pyc b/src/vlm_detect/test/__pycache__/test_local_adapter_utils.cpython-310-pytest-6.2.5.pyc new file mode 100644 index 0000000000000000000000000000000000000000..02f0f86af2086e0234e325426d6590e557671740 GIT binary patch literal 2120 zcma)7OK%)S5T2g*uGh{B;siouA!0HJyzwK!78r~GakB*B!X9W%w%hB0dC}c#$7WYZ z7%oVWxN!-ReL!Nbx#0sRj{JhTf(8Bq9DD$(de-Z4Ad+V_UsqSxR(Dr_{Y^GLUN_)* z|BLvOA56pe4Lj8be8vjA@&f>7FcKRrqFXbuOoJGS-Es(yn6aC9El=B4>?gHWEvdKa z(6^cMmC+hwF7u#mFrU?+9cSdO5!4SMudy1C;@sUR$z#46CS2xWn+Ikw$?q!>wiV5} zX!Be}SxOc7it~}AN<^Iw7c>b~d%cZWjvp5c58k)omA?Wg_%XuF2LxIR+m6ZX2c&N( z(ld5QpD?R$_DIkC)a+Y5OMa}(U5hzAYmG1${cGl)%{*oAI(x>3A!lIRRb-Edd9XaR zQo=muk0Rd0h@Jrv?_dO45aDYZqIGNT5mtL>UpD$StFy759TBh{g;BMhvqyHwmr!fl z=o5Hhh0K78HC78_Knh!J<-B_lcK643U;nQd*#G9wpMQd3Plckxl^l>G zb9s3{wr}0InQzSpvi|wdm|C7f>sBCdVmlkipx&QaF1>dGDJ;lcSc&Wm?81UTgjs}n zggZwQp_E)Gon*nMvHBT#KDH_x*o(j-2p<8JN?}vUTp(Z8g0Z0GMqrHtTbzRi^lt3&#PPJ)G zwNsxb5I?CT-3a3@hc8z~qf|{oQq3_1DlcZC7fclXu^#Gm7guSipfhC|K-Fdh-xQGo zH5|DLZ|KuIEF4r0sb#phexge=|D#6bJ;F24qN0heS5n!MzDN2B`L8D#>&E;If-A?% OIBlFaeajzfxc>s4Qa?5T literal 0 HcmV?d00001 diff --git a/src/vlm_detect/test/test_local_adapter_utils.py b/src/vlm_detect/test/test_local_adapter_utils.py new file mode 100644 index 0000000..26e274f --- /dev/null +++ b/src/vlm_detect/test/test_local_adapter_utils.py @@ -0,0 +1,25 @@ +from types import SimpleNamespace + +from vlm_detect.local_adapter_utils import extract_perception_text, is_trigger_match + + +def test_extracts_first_non_empty_target_type(): + msg = SimpleNamespace( + targets=[ + SimpleNamespace(type=""), + SimpleNamespace(type="病人坐在床边"), + ] + ) + + assert extract_perception_text(msg) == "病人坐在床边" + + +def test_returns_empty_string_when_no_target_text(): + msg = SimpleNamespace(targets=[SimpleNamespace(type="")]) + + assert extract_perception_text(msg) == "" + + +def test_trigger_match_casts_values_to_int(): + assert is_trigger_match("9", 9) + assert not is_trigger_match(8, 9) diff --git a/src/vlm_detect/vlm_backup/local_adapter_utils.py b/src/vlm_detect/vlm_backup/local_adapter_utils.py new file mode 100644 index 0000000..3e44ad8 --- /dev/null +++ b/src/vlm_detect/vlm_backup/local_adapter_utils.py @@ -0,0 +1,13 @@ +def extract_perception_text(msg): + for target in getattr(msg, "targets", []): + text = getattr(target, "type", "") + if text: + return text + return "" + + +def is_trigger_match(value, expected): + try: + return int(value) == int(expected) + except (TypeError, ValueError): + return False diff --git a/src/vlm_detect/vlm_backup/local_vlm_adapter_node.py b/src/vlm_detect/vlm_backup/local_vlm_adapter_node.py new file mode 100644 index 0000000..eeec997 --- /dev/null +++ b/src/vlm_detect/vlm_backup/local_vlm_adapter_node.py @@ -0,0 +1,212 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Local VLM adapter for hobot_llamacpp. + +This node keeps the vlm_detect external contract while delegating inference to +hobot_llamacpp over ROS topics. +""" + +import threading +import time + +import cv2 +import numpy as np +import rclpy +from ai_msgs.msg import PerceptionTargets +from cv_bridge import CvBridge +from origincar_msg.srv import Speak +from rclpy.node import Node +from sensor_msgs.msg import CompressedImage, Image +from std_msgs.msg import Int32, String + +from .local_adapter_utils import extract_perception_text, is_trigger_match + + +class LocalVLMAdapter(Node): + def __init__(self): + super().__init__("local_vlm_adapter") + + self.declare_parameter("image_topic", "/image_mjpeg") + self.declare_parameter("trigger_topic", "/sign4return") + self.declare_parameter("trigger_sign", 9) + self.declare_parameter( + "prompt_text", "请描述这张图片的内容，用一句简短的话概括，不超过20个字。" + ) + self.declare_parameter("result_topic", "/vlm_result") + self.declare_parameter("llamacpp_prompt_topic", "/prompt_text") + self.declare_parameter("llamacpp_image_topic", "/llamacpp/image") + self.declare_parameter("llamacpp_result_topic", "/llama_cpp_node") + self.declare_parameter("tts_service", "/tts/speak") + self.declare_parameter("enable_tts", True) + self.declare_parameter("inference_timeout_sec", 90.0) + self.declare_parameter("publish_delay_sec", 0.05) + + image_topic = self.get_parameter("image_topic").value + trigger_topic = self.get_parameter("trigger_topic").value + self.trigger_sign = self.get_parameter("trigger_sign").value + self.prompt_text = self.get_parameter("prompt_text").value + result_topic = self.get_parameter("result_topic").value + self.inference_timeout_sec = float( + self.get_parameter("inference_timeout_sec").value + ) + self.publish_delay_sec = float(self.get_parameter("publish_delay_sec").value) + self.enable_tts = bool(self.get_parameter("enable_tts").value) + + llamacpp_prompt_topic = self.get_parameter("llamacpp_prompt_topic").value + llamacpp_image_topic = self.get_parameter("llamacpp_image_topic").value + llamacpp_result_topic = self.get_parameter("llamacpp_result_topic").value + tts_service = self.get_parameter("tts_service").value + + self.bridge = CvBridge() + self.latest_image_msg = None + self.image_lock = threading.Lock() + self.inference_lock = threading.Lock() + self.waiting_for_result = False + self.pending_result = "" + self.result_event = threading.Event() + + self.image_sub = self.create_subscription( + CompressedImage, image_topic, self.image_callback, 10 + ) + self.trigger_sub = self.create_subscription( + Int32, trigger_topic, self.trigger_callback, 10 + ) + self.llamacpp_result_sub = self.create_subscription( + PerceptionTargets, llamacpp_result_topic, self.llamacpp_result_callback, 10 + ) + + self.prompt_pub = self.create_publisher(String, llamacpp_prompt_topic, 10) + self.image_pub = self.create_publisher(Image, llamacpp_image_topic, 10) + self.result_pub = self.create_publisher(String, result_topic, 10) + + self.tts_client = self.create_client(Speak, tts_service) + + self.get_logger().info( + "Local VLM adapter ready | image=%s | trigger=%s(sign=%s) | " + "prompt=%s | image_out=%s | result_in=%s | result_out=%s" + % ( + image_topic, + trigger_topic, + self.trigger_sign, + llamacpp_prompt_topic, + llamacpp_image_topic, + llamacpp_result_topic, + result_topic, + ) + ) + + def image_callback(self, msg): + with self.image_lock: + self.latest_image_msg = msg + + def trigger_callback(self, msg): + if not is_trigger_match(msg.data, self.trigger_sign): + return + + with self.inference_lock: + if self.waiting_for_result: + self.get_logger().warning("Local VLM inference already running") + return + self.waiting_for_result = True + self.pending_result = "" + self.result_event.clear() + + thread = threading.Thread(target=self._run_inference_once, daemon=True) + thread.start() + + def llamacpp_result_callback(self, msg): + text = extract_perception_text(msg) + if not text: + return + with self.inference_lock: + if not self.waiting_for_result: + return + self.pending_result = text + self.result_event.set() + + def _run_inference_once(self): + try: + image_msg = self._build_image_msg_from_latest() + if image_msg is None: + self.get_logger().warning("No cached image available for local VLM") + return + + prompt_msg = String() + prompt_msg.data = self.prompt_text + self.prompt_pub.publish(prompt_msg) + time.sleep(self.publish_delay_sec) + self.image_pub.publish(image_msg) + + self.get_logger().info("Local VLM request sent to hobot_llamacpp") + if not self.result_event.wait(timeout=self.inference_timeout_sec): + self.get_logger().error( + "Timed out waiting for hobot_llamacpp result after %.1fs" + % self.inference_timeout_sec + ) + return + + with self.inference_lock: + result = self.pending_result + + result_msg = String() + result_msg.data = result + self.result_pub.publish(result_msg) + self._speak_async(result) + self.get_logger().info("Local VLM result: %s" % result) + finally: + with self.inference_lock: + self.waiting_for_result = False + self.pending_result = "" + self.result_event.clear() + + def _build_image_msg_from_latest(self): + with self.image_lock: + compressed = self.latest_image_msg + if compressed is None: + return None + + np_arr = np.frombuffer(compressed.data, np.uint8) + cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + if cv_image is None: + self.get_logger().error("Failed to decode cached compressed image") + return None + + image_msg = self.bridge.cv2_to_imgmsg(cv_image, encoding="bgr8") + image_msg.header = compressed.header + return image_msg + + def _speak_async(self, text): + if not self.enable_tts: + return + if not self.tts_client.service_is_ready(): + self.get_logger().warning("TTS service not available") + return + req = Speak.Request() + req.text = text + future = self.tts_client.call_async(req) + future.add_done_callback(self._tts_done_callback) + + def _tts_done_callback(self, future): + try: + resp = future.result() + if not resp.success: + self.get_logger().warning("TTS failed: %s" % resp.message) + except Exception as exc: + self.get_logger().error("TTS call error: %s" % exc) + + +def main(args=None): + rclpy.init(args=args) + node = LocalVLMAdapter() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/vlm_detect/vlm_detect/qr_tts_bridge.py b/src/vlm_detect/vlm_backup/qr_tts_bridge.py similarity index 100% rename from src/vlm_detect/vlm_detect/qr_tts_bridge.py rename to src/vlm_detect/vlm_backup/qr_tts_bridge.py diff --git a/src/vlm_detect/vlm_detect/tts_node.py b/src/vlm_detect/vlm_backup/tts_node.py similarity index 100% rename from src/vlm_detect/vlm_detect/tts_node.py rename to src/vlm_detect/vlm_backup/tts_node.py diff --git a/src/vlm_detect/vlm_detect/__pycache__/local_adapter_utils.cpython-310.pyc b/src/vlm_detect/vlm_detect/__pycache__/local_adapter_utils.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..15a560fa796aa7be4301d63b94308ca0245facfa GIT binary patch literal 568 zcmYjN&1%~~5T04dwjAtYibJox3PSV(p%nTdW+% z_L{fIrJa>%@xaW^x6I%7)${oou)X|Q-hDuTM-NU-FnG;wmmCx*`~_b?2^H{*DqKT6 z%DC^p1)Po8->}=vAgtm0SOq*ST)7EJlWVdr zq_)uxVssP6lE}n?3o4tt@le7QdQZQ>M7-$fI@#2fPV8=L3agX0C<|Gq4>qwTOYX}m zRho20r)gPdvP`9t4QZ3^s3`4a)4CTt)yRy}Mw?7GRMgwlZ}LR#jKczF7-EFtPbiER zF!MV7GoMe{dkro)`7!k>bUpVDzlqPxHv~cwx1v2q=8tg0Dy`uGVzGMfX%<^@(~m5` zJ7a3&&OXU