标定+USB摄像头启动

2026-07-12 13:52:32 +08:00
parent b044f6f294
commit f05c427efe
42 changed files with 1259 additions and 9964 deletions
--- a/src/car_image_proc/CMakeLists.txt
+++ b/src/car_image_proc/CMakeLists.txt
@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.8)
+project(car_image_proc)
+
+find_package(ament_cmake REQUIRED)
+
+# This package currently only provides launch files.
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
--- a/src/car_image_proc/launch/image_proc.launch.py
+++ b/src/car_image_proc/launch/image_proc.launch.py
@@ -0,0 +1,41 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    image_topic_arg = DeclareLaunchArgument(
+        'image_topic',
+        default_value='/image',
+        description='Input raw image topic.'
+    )
+    camera_info_topic_arg = DeclareLaunchArgument(
+        'camera_info_topic',
+        default_value='/camera_info',
+        description='Input camera info topic.'
+    )
+    image_rect_topic_arg = DeclareLaunchArgument(
+        'image_rect_topic',
+        default_value='/image_rect',
+        description='Output rectified image topic.'
+    )
+
+    rectify_node = Node(
+        package='image_proc',
+        executable='rectify_node',
+        name='rectify_node',
+        output='screen',
+        remappings=[
+            ('image', LaunchConfiguration('image_topic')),
+            ('camera_info', LaunchConfiguration('camera_info_topic')),
+            ('image_rect', LaunchConfiguration('image_rect_topic')),
+        ],
+    )
+
+    return LaunchDescription([
+        image_topic_arg,
+        camera_info_topic_arg,
+        image_rect_topic_arg,
+        rectify_node,
+    ])
--- a/src/car_image_proc/package.xml
+++ b/src/car_image_proc/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>car_image_proc</name>
+  <version>0.0.1</version>
+  <description>Launch wrapper for rectifying the car camera image stream.</description>
+
+  <maintainer email="sunrise@example.com">sunrise</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>image_proc</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>rclcpp_components</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/src/car_usb_cam/config/usb_camera_calibration.yaml
+++ b/src/car_usb_cam/config/usb_camera_calibration.yaml
@@ -0,0 +1,30 @@
+image_width: 1920
+image_height: 1080
+camera_name: usb_camera
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [281.59527801, 0.0, 364.60571628,
+         0.0, 285.67845685, 204.92614855,
+         0.0, 0.0, 1.0]
+
+distortion_model: plumb_bob
+
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [ 0.03532389,-0.05576607, 0.00027529, 0.00241747, 0.01024998]
+
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.0, 0.0, 0.0,
+         0.0, 1.0, 0.0,
+         0.0, 0.0, 1.0]
+
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [281.59527801, 0.0, 364.60571628, 0.0,
+         0.0, 285.67845685, 204.92614855, 0.0,
+         0.0, 0.0, 1.0, 0.0]
--- a/src/car_usb_cam/launch/pycache/hobot_usb_cam.launch.cpython-310.pyc
+++ b/src/car_usb_cam/launch/pycache/hobot_usb_cam.launch.cpython-310.pyc
--- a/src/car_usb_cam/launch/pycache/hobot_usb_cam_websocket.launch.cpython-310.pyc
+++ b/src/car_usb_cam/launch/pycache/hobot_usb_cam_websocket.launch.cpython-310.pyc
--- a/src/car_usb_cam/launch/hobot_usb_cam.launch.py
+++ b/src/car_usb_cam/launch/hobot_usb_cam.launch.py
@@ -0,0 +1,93 @@
+# Copyright (c) 2024，D-Robotics.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python import get_package_share_directory
+from ament_index_python.packages import get_package_prefix
+import os
+
+def generate_launch_description():
+    config_file_path = os.path.join(
+        get_package_prefix('hobot_usb_cam'),
+        "/home/sunrise/yiliao_ws/src/car_usb_cam/config/usb_camera_calibration.yaml")
+    print("config_file_path is ", config_file_path)
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'usb_camera_calibration_file_path',
+            default_value=TextSubstitution(text=str(config_file_path)),
+            description='camera calibration file path'),
+        DeclareLaunchArgument(
+            'usb_frame_id',
+            default_value='default_usb_cam',
+            description='image message frame_id'),
+        DeclareLaunchArgument(
+            'usb_framerate',
+            default_value='30',
+            description='framerate'),
+        DeclareLaunchArgument(
+            'usb_image_height',
+            default_value='450',
+            description='image height'),
+        DeclareLaunchArgument(
+            'usb_image_width',
+            default_value='800',
+            description='image width'),
+        DeclareLaunchArgument(
+            'usb_io_method',
+            default_value='mmap',
+            description='io_method, mmap/read/userptr'),
+        DeclareLaunchArgument(
+            'usb_pixel_format',
+            default_value='yuyv2rgb',
+            description='pixel format, mjpeg/yuyv2rgb'),
+        DeclareLaunchArgument(
+            'usb_video_device',
+            default_value='/dev/video0',
+            description='usb camera device'),
+        DeclareLaunchArgument(
+            'usb_zero_copy',
+            default_value='False',
+            description='use zero copy or not'),
+        # 启动零拷贝环境配置node
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(
+                    get_package_share_directory('hobot_shm'),
+                    'launch/hobot_shm.launch.py'))
+        ),
+        Node(
+            package='hobot_usb_cam',
+            executable='hobot_usb_cam',
+            name='hobot_usb_cam',
+            parameters=[
+                {"camera_calibration_file_path": LaunchConfiguration(
+                    'usb_camera_calibration_file_path')},
+                {"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']
+        )
+    ])
--- a/src/car_usb_cam/launch/hobot_usb_cam_websocket.launch.py
+++ b/src/car_usb_cam/launch/hobot_usb_cam_websocket.launch.py
@@ -0,0 +1,94 @@
+# Copyright (c) 2024，D-Robotics.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python import get_package_share_directory
+
+
+def generate_launch_description():
+    camera_node = None
+
+    print("using usb camera")
+    # using usb cam publish image
+    usb_cam_device_arg = DeclareLaunchArgument(
+        'device',
+        default_value='/dev/video0',
+        description='usb camera device')
+
+    usb_node = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory('car_usb_cam'),
+                'launch/hobot_usb_cam.launch.py')),
+        launch_arguments={
+            'usb_image_width': '800',
+            'usb_image_height': '450',
+            'usb_framerate': '30',
+            'usb_pixel_format': 'yuyv2rgb',# yuyv2rgb
+            'usb_zero_copy': 'False',
+            'usb_video_device': LaunchConfiguration('device')
+        }.items()
+    )
+
+    # nv12->jpeg
+    jpeg_codec_node = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory('hobot_codec'),
+                'launch/hobot_codec_encode.launch.py')),
+        launch_arguments={
+            #'codec_in_mode': 'shared_mem',
+            'codec_in_mode': 'ros',
+            'codec_in_format': 'rgb8',#rgb8
+            'codec_out_mode': 'ros', #
+            #'codec_sub_topic': '/hbmem_img',
+            'codec_sub_topic': '/image',
+            'codec_pub_topic': '/image_mjpeg' # 原image_mjpeg
+        }.items()
+    )
+    # web
+    web_node = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory('websocket'),
+                'launch/websocket.launch.py')),
+        launch_arguments={
+            'websocket_image_topic': '/image_mjpeg',
+            'websocket_only_show_image': 'True'
+        }.items()
+    )
+
+
+    return LaunchDescription([
+        # 启动零拷贝环境配置node
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(
+                    get_package_share_directory('hobot_shm'),
+                    'launch/hobot_shm.launch.py'))
+        ),
+        usb_cam_device_arg,
+        usb_node,
+        # image codec
+        jpeg_codec_node,
+        # web display
+        # web_node
+    ])
--- a/src/car_usb_cam/local_setup.bash
+++ b/src/car_usb_cam/local_setup.bash
@@ -0,0 +1,46 @@
+# generated from ament_package/template/package_level/local_setup.bash.in
+
+# source local_setup.sh from same directory as this file
+_this_path=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" && pwd)
+# provide AMENT_CURRENT_PREFIX to shell script
+AMENT_CURRENT_PREFIX=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." && pwd)
+# store AMENT_CURRENT_PREFIX to restore it before each environment hook
+_package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX
+
+# trace output
+if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
+  echo "# . \"$_this_path/local_setup.sh\""
+fi
+. "$_this_path/local_setup.sh"
+unset _this_path
+
+# unset AMENT_ENVIRONMENT_HOOKS
+# if not appending to them for return
+if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
+  unset AMENT_ENVIRONMENT_HOOKS
+fi
+
+# restore AMENT_CURRENT_PREFIX before evaluating the environment hooks
+AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
+# list all environment hooks of this package
+
+# source all shell-specific environment hooks of this package
+# if not returning them
+if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
+  _package_local_setup_IFS=$IFS
+  IFS=":"
+  for _hook in $AMENT_ENVIRONMENT_HOOKS; do
+    # restore AMENT_CURRENT_PREFIX for each environment hook
+    AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
+    # restore IFS before sourcing other files
+    IFS=$_package_local_setup_IFS
+    . "$_hook"
+  done
+  unset _hook
+  IFS=$_package_local_setup_IFS
+  unset _package_local_setup_IFS
+  unset AMENT_ENVIRONMENT_HOOKS
+fi
+
+unset _package_local_setup_AMENT_CURRENT_PREFIX
+unset AMENT_CURRENT_PREFIX
--- a/src/car_usb_cam/package.xml
+++ b/src/car_usb_cam/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>car_usb_cam</name>
+  <version>2.3.0</version>
+  <description>TogetheROS hobot usb camera</description>
+  <maintainer email="kairui.wang@d-robotics.cc">kairui</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>hbm_img_msgs</depend>
+  <depend>v4l-utils</depend>
+  <depend>yaml_cpp_vendor</depend>
+  <!-- Only required for MJPEG to RGB converison -->
+  <depend>ffmpeg</depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
+++ b/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
@@ -212,7 +212,7 @@ planner_server:
      angle_quantization_bins: 72
      analytic_expansion_ratio: 2.0
      analytic_expansion_max_length: 3.0
-      minimum_turning_radius: 0.35
+      minimum_turning_radius: 0.5
      reverse_penalty: 1.3
      change_penalty: 0.0
      non_straight_penalty: 0.0
--- a/src/origincar_base/include/origincar_base/origincar_base.h
+++ b/src/origincar_base/include/origincar_base/origincar_base.h
@@ -223,15 +223,12 @@ private:
 	Vel_Pos_Data Robot_Vel;
 	MPU6050_DATA Mpu6050_Data;
 	float Power_voltage;
-	float gyro_z_bias_intercept_;
-	float gyro_z_bias_slope_;
 	float gyro_z_raw_from_mcu_;
 	float gyro_z_after_startup_bias_;
 	float gyro_z_after_median_;
 	float gyro_z_filtered_pre_bias_;
 	float gyro_z_bias_model_;
 	float gyro_z_final_for_yaw_;
-	double gyro_z_bias_elapsed_s_;
 	float gyro_z_low_pass_alpha_;
 	float gyro_z_low_pass_;
 	std::array<float, 3> gyro_z_median_window_;
--- a/src/origincar_base/launch/base_serial.launch.py
+++ b/src/origincar_base/launch/base_serial.launch.py
@@ -11,8 +11,6 @@ def generate_launch_description():
         'cmd_vel': 'cmd_vel',
         'akm_cmd_vel': 'none',
         'product_number': 0,
-         'gyro_z_bias_intercept': 0.0,
-         'gyro_z_bias_slope': 0.0,
         'gyro_z_low_pass_alpha': 0.6,
         'odom_pose_cov_x': 0.01,
         'odom_pose_cov_y': 0.01,
--- a/src/origincar_base/src/origincar_base.cpp
+++ b/src/origincar_base/src/origincar_base.cpp
@@ -5,6 +5,7 @@
 #include "robot_localization/srv/set_pose.hpp"
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <algorithm>
+#include <cmath>

 using std::placeholders::_1;
 using namespace std;
@@ -16,6 +17,7 @@ float gyro_z_sum = 0;
 namespace
 {
  constexpr float kDefaultGyroZLowPassAlpha = 0.6f;
+  constexpr float kStationaryLinearSpeedThreshold = 0.01f;

  float median3(float a, float b, float c)
  {
@@ -33,6 +35,12 @@ namespace
    }
    return b;
  }
+
+  bool shouldForceZeroYawRate(float vx, float vy)
+  {
+    return std::fabs(vx) < kStationaryLinearSpeedThreshold &&
+           std::fabs(vy) < kStationaryLinearSpeedThreshold;
+  }
 }

 int main(int argc, char *argv[])
@@ -370,6 +378,8 @@ bool origincar_base::Get_Sensor_Data()
          gyro_z_after_startup_bias_ = 0.0f;
          gyro_z_after_median_ = 0.0f;
          gyro_z_filtered_pre_bias_ = 0.0f;
+          gyro_z_bias_model_ = 0.0f;
+          gyro_z_final_for_yaw_ = 0.0f;
          Mpu6050.angular_velocity.z = 0;
        }
        else
@@ -405,8 +415,13 @@ bool origincar_base::Get_Sensor_Data()
                (1.0f - gyro_z_low_pass_alpha_) * median_gyro_z;
          }
          gyro_z_filtered_pre_bias_ = gyro_z_low_pass_;
-          Mpu6050.angular_velocity.z = gyro_z_filtered_pre_bias_;
-          // RCLCPP_INFO(this->get_logger(),"gyro_z_sum: %.2f, err: %.2f, gyroz: %.2f ", gyro_z_sum ,(gyro_z_sum / (float)Init_imu_num), Mpu6050.angular_velocity.z);
+          gyro_z_bias_model_ = 0.0f;
+          gyro_z_final_for_yaw_ = gyro_z_filtered_pre_bias_;
+          if (shouldForceZeroYawRate(Robot_Vel.X, Robot_Vel.Y))
+          {
+            gyro_z_final_for_yaw_ = 0.0f;
+          }
+          Mpu6050.angular_velocity.z = gyro_z_final_for_yaw_;
        }
        Robot_Vel.Z = Mpu6050.angular_velocity.z;
        transition_16 = 0;
@@ -433,12 +448,6 @@ void origincar_base::Control()
    Sampling_Time = (current_time - last_time).seconds();
    if (true == Get_Sensor_Data())
    {
-      gyro_z_bias_elapsed_s_ += Sampling_Time;
-      gyro_z_bias_model_ =
-          gyro_z_bias_intercept_ + gyro_z_bias_slope_ * static_cast<float>(gyro_z_bias_elapsed_s_);
-      gyro_z_final_for_yaw_ = gyro_z_filtered_pre_bias_ - gyro_z_bias_model_;
-      Mpu6050.angular_velocity.z = gyro_z_final_for_yaw_;
-      Robot_Vel.Z = gyro_z_final_for_yaw_;
      Robot_Pos.X += 1.03 * (Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;
      Robot_Pos.Y += 1.01 * (Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time; // 1.125
      Robot_Pos.Z += Robot_Vel.Z * Sampling_Time;
@@ -460,15 +469,12 @@ origincar_base::origincar_base()
  memset(&Receive_Data, 0, sizeof(Receive_Data));
  memset(&Send_Data, 0, sizeof(Send_Data));
  memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data));
-  gyro_z_bias_intercept_ = 0.0f;
-  gyro_z_bias_slope_ = 0.0f;
  gyro_z_raw_from_mcu_ = 0.0f;
  gyro_z_after_startup_bias_ = 0.0f;
  gyro_z_after_median_ = 0.0f;
  gyro_z_filtered_pre_bias_ = 0.0f;
  gyro_z_bias_model_ = 0.0f;
  gyro_z_final_for_yaw_ = 0.0f;
-  gyro_z_bias_elapsed_s_ = 0.0;
  gyro_z_median_window_.fill(0.0f);
  gyro_z_median_index_ = 0;
  gyro_z_median_count_ = 0;
@@ -486,8 +492,6 @@ origincar_base::origincar_base()
  this->declare_parameter<std::string>("robot_frame_id", "base_link");
  this->declare_parameter<std::string>("gyro_frame_id", "gyro_link");
  this->declare_parameter<bool>("publish_tf", false);
-  this->declare_parameter<double>("gyro_z_bias_intercept", 0.0);
-  this->declare_parameter<double>("gyro_z_bias_slope", 0.0);
  this->declare_parameter<double>("gyro_z_low_pass_alpha", kDefaultGyroZLowPassAlpha);

  // Odom covariance parameters (tunable via YAML)
@@ -503,10 +507,6 @@ origincar_base::origincar_base()
  this->get_parameter("robot_frame_id", robot_frame_id);
  this->get_parameter("gyro_frame_id", gyro_frame_id);
  this->get_parameter("publish_tf", publish_tf_);
-  gyro_z_bias_intercept_ =
-      static_cast<float>(this->get_parameter("gyro_z_bias_intercept").as_double());
-  gyro_z_bias_slope_ =
-      static_cast<float>(this->get_parameter("gyro_z_bias_slope").as_double());
  gyro_z_low_pass_alpha_ =
      static_cast<float>(this->get_parameter("gyro_z_low_pass_alpha").as_double());
  if (gyro_z_low_pass_alpha_ < 0.0f)
--- a/src/vlm_detect/config/vlm_detect.yaml
+++ b/src/vlm_detect/config/vlm_detect.yaml
@@ -0,0 +1,36 @@
+# vlm_detect 参数配置
+# 使用: ros2 launch vlm_detect vlm_detect.launch.py
+
+vlm_node:
+  ros__parameters:
+    # VLM 推理服务地址
+    vlm_host: "http://192.168.10.189:8000"
+    # 模型名称 (OpenAI 格式)
+    vlm_model: "./OpenGVLab/InternVL3-1B/"
+    # 订阅的压缩图像话题
+    image_topic: "/image_mjpeg"
+    # 订阅的触发信号话题
+    trigger_topic: "/sign4return"
+    # 触发信号值
+    trigger_sign: 9
+    # 发布结果的话题
+    result_topic: "/vlm_result"
+    # 发送给 VLM 的提示词
+    prompt_text: "描述图片中有一个病人的特征，字数控制在20字以内。"
+    # 最大输出 token 数
+    max_tokens: 100
+
+tts_node:
+  ros__parameters:
+    # VLM 推理服务地址 (需与 vlm_node 一致)
+    vlm_host: "http://192.168.10.189:8000"
+    # 音频输出设备 (PulseAudio sink)
+    audio_sink: "alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo"
+    # 订阅 VLM 结果的话题 (需与 vlm_node 一致)
+    result_topic: "/vlm_result"
+    # TTS 语音 (edge-tts 语音名)
+    tts_voice: "zh-CN-XiaoxiaoNeural"
+    # 临时 MP3 存储路径
+    tmp_mp3_path: "/tmp/tts_out.mp3"
+    # 播放速度 (ffplay atempo, 范围 0.5~2.0)
+    tts_speed: 1.5
--- a/src/vlm_detect/launch/vlm_detect.launch.py
+++ b/src/vlm_detect/launch/vlm_detect.launch.py
@@ -0,0 +1,152 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+vlm_detect 联合启动文件
+同时启动 vlm_node (图生文) 和 tts_node (语音播报)
+
+用法:
+  ros2 launch vlm_detect vlm_detect.launch.py                          # 默认配置
+  ros2 launch vlm_detect vlm_detect.launch.py vlm_host:=http://...     # 覆盖 VLM 服务地址
+  ros2 launch vlm_detect vlm_detect.launch.py use_tts:=false           # 只启动 vlm_node
+"""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+
+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
+
+
+def generate_launch_description():
+
+    # ==================== Launch 参数 ====================
+    use_tts = LaunchConfiguration('use_tts')
+
+    config_file = LaunchConfiguration('config_file')
+
+    # vlm_node 可覆盖参数
+    vlm_host = LaunchConfiguration('vlm_host')
+    vlm_model = LaunchConfiguration('vlm_model')
+    image_topic = LaunchConfiguration('image_topic')
+    trigger_topic = LaunchConfiguration('trigger_topic')
+    trigger_sign = LaunchConfiguration('trigger_sign')
+    result_topic = LaunchConfiguration('result_topic')
+    prompt_text = LaunchConfiguration('prompt_text')
+    max_tokens = LaunchConfiguration('max_tokens')
+
+    # tts_node 可覆盖参数
+    audio_sink = LaunchConfiguration('audio_sink')
+    tts_voice = LaunchConfiguration('tts_voice')
+    tts_speed = LaunchConfiguration('tts_speed')
+
+    # ==================== 声明参数 ====================
+    declare_use_tts = DeclareLaunchArgument(
+        'use_tts', default_value='true',
+        description='是否同时启动 TTS 语音播报节点')
+
+    declare_config_file = DeclareLaunchArgument(
+        'config_file',
+        default_value=PathJoinSubstitution([
+            get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml'
+        ]),
+        description='YAML 配置文件路径')
+
+    # vlm_node 参数
+    declare_vlm_host = DeclareLaunchArgument(
+        'vlm_host', default_value='http://192.168.10.189:8000',
+        description='VLM 推理服务地址')
+    declare_vlm_model = DeclareLaunchArgument(
+        'vlm_model', default_value='./OpenGVLab/InternVL3-1B/',
+        description='VLM 模型名称')
+    declare_image_topic = DeclareLaunchArgument(
+        'image_topic', default_value='/image_mjpeg',
+        description='订阅的压缩图像话题')
+    declare_trigger_topic = DeclareLaunchArgument(
+        'trigger_topic', default_value='/sign4return',
+        description='订阅的触发信号话题')
+    declare_trigger_sign = DeclareLaunchArgument(
+        'trigger_sign', default_value='9',
+        description='触发信号值 (Int32)')
+    declare_result_topic = DeclareLaunchArgument(
+        'result_topic', default_value='/vlm_result',
+        description='发布 VLM 结果的话题')
+    declare_prompt_text = DeclareLaunchArgument(
+        'prompt_text', default_value='描述图片中有一个病人的特征，字数控制在20字以内。',
+        description='发送给 VLM 的提示词')
+    declare_max_tokens = DeclareLaunchArgument(
+        'max_tokens', default_value='100',
+        description='最大输出 token 数')
+
+    # tts_node 参数
+    declare_audio_sink = DeclareLaunchArgument(
+        'audio_sink',
+        default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo',
+        description='音频输出设备 (PulseAudio sink)')
+    declare_tts_voice = DeclareLaunchArgument(
+        'tts_voice', default_value='zh-CN-XiaoxiaoNeural',
+        description='TTS 语音名称 (edge-tts)')
+    declare_tts_speed = DeclareLaunchArgument(
+        'tts_speed', default_value='1.5',
+        description='播放速度倍率 (0.5~2.0)')
+
+    # ==================== 节点 ====================
+    vlm_node = Node(
+        package='vlm_detect',
+        executable='vlm_node',
+        name='vlm_detect',
+        output='screen',
+        parameters=[config_file,
+            {
+                'vlm_host': vlm_host,
+                'vlm_model': vlm_model,
+                'image_topic': image_topic,
+                'trigger_topic': trigger_topic,
+                'trigger_sign': trigger_sign,
+                'result_topic': result_topic,
+                'prompt_text': prompt_text,
+                'max_tokens': max_tokens,
+            }],
+    )
+
+    tts_node = Node(
+        package='vlm_detect',
+        executable='tts_node',
+        name='tts_node',
+        output='screen',
+        condition=IfCondition(use_tts),
+        parameters=[config_file,
+            {
+                'vlm_host': vlm_host,
+                'audio_sink': audio_sink,
+                'result_topic': result_topic,
+                'tts_voice': tts_voice,
+                'tts_speed': tts_speed,
+            }],
+    )
+
+    # ==================== 启动描述 ====================
+    return LaunchDescription([
+        # 参数声明
+        declare_use_tts,
+        declare_config_file,
+        declare_vlm_host,
+        declare_vlm_model,
+        declare_image_topic,
+        declare_trigger_topic,
+        declare_trigger_sign,
+        declare_result_topic,
+        declare_prompt_text,
+        declare_max_tokens,
+        declare_audio_sink,
+        declare_tts_voice,
+        declare_tts_speed,
+        # 节点
+        LogInfo(msg=['配置文件: ', config_file]),
+        LogInfo(msg=['VLM 服务: ', vlm_host]),
+        LogInfo(msg=['TTS 播报: ', use_tts]),
+        vlm_node,
+        tts_node,
+    ])
--- a/src/vlm_detect/setup.py
+++ b/src/vlm_detect/setup.py
@@ -1,3 +1,5 @@
+import os
+from glob import glob
 from setuptools import find_packages, setup

 package_name = 'vlm_detect'
@@ -10,19 +12,21 @@ setup(
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/launch', glob('launch/*.launch.py')),
+        ('share/' + package_name + '/config', glob('config/*.yaml')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='root',
    maintainer_email='root@todo.todo',
-    description='TODO: Package description',
+    description='VLM 图生文检测 + TTS 语音播报',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
-      'console_scripts': [
-        'vlm_node = vlm_detect.vlm_node:main',
+        'console_scripts': [
+            'vlm_node = vlm_detect.vlm_node:main',
            'test_publisher = vlm_detect.test_publisher:main',
            'tts_node = vlm_detect.tts_node:main',
-      ],
+        ],
    },
 )
--- a/src/vlm_detect/vlm_detect/pycache/init.cpython-310.pyc
+++ b/src/vlm_detect/vlm_detect/pycache/init.cpython-310.pyc
--- a/src/vlm_detect/vlm_detect/pycache/test_publisher.cpython-310.pyc
+++ b/src/vlm_detect/vlm_detect/pycache/test_publisher.cpython-310.pyc
--- a/src/vlm_detect/vlm_detect/pycache/tts_node.cpython-310.pyc
+++ b/src/vlm_detect/vlm_detect/pycache/tts_node.cpython-310.pyc
--- a/src/vlm_detect/vlm_detect/pycache/vlm_node.cpython-310.pyc
+++ b/src/vlm_detect/vlm_detect/pycache/vlm_node.cpython-310.pyc
--- a/src/vlm_detect/vlm_detect/tts_node.py
+++ b/src/vlm_detect/vlm_detect/tts_node.py
@@ -1,42 +1,71 @@
 #!/usr/bin/env python3
+# -*- coding: utf-8 -*-
 import rclpy, subprocess, requests, os
 from rclpy.node import Node
 from std_msgs.msg import String

-VLM_HOST = "http://192.168.10.173:8000"
-# USB Audio Device (Card 1)
-AUDIO_SINK = "alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo"
-AUDIO_ENV = {**os.environ, "PULSE_SINK": AUDIO_SINK}
-
 class TTSNode(Node):
    def __init__(self):
        super().__init__("tts_node")
-        self.sub = self.create_subscription(String, "/vlm_result", self.callback, 10)
-        self.get_logger().info("TTS 播报节点已启动 (USB Audio Device, edge-tts 自然语音)")
+
+        self.declare_parameter('vlm_host', 'http://192.168.10.189:8000')
+        self.declare_parameter('audio_sink',
+            'alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
+        self.declare_parameter('result_topic', '/vlm_result')
+        self.declare_parameter('tts_voice', 'zh-CN-XiaoxiaoNeural')
+        self.declare_parameter('tmp_mp3_path', '/tmp/tts_out.mp3')
+        self.declare_parameter('tts_speed', 1.5)
+
+        self.vlm_host = self.get_parameter('vlm_host').value
+        audio_sink = self.get_parameter('audio_sink').value
+        result_topic = self.get_parameter('result_topic').value
+        self.tts_voice = self.get_parameter('tts_voice').value
+        self.tmp_mp3 = self.get_parameter('tmp_mp3_path').value
+        self.tts_speed = self.get_parameter('tts_speed').value
+
+        self.audio_env = {**os.environ, "PULSE_SINK": audio_sink}
+
+        self.sub = self.create_subscription(String, result_topic, self.callback, 10)
+        self.get_logger().info(
+            f"TTS 节点启动 | host={self.vlm_host} | sink={audio_sink} | "
+            f"voice={self.tts_voice} | speed={self.tts_speed}x"
+        )

    def callback(self, msg):
        text = msg.data
-        self.get_logger().info(f"播报: {text}")
+        self.get_logger().info(f"语音播报: {text}")
        try:
-            resp = requests.post(f"{VLM_HOST}/v1/tts",
-                json={"text": text, "voice": "zh-CN-XiaoxiaoNeural"}, timeout=60)
-            mp3 = "/tmp/tts_out.mp3"
-            with open(mp3, "wb") as f:
+            resp = requests.post(
+                f"{self.vlm_host}/v1/tts",
+                json={"text": text, "voice": self.tts_voice},
+                timeout=60
+            )
+            resp.raise_for_status()
+            with open(self.tmp_mp3, "wb") as f:
                f.write(resp.content)
-            subprocess.Popen(["ffplay", "-nodisp", "-autoexit", mp3],
+            speed_str = f"atempo={self.tts_speed}"
+            subprocess.Popen(
+                ["ffplay", "-nodisp", "-autoexit", "-af", speed_str, self.tmp_mp3],
                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
-                env=AUDIO_ENV)
+                env=self.audio_env
+            )
        except Exception as e:
-            self.get_logger().error(f"TTS 失败,降级 espeak: {e}")
-            subprocess.Popen(["espeak-ng", "-v", "zh", "-s", "150", text],
-                env=AUDIO_ENV)
+            self.get_logger().error(f"TTS 失败, 降级 espeak: {e}")
+            subprocess.Popen(
+                ["espeak-ng", "-v", "zh", "-s", "150", text],
+                env=self.audio_env
+            )

 def main(args=None):
    rclpy.init(args=args)
    node = TTSNode()
-    try: rclpy.spin(node)
-    except KeyboardInterrupt: pass
-    finally: node.destroy_node(); rclpy.shutdown()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()

 if __name__ == "__main__":
    main()
--- a/src/vlm_detect/vlm_detect/vlm_node.py
+++ b/src/vlm_detect/vlm_detect/vlm_node.py
@@ -1,146 +1,128 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-import rclpy
-from rclpy.node import Node
-from std_msgs.msg import Int32, String
-from sensor_msgs.msg import CompressedImage
-from cv_bridge import CvBridge
-import cv2
-import base64
-import threading
-from openai import OpenAI
-import os
-import time
-import numpy as np
-
-class VLMProcessor(Node):
-    def __init__(self):
-        super().__init__('vlm_detect')
-        
-        # 初始化 OpenAI 客户端
-        self.client = OpenAI(
-            base_url="http://192.168.10.173:8000/v1",  # 本地 API 地址
-            api_key="EMPTY",  # 不需要真实 API key
-        )
-     
-        # ROS2 组件
-        self.bridge = CvBridge()
-        self.latest_image = None
-        self.image_lock = threading.Lock()
-        
-        # 订阅图像话题
-        self.image_sub = self.create_subscription(
-            CompressedImage,
-            '/image_mjpeg',
-            self.image_callback,
-            10
-        )
-        
-        # 订阅触发信号
-        self.sign_sub = self.create_subscription(
-            Int32,
-            '/sign4return',
-            self.sign_callback,
-            10
-        )
-        
-        # 发布结果
-        self.result_pub = self.create_publisher(
-            String,
-            '/vlm_result',
-            10
-        )
-        
-        self.get_logger().info("VLM Processor...")
-
-    def image_callback(self, msg):
-        """保存最新的图像"""
-        with self.image_lock:
-            try:
-                # bridge = CvBridge()
-                np_arr = np.frombuffer(msg.data, np.uint8)
-                # 使用 OpenCV 解码图像
-                cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
-                # cv_image =bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
-                self.latest_image = cv_image
-                self.get_logger().debug("recive picture")
-            except Exception as e:
-                self.get_logger().error(f"err picture: {e}")
-
-    def sign_callback(self, msg):
-        """处理触发信号"""
-        if msg.data == 9:
-            self.get_logger().info(f"收到触发信号 ({msg.data})，开始处理图像...")
-            
-            # 检查是否有可用图像
-            with self.image_lock:
-                if self.latest_image is None:
-                    self.get_logger().warning("没有可用图像")
-                    return
-                
-                # 保存临时图像文件
-                temp_path = "/tmp/vlm_temp_image.jpg"
-                cv2.imwrite(temp_path, self.latest_image)
-                self.get_logger().info(f"已保存临时图像: {temp_path}")
-            
-            # 处理图像
-            try:
-                description = self.process_image(temp_path)
-                self.get_logger().info(f"图像描述结果: {description}")
-                
-                # 发布结果
-                result_msg = String()
-                result_msg.data = description
-                self.result_pub.publish(result_msg)
-                
-                # 清理临时文件
-                os.remove(temp_path)
-            except Exception as e:
-                self.get_logger().error(f"处理图像时出错: {e}")
-
-    def process_image(self, image_path):
-        """使用 VLM 模型处理图像"""
-        # 读取并编码图像
-        with open(image_path, "rb") as image_file:
-            base64_image = base64.b64encode(image_file.read()).decode('utf-8')
-        
-        # 发送请求到 VLM 模型
-        start_time = time.time()
-        
-        response = self.client.chat.completions.create(
-            model="./OpenGVLab/InternVL3-1B/",
-            messages=[
-                {
-                    "role": "user",
-                    "content": [
-                        {"type": "text", "text": "描述图片中有一个病人的特征，字数控制在20字以内。"},
-                        {
-                            "type": "image_url",
-                            "image_url": {
-                                "url": f"data:image/jpeg;base64,{base64_image}"
-                            },
-                        },
-                    ]
-                }
-            ],
-            max_tokens=100,
-        )
-        
-        processing_time = time.time() - start_time
-        self.get_logger().info(f"VLM 处理耗时 {processing_time:.1f}s")
-        
-        return response.choices[0].message.content
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = VLMProcessor()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Int32, String
+from sensor_msgs.msg import CompressedImage
+from cv_bridge import CvBridge
+import cv2
+import base64
+import threading
+from openai import OpenAI
+import os
+import time
+import numpy as np
+
+class VLMProcessor(Node):
+    def __init__(self):
+        super().__init__('vlm_detect')
+
+        # 声明 ROS2 参数
+        self.declare_parameter('vlm_host', 'http://192.168.10.189:8000')
+        self.declare_parameter('vlm_model', './OpenGVLab/InternVL3-1B/')
+        self.declare_parameter('image_topic', '/image_mjpeg')
+        self.declare_parameter('trigger_topic', '/sign4return')
+        self.declare_parameter('trigger_sign', 9)
+        self.declare_parameter('result_topic', '/vlm_result')
+        self.declare_parameter('prompt_text', '描述图片中有一个病人的特征，字数控制在20字以内。')
+        self.declare_parameter('max_tokens', 100)
+
+        vlm_host = self.get_parameter('vlm_host').value
+        vlm_model = self.get_parameter('vlm_model').value
+        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
+        result_topic = self.get_parameter('result_topic').value
+        self.prompt_text = self.get_parameter('prompt_text').value
+        self.max_tokens = self.get_parameter('max_tokens').value
+
+        # 初始化 OpenAI 客户端
+        self.client = OpenAI(
+            base_url=f"{vlm_host}/v1",
+            api_key="EMPTY",
+        )
+        self.vlm_model = vlm_model
+
+        # ROS2 组件
+        self.bridge = CvBridge()
+        self.latest_image = None
+        self.image_lock = threading.Lock()
+
+        self.image_sub = self.create_subscription(
+            CompressedImage, image_topic, self.image_callback, 10
+        )
+        self.sign_sub = self.create_subscription(
+            Int32, trigger_topic, self.sign_callback, 10
+        )
+        self.result_pub = self.create_publisher(String, result_topic, 10)
+
+        self.get_logger().info(
+            f"VLM Processor 启动 | host={vlm_host} | model={vlm_model} | "
+            f"image={image_topic} | trigger={trigger_topic}(sign={self.trigger_sign})"
+        )
+
+    def image_callback(self, msg):
+        with self.image_lock:
+            try:
+                np_arr = np.frombuffer(msg.data, np.uint8)
+                cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+                self.latest_image = cv_image
+                self.get_logger().debug("图片已接收")
+            except Exception as e:
+                self.get_logger().error(f"图片接收错误: {e}")
+
+    def sign_callback(self, msg):
+        if msg.data == self.trigger_sign:
+            self.get_logger().info(f"收到触发信号 ({msg.data}), 开始处理...")
+            with self.image_lock:
+                if self.latest_image is None:
+                    self.get_logger().warning("无可用图片")
+                    return
+                temp_path = "/tmp/vlm_temp_image.jpg"
+                cv2.imwrite(temp_path, self.latest_image)
+                self.get_logger().info(f"临时图片已保存: {temp_path}")
+
+            try:
+                description = self.process_image(temp_path)
+                self.get_logger().info(f"图像描述: {description}")
+                result_msg = String()
+                result_msg.data = description
+                self.result_pub.publish(result_msg)
+                os.remove(temp_path)
+            except Exception as e:
+                self.get_logger().error(f"图像处理出错: {e}")
+
+    def process_image(self, image_path):
+        with open(image_path, "rb") as image_file:
+            base64_image = base64.b64encode(image_file.read()).decode('utf-8')
+
+        start_time = time.time()
+        response = self.client.chat.completions.create(
+            model=self.vlm_model,
+            messages=[{
+                "role": "user",
+                "content": [
+                    {"type": "text", "text": self.prompt_text},
+                    {"type": "image_url", "image_url": {
+                        "url": f"data:image/jpeg;base64,{base64_image}"
+                    }},
+                ]
+            }],
+            max_tokens=self.max_tokens,
+        )
+        self.get_logger().info(f"VLM 推理耗时 {time.time() - start_time:.1f}s")
+        return response.choices[0].message.content
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VLMProcessor()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()