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 0000000..0eaa20e Binary files /dev/null and b/src/car_usb_cam/test/__pycache__/test_unified_launch_config.cpython-310-pytest-6.2.5.pyc differ 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 0000000..02f0f86 Binary files /dev/null and b/src/vlm_detect/test/__pycache__/test_local_adapter_utils.cpython-310-pytest-6.2.5.pyc differ 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 0000000..15a560f Binary files /dev/null and b/src/vlm_detect/vlm_detect/__pycache__/local_adapter_utils.cpython-310.pyc differ