vlm和tts

2026-07-23 14:51:51 +08:00
parent 90761b40b2
commit a5cee5e142
35 changed files with 12694 additions and 168 deletions
--- a/master_launch_使用指南.md
+++ b/master_launch_使用指南.md
@@ -0,0 +1,227 @@
+# master_launch.py 使用指南
+
+## 启动顺序
+
+```
+t=0s  底盘驱动 + EKF + TF + IMU (origincar_bringup)
+t=2s  激光雷达 (lslidar_driver)
+t=5s  SLAM 建图 (slam_toolbox)
+t=8s  Nav2 导航栈
+t=10s VLM 图生文 + TTS 语音播报
+```
+
+每个阶段依赖前一个阶段就绪，属于**严格时间线启动**，不可调换。
+
+---
+
+## 参数一览
+
+| 参数 | 默认值 | 说明 |
+|---|---|---|
+| `use_base` | true | 底盘驱动 + EKF + TF |
+| `use_lidar` | true | 激光雷达 (lsn10) |
+| `use_slam` | true | slam_toolbox 建图 |
+| `use_nav2` | true | Nav2 导航栈 |
+| `use_vlm` | true | VLM 图生文 + TTS |
+| `use_tts` | true | TTS 语音播报开关 |
+| `akmcar` | true | 阿克曼底盘 (false=差速) |
+| `carto_slam` | false | 使用 Cartographer 替代 EKF |
+| `vlm_host` | http://192.168.10.189:8000 | VLM 推理服务地址 |
+| `use_sim_time` | false | 使用仿真时间 |
+
+---
+
+## 常用命令
+
+### 全部启动
+```bash
+ros2 launch my_robot_bringup master_launch.py
+```
+
+### 只调试底盘
+```bash
+ros2 launch my_robot_bringup master_launch.py use_lidar:=false use_slam:=false use_nav2:=false use_vlm:=false
+```
+
+### 只建图（底盘 + 雷达 + SLAM）
+```bash
+ros2 launch my_robot_bringup master_launch.py use_nav2:=false use_vlm:=false
+```
+
+### 只启动 VLM（调试图生文）
+```bash
+ros2 launch my_robot_bringup master_launch.py use_base:=false use_lidar:=false use_slam:=false use_nav2:=false
+```
+
+### 底盘 + 雷达 + SLAM + 导航（不加 VLM）
+```bash
+ros2 launch my_robot_bringup master_launch.py use_vlm:=false use_tts:=false
+```
+
+### 更换 VLM 服务地址
+```bash
+ros2 launch my_robot_bringup master_launch.py vlm_host:=http://192.168.10.200:8000
+```
+
+### 差分底盘模式
+```bash
+ros2 launch my_robot_bringup master_launch.py akmcar:=false
+```
+
+### 使用 Cartographer 替代 EKF
+```bash
+ros2 launch my_robot_bringup master_launch.py carto_slam:=true
+```
+
+### 使用自定义 SLAM 参数
+```bash
+ros2 launch my_robot_bringup master_launch.py slam_params_file:=/path/to/my_slam.yaml
+```
+
+### 使用自定义 Nav2 参数
+```bash
+ros2 launch my_robot_bringup master_launch.py nav2_params_file:=/path/to/my_nav2.yaml
+```
+
+---
+
+## 前置条件
+
+1. **VLM 服务** — WSL 中先启动 `python vlm_server.py`（如需使用 VLM）
+2. **Portproxy 转发** — Windows 上管理员 PowerShell 执行 `setup_vlm_forward.ps1`（仅需一次，IP 不变则不用重跑）
+3. **重刷环境** — 启动前清理残留进程：
+   ```bash
+   pkill -f ros2; pkill -f espeak; pkill -f ffplay
+   ```
+
+---
+
+## 网络架构（VLM 相关）
+
+```
+机器人 (192.168.10.x)
+    ↓ http://192.168.10.189:8000
+Windows 宿主机 (IP 需固定)
+    ↓ netsh portproxy → 127.0.0.1:8000
+WSL2 vlm_server.py
+```
+
+- Windows IP 不变 → 一切正常
+- Windows IP 变化 → 重跑 `setup_vlm_forward.ps1`，启动时传 `vlm_host:=http://新IP:8000`
+
+
+## 改动的地方
+  master_launch.py 变更内容
+
+  阶段: 1 (t=0s)
+  原来: 底盘 + EKF + TF + IMU
+  现在: 不变
+  ────────────────────────────────────────
+  阶段: 2 (t=2s)
+  原来: 激光雷达
+  现在: 不变
+  ────────────────────────────────────────
+  阶段: 3 (t=4s)
+  原来: —
+  现在: 障碍物检测
+    (obstacle_scanner.launch.py)
+  ────────────────────────────────────────
+  阶段: 4 (t=5s)
+  原来: SLAM Toolbox
+  现在: Map Server + AMCL 定位 (预建地图替代
+    SLAM)
+  ────────────────────────────────────────
+  阶段: 5 (t=7s)
+  原来: Nav2 导航栈
+  现在: Hybrid A* 路径规划 + Pure Pursuit
+    轨迹跟随
+  ────────────────────────────────────────
+  阶段: 6 (t=10s)
+
+● Bash(rm -f /c/Users/DHQ/master_launch_new.py)
+  ⎿  Done
+
+● 修改完成。以下是改动总结：
+
+  master_launch.py 变更内容
+
+  ┌───────────┬───────────────────────┬────────────────────────────────────────────┐
+  │   阶段    │         原来          │                    现在                    │
+  ├───────────┼───────────────────────┼────────────────────────────────────────────┤
+  │ 1 (t=0s)  │ 底盘 + EKF + TF + IMU │ 不变                                       │
+  ├───────────┼───────────────────────┼────────────────────────────────────────────┤
+  │ 2 (t=2s)  │ 激光雷达              │ 不变                                       │
+  ├───────────┼───────────────────────┼────────────────────────────────────────────┤
+  │ 3 (t=4s)  │ —                     │ 障碍物检测 (obstacle_scanner.launch.py)    │
+  ├───────────┼───────────────────────┼────────────────────────────────────────────┤
+  │ 4 (t=5s)  │ SLAM Toolbox          │ Map Server + AMCL 定位 (预建地图替代 SLAM) │
+  ├───────────┼───────────────────────┼────────────────────────────────────────────┤
+  │ 5 (t=7s)  │ Nav2 导航栈           │ Hybrid A* 路径规划 + Pure Pursuit 轨迹跟随 │
+  ├───────────┼───────────────────────┼────────────────────────────────────────────┤
+  │ 6 (t=10s) │ VLM + TTS             │ 不变                                       │
+  └───────────┴───────────────────────┴────────────────────────────────────────────┘
+
+  具体改动点
+
+  1. 移除：cyy_slamtoolbox、gc_navigation2_slamtoolbox、slam_toolbox 节点、nav2_bringup launch
+  2. 新增：obstacle_scanner 节点（发布 /obstacles）
+  3. 新增：map_server + amcl + lifecycle_manager 用于定位
+  4. 新增：grid_astar_theta_node（路径规划，发布 /plan）
+  5. 新增：topology_pure_pursuit_node.py（轨迹跟随，发布 /cmd_vel）
+  6. 话题对齐：planner 的 obstacles_topic 参数覆盖为 /obstacles（与 obstacle_scanner 发布的 topic 一致）
+  7. 原文件已备份为 master_launch.py.bak
+  
+  常用命令
+  source /opt/ros/humble/setup.bash
+  source ~/yiliao_ws/install/setup.bash
+  全部启动
+
+  ros2 launch my_robot_bringup master_launch.py
+
+  只调试底盘
+
+  ros2 launch my_robot_bringup master_launch.py use_lidar:=false use_obstacle:=false use_planner:=false use_vlm:=false
+
+  底盘 + 雷达 + 定位 + 规划跟踪（不开障碍物检测和 VLM）
+
+  ros2 launch my_robot_bringup master_launch.py use_obstacle:=false use_vlm:=false
+
+  只启动 VLM（调试图生文）
+
+  ros2 launch my_robot_bringup master_launch.py use_base:=false use_lidar:=false use_obstacle:=false use_planner:=false
+
+  底盘 + 雷达 + 障碍物检测 + 规划跟踪（不加 VLM）
+
+  ros2 launch my_robot_bringup master_launch.py use_vlm:=false use_tts:=false
+
+  只做障碍物检测（不开规划不开 VLM）
+
+  ros2 launch my_robot_bringup master_launch.py use_planner:=false use_vlm:=false
+
+  只做路径规划+跟踪（不开障碍物检测，靠预建地图）
+
+  ros2 launch my_robot_bringup master_launch.py use_obstacle:=false use_vlm:=false
+
+  更换 VLM 服务地址
+
+  ros2 launch my_robot_bringup master_launch.py vlm_host:=http://192.168.10.200:8000
+
+  差分底盘模式
+
+  ros2 launch my_robot_bringup master_launch.py akmcar:=false
+
+  使用 Cartographer 替代 EKF
+
+  ros2 launch my_robot_bringup master_launch.py carto_slam:=true
+
+  更换地图文件
+
+  ros2 launch my_robot_bringup master_launch.py map_file:=/path/to/my_map.yaml
+
+  更换规划器/跟踪器/AMCL 参数文件
+
+  ros2 launch my_robot_bringup master_launch.py planner_params_file:=/path/to/planner.yaml
+  ros2 launch my_robot_bringup master_launch.py tracker_params_file:=/path/to/tracker.yaml
+  ros2 launch my_robot_bringup master_launch.py amcl_params_file:=/path/to/amcl.yaml
+
+
--- a/must_know.md
+++ b/must_know.md
@@ -236,4 +236,7 @@ cd /home/sunrise/yiliao_ws && colcon build --packages-select gc_navigation2_slam

 # 查看完整提交历史
 cd /home/sunrise/yiliao_ws && git log --oneline 436e8a2..HEAD
+
+# 启动图生文
+
 ```
--- a/src/my_robot_bringup/launch/master_launch.py
+++ b/src/my_robot_bringup/launch/master_launch.py
@@ -130,21 +130,21 @@ def generate_launch_description():

    # ================================================================
    # 阶段 4: USB 摄像头 + 二维码识别  (t=4s)
-    # usb_cam: 驱动 USB 摄像头，发布 /image_raw
+    # car_usb_cam: 使用 hobot_usb_cam 驱动 USB 摄像头，发布 /image_raw
    # qr_detect: 订阅 /image_raw，识别二维码，发布 qr_results
    # ================================================================
-    usb_camera = Node(
-        package='usb_cam',
-        executable='usb_cam_node_exe',
-        name='usb_cam',
-        output='screen',
+    usb_camera = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            FindPackageShare('car_usb_cam'), '/launch', '/hobot_usb_cam.launch.py'
+        ]),
        condition=IfCondition(use_qr),
-        parameters=[{
-            'video_device': camera_device,
-            'image_size': [640, 480],
-            'pixel_format': 'YUYV',
-            'framerate': 30.0,
-        }],
+        launch_arguments={
+            'usb_video_device': camera_device,
+            'usb_image_width': '640',
+            'usb_image_height': '480',
+            'usb_framerate': '30',
+            'usb_pixel_format': 'yuyv2rgb',
+        }.items(),
    )

    qr_detect = Node(
--- a/src/origincar_description/CMakeLists.txt
+++ b/src/origincar_description/CMakeLists.txt
@@ -22,11 +22,7 @@ find_package(ament_cmake REQUIRED)
 # find_package(<dependency> REQUIRED)

 install(
-<<<<<<< HEAD
  DIRECTORY launch urdf rviz meshes world 
-=======
-  DIRECTORY launch urdf rviz meshes world
->>>>>>> mo_new
  DESTINATION share/${PROJECT_NAME}
 )

--- a/src/origincar_msg/CMakeLists.txt
+++ b/src/origincar_msg/CMakeLists.txt
@@ -25,6 +25,7 @@ find_package(std_msgs REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Data.msg"
  "msg/Sign.msg"
+  "srv/Speak.srv"
  DEPENDENCIES std_msgs
  ADD_LINTER_TESTS
 )
--- a/src/origincar_msg/srv/Speak.srv
+++ b/src/origincar_msg/srv/Speak.srv
@@ -0,0 +1,4 @@
+string text
+---
+bool success
+string message
--- a/src/planner/config/odom_hybrid_astar.yaml
+++ b/src/planner/config/odom_hybrid_astar.yaml
@@ -46,9 +46,10 @@ grid_astar_theta_planner:
    yaw_bins: 36
    primitive_length: 0.12
    collision_sample_step: 0.03
-    goal_xy_tolerance: 0.06
+    goal_xy_tolerance: 0.10
    goal_yaw_tolerance: 0.174533
    require_goal_yaw: false
+    snap_final_xy_to_goal: true
    use_grid_heuristic: true
    heuristic_weight: 1.35
    goal_yaw_heuristic_weight: 0.0
--- a/src/planner/config/odom_pure_pursuit.yaml
+++ b/src/planner/config/odom_pure_pursuit.yaml
@@ -14,18 +14,28 @@ topology_pure_pursuit:
    cancel_topic: /navigation_cancel

    control_rate: 20.0
-    lookahead_distance: 0.40
+    lookahead_distance: 0.28
+    min_lookahead_distance: 0.10
+    lookahead_curvature_gain: 0.80
+    lookahead_error_gain: 2.00
+    heading_correction_gain: 0.80
+    cross_track_correction_gain: 1.20
+    cross_track_speed_gain: 1.80
+    min_tracking_speed_ratio: 0.35
+    path_reacquire_distance: 0.45
    goal_tolerance: 0.06
    goal_yaw_tolerance: 0.174533
    align_goal_yaw: false
    final_alignment_offset: 0.25
    final_alignment_speed: 0.06
-    linear_speed: 0.60
-    min_linear_speed: 0.08
-    reverse_speed: 0.25
-    slowdown_distance: 0.35
-    curvature_slowdown_gain: 0.10
-    max_angular_speed: 2.80
+    linear_speed: 0.45
+    min_linear_speed: 0.06
+    reverse_speed: 0.18
+    slowdown_distance: 0.45
+    curvature_slowdown_gain: 0.06
+    min_curvature_speed_ratio: 0.70
+    min_goal_slowdown_ratio: 0.35
+    max_angular_speed: 3.20
    min_turning_radius: 0.25

    latency_compensation: true
--- a/src/planner/config/real_hybrid_astar.yaml
+++ b/src/planner/config/real_hybrid_astar.yaml
@@ -47,6 +47,7 @@ grid_astar_theta_planner:
    # Point navigation does not require the car to finish at a specific yaw.
    # Set true when final docking orientation is required.
    require_goal_yaw: false
+    snap_final_xy_to_goal: true
    use_grid_heuristic: true
    heuristic_weight: 1.25
    goal_yaw_heuristic_weight: 0.0
--- a/src/planner/config/real_pure_pursuit.yaml
+++ b/src/planner/config/real_pure_pursuit.yaml
@@ -14,16 +14,26 @@ topology_pure_pursuit:
    cancel_topic: /navigation_cancel

    control_rate: 20.0
-    lookahead_distance: 0.45
+    lookahead_distance: 0.30
+    min_lookahead_distance: 0.12
+    lookahead_curvature_gain: 0.70
+    lookahead_error_gain: 1.60
+    heading_correction_gain: 0.70
+    cross_track_correction_gain: 1.00
+    cross_track_speed_gain: 1.50
+    min_tracking_speed_ratio: 0.35
+    path_reacquire_distance: 0.45
    goal_tolerance: 0.15
    goal_yaw_tolerance: 0.174533
    align_goal_yaw: false
-    linear_speed: 0.20
+    linear_speed: 0.18
    min_linear_speed: 0.06
-    reverse_speed: 0.10
+    reverse_speed: 0.08
    slowdown_distance: 0.70
-    curvature_slowdown_gain: 0.25
-    max_angular_speed: 1.20
+    curvature_slowdown_gain: 0.12
+    min_curvature_speed_ratio: 0.65
+    min_goal_slowdown_ratio: 0.35
+    max_angular_speed: 1.50
    min_turning_radius: 0.40

    latency_compensation: true
--- a/src/planner/launch/odom_hybrid_astar.launch.py
+++ b/src/planner/launch/odom_hybrid_astar.launch.py
@@ -41,11 +41,21 @@ def generate_launch_description():
    planner_min_turning_radius = LaunchConfiguration("planner_min_turning_radius")
    tracker_min_turning_radius = LaunchConfiguration("tracker_min_turning_radius")
    lookahead_distance = LaunchConfiguration("lookahead_distance")
+    min_lookahead_distance = LaunchConfiguration("min_lookahead_distance")
+    lookahead_curvature_gain = LaunchConfiguration("lookahead_curvature_gain")
+    lookahead_error_gain = LaunchConfiguration("lookahead_error_gain")
+    heading_correction_gain = LaunchConfiguration("heading_correction_gain")
+    cross_track_correction_gain = LaunchConfiguration("cross_track_correction_gain")
+    cross_track_speed_gain = LaunchConfiguration("cross_track_speed_gain")
+    min_tracking_speed_ratio = LaunchConfiguration("min_tracking_speed_ratio")
+    path_reacquire_distance = LaunchConfiguration("path_reacquire_distance")
    linear_speed = LaunchConfiguration("linear_speed")
    min_linear_speed = LaunchConfiguration("min_linear_speed")
    reverse_speed = LaunchConfiguration("reverse_speed")
    slowdown_distance = LaunchConfiguration("slowdown_distance")
    curvature_slowdown_gain = LaunchConfiguration("curvature_slowdown_gain")
+    min_curvature_speed_ratio = LaunchConfiguration("min_curvature_speed_ratio")
+    min_goal_slowdown_ratio = LaunchConfiguration("min_goal_slowdown_ratio")
    max_angular_speed = LaunchConfiguration("max_angular_speed")
    goal_tolerance = LaunchConfiguration("goal_tolerance")
    goal_yaw_tolerance = LaunchConfiguration("goal_yaw_tolerance")
@@ -54,6 +64,7 @@ def generate_launch_description():
    final_alignment_speed = LaunchConfiguration("final_alignment_speed")
    require_goal_yaw = LaunchConfiguration("require_goal_yaw")
    planner_goal_xy_tolerance = LaunchConfiguration("planner_goal_xy_tolerance")
+    snap_final_xy_to_goal = LaunchConfiguration("snap_final_xy_to_goal")
    use_grid_heuristic = LaunchConfiguration("use_grid_heuristic")
    heuristic_weight = LaunchConfiguration("heuristic_weight")
    goal_yaw_heuristic_weight = LaunchConfiguration("goal_yaw_heuristic_weight")
@@ -83,6 +94,30 @@ def generate_launch_description():
        tracker_min_turning_radius, value_type=float
    )
    lookahead_distance_value = ParameterValue(lookahead_distance, value_type=float)
+    min_lookahead_distance_value = ParameterValue(
+        min_lookahead_distance, value_type=float
+    )
+    lookahead_curvature_gain_value = ParameterValue(
+        lookahead_curvature_gain, value_type=float
+    )
+    lookahead_error_gain_value = ParameterValue(
+        lookahead_error_gain, value_type=float
+    )
+    heading_correction_gain_value = ParameterValue(
+        heading_correction_gain, value_type=float
+    )
+    cross_track_correction_gain_value = ParameterValue(
+        cross_track_correction_gain, value_type=float
+    )
+    cross_track_speed_gain_value = ParameterValue(
+        cross_track_speed_gain, value_type=float
+    )
+    min_tracking_speed_ratio_value = ParameterValue(
+        min_tracking_speed_ratio, value_type=float
+    )
+    path_reacquire_distance_value = ParameterValue(
+        path_reacquire_distance, value_type=float
+    )
    linear_speed_value = ParameterValue(linear_speed, value_type=float)
    min_linear_speed_value = ParameterValue(min_linear_speed, value_type=float)
    reverse_speed_value = ParameterValue(reverse_speed, value_type=float)
@@ -90,6 +125,12 @@ def generate_launch_description():
    curvature_slowdown_gain_value = ParameterValue(
        curvature_slowdown_gain, value_type=float
    )
+    min_curvature_speed_ratio_value = ParameterValue(
+        min_curvature_speed_ratio, value_type=float
+    )
+    min_goal_slowdown_ratio_value = ParameterValue(
+        min_goal_slowdown_ratio, value_type=float
+    )
    max_angular_speed_value = ParameterValue(max_angular_speed, value_type=float)
    goal_tolerance_value = ParameterValue(goal_tolerance, value_type=float)
    goal_yaw_tolerance_value = ParameterValue(goal_yaw_tolerance, value_type=float)
@@ -104,6 +145,9 @@ def generate_launch_description():
    planner_goal_xy_tolerance_value = ParameterValue(
        planner_goal_xy_tolerance, value_type=float
    )
+    snap_final_xy_to_goal_value = ParameterValue(
+        snap_final_xy_to_goal, value_type=bool
+    )
    use_grid_heuristic_value = ParameterValue(use_grid_heuristic, value_type=bool)
    heuristic_weight_value = ParameterValue(heuristic_weight, value_type=float)
    goal_yaw_heuristic_weight_value = ParameterValue(
@@ -225,6 +269,7 @@ def generate_launch_description():
            "min_turning_radius": planner_min_turning_radius_value,
            "require_goal_yaw": require_goal_yaw_value,
            "goal_xy_tolerance": planner_goal_xy_tolerance_value,
+            "snap_final_xy_to_goal": snap_final_xy_to_goal_value,
            "use_grid_heuristic": use_grid_heuristic_value,
            "heuristic_weight": heuristic_weight_value,
            "goal_yaw_heuristic_weight": goal_yaw_heuristic_weight_value,
@@ -251,11 +296,21 @@ def generate_launch_description():
            "cmd_vel_topic": "/planner_cmd_vel",
            "min_turning_radius": tracker_min_turning_radius_value,
            "lookahead_distance": lookahead_distance_value,
+            "min_lookahead_distance": min_lookahead_distance_value,
+            "lookahead_curvature_gain": lookahead_curvature_gain_value,
+            "lookahead_error_gain": lookahead_error_gain_value,
+            "heading_correction_gain": heading_correction_gain_value,
+            "cross_track_correction_gain": cross_track_correction_gain_value,
+            "cross_track_speed_gain": cross_track_speed_gain_value,
+            "min_tracking_speed_ratio": min_tracking_speed_ratio_value,
+            "path_reacquire_distance": path_reacquire_distance_value,
            "linear_speed": linear_speed_value,
            "min_linear_speed": min_linear_speed_value,
            "reverse_speed": reverse_speed_value,
            "slowdown_distance": slowdown_distance_value,
            "curvature_slowdown_gain": curvature_slowdown_gain_value,
+            "min_curvature_speed_ratio": min_curvature_speed_ratio_value,
+            "min_goal_slowdown_ratio": min_goal_slowdown_ratio_value,
            "max_angular_speed": max_angular_speed_value,
            "goal_tolerance": goal_tolerance_value,
            "goal_yaw_tolerance": goal_yaw_tolerance_value,
@@ -308,15 +363,25 @@ def generate_launch_description():
        ),
        DeclareLaunchArgument(
            "lookahead_distance",
-            default_value="0.40",
+            default_value="0.28",
            description="Pure Pursuit lookahead in meters.",
        ),
-        DeclareLaunchArgument("linear_speed", default_value="0.60"),
-        DeclareLaunchArgument("min_linear_speed", default_value="0.08"),
-        DeclareLaunchArgument("reverse_speed", default_value="0.25"),
-        DeclareLaunchArgument("slowdown_distance", default_value="0.35"),
-        DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.10"),
-        DeclareLaunchArgument("max_angular_speed", default_value="2.80"),
+        DeclareLaunchArgument("min_lookahead_distance", default_value="0.10"),
+        DeclareLaunchArgument("lookahead_curvature_gain", default_value="0.80"),
+        DeclareLaunchArgument("lookahead_error_gain", default_value="2.00"),
+        DeclareLaunchArgument("heading_correction_gain", default_value="0.80"),
+        DeclareLaunchArgument("cross_track_correction_gain", default_value="1.20"),
+        DeclareLaunchArgument("cross_track_speed_gain", default_value="1.80"),
+        DeclareLaunchArgument("min_tracking_speed_ratio", default_value="0.35"),
+        DeclareLaunchArgument("path_reacquire_distance", default_value="0.45"),
+        DeclareLaunchArgument("linear_speed", default_value="0.45"),
+        DeclareLaunchArgument("min_linear_speed", default_value="0.06"),
+        DeclareLaunchArgument("reverse_speed", default_value="0.18"),
+        DeclareLaunchArgument("slowdown_distance", default_value="0.45"),
+        DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.06"),
+        DeclareLaunchArgument("min_curvature_speed_ratio", default_value="0.70"),
+        DeclareLaunchArgument("min_goal_slowdown_ratio", default_value="0.35"),
+        DeclareLaunchArgument("max_angular_speed", default_value="3.20"),
        DeclareLaunchArgument("goal_tolerance", default_value="0.06"),
        DeclareLaunchArgument("goal_yaw_tolerance", default_value="0.174533"),
        DeclareLaunchArgument("align_goal_yaw", default_value="false"),
@@ -329,9 +394,14 @@ def generate_launch_description():
        ),
        DeclareLaunchArgument(
            "planner_goal_xy_tolerance",
-            default_value="0.06",
+            default_value="0.10",
            description="Hybrid A* final XY tolerance in meters.",
        ),
+        DeclareLaunchArgument(
+            "snap_final_xy_to_goal",
+            default_value="true",
+            description="Publish the final path XY exactly at the requested goal XY without forcing yaw.",
+        ),
        DeclareLaunchArgument(
            "use_grid_heuristic",
            default_value="true",
--- a/src/planner/launch/real_hybrid_astar.launch.py
+++ b/src/planner/launch/real_hybrid_astar.launch.py
@@ -32,9 +32,27 @@ def generate_launch_description():
    enable_motion = LaunchConfiguration("enable_motion")
    wheelbase = LaunchConfiguration("wheelbase")
    max_steering_angle = LaunchConfiguration("max_steering_angle")
+    lookahead_distance = LaunchConfiguration("lookahead_distance")
+    min_lookahead_distance = LaunchConfiguration("min_lookahead_distance")
+    lookahead_curvature_gain = LaunchConfiguration("lookahead_curvature_gain")
+    lookahead_error_gain = LaunchConfiguration("lookahead_error_gain")
+    heading_correction_gain = LaunchConfiguration("heading_correction_gain")
+    cross_track_correction_gain = LaunchConfiguration("cross_track_correction_gain")
+    cross_track_speed_gain = LaunchConfiguration("cross_track_speed_gain")
+    min_tracking_speed_ratio = LaunchConfiguration("min_tracking_speed_ratio")
+    path_reacquire_distance = LaunchConfiguration("path_reacquire_distance")
+    linear_speed = LaunchConfiguration("linear_speed")
+    min_linear_speed = LaunchConfiguration("min_linear_speed")
+    reverse_speed = LaunchConfiguration("reverse_speed")
+    slowdown_distance = LaunchConfiguration("slowdown_distance")
+    curvature_slowdown_gain = LaunchConfiguration("curvature_slowdown_gain")
+    min_curvature_speed_ratio = LaunchConfiguration("min_curvature_speed_ratio")
+    min_goal_slowdown_ratio = LaunchConfiguration("min_goal_slowdown_ratio")
+    max_angular_speed = LaunchConfiguration("max_angular_speed")
    use_grid_heuristic = LaunchConfiguration("use_grid_heuristic")
    heuristic_weight = LaunchConfiguration("heuristic_weight")
    goal_yaw_heuristic_weight = LaunchConfiguration("goal_yaw_heuristic_weight")
+    snap_final_xy_to_goal = LaunchConfiguration("snap_final_xy_to_goal")
    state_log_period = LaunchConfiguration("state_log_period")
    search_progress_log_period_ms = LaunchConfiguration("search_progress_log_period_ms")
    use_sim_time = LaunchConfiguration("use_sim_time")
@@ -42,11 +60,53 @@ def generate_launch_description():
    enable_motion_value = ParameterValue(enable_motion, value_type=bool)
    wheelbase_value = ParameterValue(wheelbase, value_type=float)
    max_steering_angle_value = ParameterValue(max_steering_angle, value_type=float)
+    lookahead_distance_value = ParameterValue(lookahead_distance, value_type=float)
+    min_lookahead_distance_value = ParameterValue(
+        min_lookahead_distance, value_type=float
+    )
+    lookahead_curvature_gain_value = ParameterValue(
+        lookahead_curvature_gain, value_type=float
+    )
+    lookahead_error_gain_value = ParameterValue(
+        lookahead_error_gain, value_type=float
+    )
+    heading_correction_gain_value = ParameterValue(
+        heading_correction_gain, value_type=float
+    )
+    cross_track_correction_gain_value = ParameterValue(
+        cross_track_correction_gain, value_type=float
+    )
+    cross_track_speed_gain_value = ParameterValue(
+        cross_track_speed_gain, value_type=float
+    )
+    min_tracking_speed_ratio_value = ParameterValue(
+        min_tracking_speed_ratio, value_type=float
+    )
+    path_reacquire_distance_value = ParameterValue(
+        path_reacquire_distance, value_type=float
+    )
+    linear_speed_value = ParameterValue(linear_speed, value_type=float)
+    min_linear_speed_value = ParameterValue(min_linear_speed, value_type=float)
+    reverse_speed_value = ParameterValue(reverse_speed, value_type=float)
+    slowdown_distance_value = ParameterValue(slowdown_distance, value_type=float)
+    curvature_slowdown_gain_value = ParameterValue(
+        curvature_slowdown_gain, value_type=float
+    )
+    min_curvature_speed_ratio_value = ParameterValue(
+        min_curvature_speed_ratio, value_type=float
+    )
+    min_goal_slowdown_ratio_value = ParameterValue(
+        min_goal_slowdown_ratio, value_type=float
+    )
+    max_angular_speed_value = ParameterValue(max_angular_speed, value_type=float)
    use_grid_heuristic_value = ParameterValue(use_grid_heuristic, value_type=bool)
    heuristic_weight_value = ParameterValue(heuristic_weight, value_type=float)
    goal_yaw_heuristic_weight_value = ParameterValue(
        goal_yaw_heuristic_weight, value_type=float
    )
+    snap_final_xy_to_goal_value = ParameterValue(
+        snap_final_xy_to_goal, value_type=bool
+    )
    state_log_period_value = ParameterValue(state_log_period, value_type=float)
    search_progress_log_period_ms_value = ParameterValue(
        search_progress_log_period_ms, value_type=float
@@ -168,6 +228,7 @@ def generate_launch_description():
            "use_grid_heuristic": use_grid_heuristic_value,
            "heuristic_weight": heuristic_weight_value,
            "goal_yaw_heuristic_weight": goal_yaw_heuristic_weight_value,
+            "snap_final_xy_to_goal": snap_final_xy_to_goal_value,
            "state_log_period": state_log_period_value,
            "search_progress_log_period_ms": search_progress_log_period_ms_value,
        }],
@@ -182,6 +243,23 @@ def generate_launch_description():
            "use_sim_time": use_sim_time_value,
            "publish_cmd_vel": enable_motion_value,
            "cmd_vel_topic": "/planner_cmd_vel",
+            "lookahead_distance": lookahead_distance_value,
+            "min_lookahead_distance": min_lookahead_distance_value,
+            "lookahead_curvature_gain": lookahead_curvature_gain_value,
+            "lookahead_error_gain": lookahead_error_gain_value,
+            "heading_correction_gain": heading_correction_gain_value,
+            "cross_track_correction_gain": cross_track_correction_gain_value,
+            "cross_track_speed_gain": cross_track_speed_gain_value,
+            "min_tracking_speed_ratio": min_tracking_speed_ratio_value,
+            "path_reacquire_distance": path_reacquire_distance_value,
+            "linear_speed": linear_speed_value,
+            "min_linear_speed": min_linear_speed_value,
+            "reverse_speed": reverse_speed_value,
+            "slowdown_distance": slowdown_distance_value,
+            "curvature_slowdown_gain": curvature_slowdown_gain_value,
+            "min_curvature_speed_ratio": min_curvature_speed_ratio_value,
+            "min_goal_slowdown_ratio": min_goal_slowdown_ratio_value,
+            "max_angular_speed": max_angular_speed_value,
            "state_log_period": state_log_period_value,
        }],
    )
@@ -211,6 +289,23 @@ def generate_launch_description():
        ),
        DeclareLaunchArgument("wheelbase", default_value="0.143"),
        DeclareLaunchArgument("max_steering_angle", default_value="0.60"),
+        DeclareLaunchArgument("lookahead_distance", default_value="0.30"),
+        DeclareLaunchArgument("min_lookahead_distance", default_value="0.12"),
+        DeclareLaunchArgument("lookahead_curvature_gain", default_value="0.70"),
+        DeclareLaunchArgument("lookahead_error_gain", default_value="1.60"),
+        DeclareLaunchArgument("heading_correction_gain", default_value="0.70"),
+        DeclareLaunchArgument("cross_track_correction_gain", default_value="1.00"),
+        DeclareLaunchArgument("cross_track_speed_gain", default_value="1.50"),
+        DeclareLaunchArgument("min_tracking_speed_ratio", default_value="0.35"),
+        DeclareLaunchArgument("path_reacquire_distance", default_value="0.45"),
+        DeclareLaunchArgument("linear_speed", default_value="0.18"),
+        DeclareLaunchArgument("min_linear_speed", default_value="0.06"),
+        DeclareLaunchArgument("reverse_speed", default_value="0.08"),
+        DeclareLaunchArgument("slowdown_distance", default_value="0.70"),
+        DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.12"),
+        DeclareLaunchArgument("min_curvature_speed_ratio", default_value="0.65"),
+        DeclareLaunchArgument("min_goal_slowdown_ratio", default_value="0.35"),
+        DeclareLaunchArgument("max_angular_speed", default_value="1.50"),
        DeclareLaunchArgument(
            "use_grid_heuristic",
            default_value="true",
@@ -226,6 +321,11 @@ def generate_launch_description():
            default_value="0.0",
            description="Weight for final yaw guidance in the Hybrid A* heuristic.",
        ),
+        DeclareLaunchArgument(
+            "snap_final_xy_to_goal",
+            default_value="true",
+            description="Publish the final path XY exactly at the requested goal XY without forcing yaw.",
+        ),
        DeclareLaunchArgument(
            "state_log_period",
            default_value="1.0",
--- a/src/planner/scripts/pycache/ackermann_cmd_bridge.cpython-313.pyc
+++ b/src/planner/scripts/pycache/ackermann_cmd_bridge.cpython-313.pyc
--- a/src/planner/scripts/pycache/evaluate_navigation_test.cpython-313.pyc
+++ b/src/planner/scripts/pycache/evaluate_navigation_test.cpython-313.pyc
--- a/src/planner/scripts/pycache/navigation_test_recorder.cpython-313.pyc
+++ b/src/planner/scripts/pycache/navigation_test_recorder.cpython-313.pyc
--- a/src/planner/scripts/pycache/odom_map_tf.cpython-313.pyc
+++ b/src/planner/scripts/pycache/odom_map_tf.cpython-313.pyc
--- a/src/planner/scripts/pycache/topology_pure_pursuit_node.cpython-313.pyc
+++ b/src/planner/scripts/pycache/topology_pure_pursuit_node.cpython-313.pyc
--- a/src/planner/scripts/goal_reach_debug_collector.py
+++ b/src/planner/scripts/goal_reach_debug_collector.py
@@ -142,6 +142,7 @@ class GoalReachDebugCollector(Node):
        self.finished = False

        self.latest_odom = None
+        self.latest_odom_received_ns = 0
        self.latest_goal = None
        self.latest_plan_points = []
        self.latest_cmd = None
@@ -208,6 +209,7 @@ class GoalReachDebugCollector(Node):

    def _odom_cb(self, msg):
        self.latest_odom = msg
+        self.latest_odom_received_ns = self.get_clock().now().nanoseconds

    def _goal_cb(self, msg):
        goal = {
@@ -348,18 +350,21 @@ class GoalReachDebugCollector(Node):
            return None

        msg = self.latest_odom
-        stamp = Time.from_msg(msg.header.stamp)
+        now_ns = self.get_clock().now().nanoseconds
+        receive_age = None
+        if self.latest_odom_received_ns > 0:
+            receive_age = max(0.0, (now_ns - self.latest_odom_received_ns) * 1e-9)
        if msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0:
-            age = 0.0
+            stamp_age = None
        else:
-            age = (self.get_clock().now() - stamp).nanoseconds * 1e-9
-        if age < 0.0:
-            age = 0.0
-        if age > self.max_odom_age:
+            stamp = Time.from_msg(msg.header.stamp)
+            stamp_age = max(0.0, (self.get_clock().now() - stamp).nanoseconds * 1e-9)
+        if receive_age is not None and receive_age > self.max_odom_age:
            return {
                "available": False,
                "reason": "odom_too_old",
-                "age_sec": age,
+                "odom_receive_age_sec": receive_age,
+                "odom_stamp_age_sec": stamp_age,
            }

        source_frame = msg.header.frame_id or self.map_frame
@@ -381,7 +386,8 @@ class GoalReachDebugCollector(Node):
            "x": x,
            "y": y,
            "yaw": yaw,
-            "odom_age_sec": age,
+            "odom_receive_age_sec": receive_age,
+            "odom_stamp_age_sec": stamp_age,
            "odom_linear_x": msg.twist.twist.linear.x,
            "odom_angular_z": msg.twist.twist.angular.z,
        }
@@ -448,6 +454,7 @@ class GoalReachDebugCollector(Node):
                    "goal_xy_tolerance",
                    "goal_yaw_tolerance",
                    "require_goal_yaw",
+                    "snap_final_xy_to_goal",
                    "resolution",
                    "yaw_bins",
                    "primitive_length",
@@ -467,11 +474,21 @@ class GoalReachDebugCollector(Node):
                    "publish_cmd_vel",
                    "cmd_vel_topic",
                    "lookahead_distance",
+                    "min_lookahead_distance",
+                    "lookahead_curvature_gain",
+                    "lookahead_error_gain",
+                    "heading_correction_gain",
+                    "cross_track_correction_gain",
+                    "cross_track_speed_gain",
+                    "min_tracking_speed_ratio",
+                    "path_reacquire_distance",
                    "linear_speed",
                    "min_linear_speed",
                    "reverse_speed",
                    "slowdown_distance",
                    "curvature_slowdown_gain",
+                    "min_curvature_speed_ratio",
+                    "min_goal_slowdown_ratio",
                    "min_turning_radius",
                    "max_angular_speed",
                ],
@@ -628,8 +645,9 @@ class GoalReachDebugCollector(Node):

        planner_xy = planner.get("goal_xy_tolerance")
        tracker_xy = tracker.get("goal_tolerance")
+        snap_xy = bool(planner.get("snap_final_xy_to_goal"))
        if isinstance(planner_xy, (int, float)) and isinstance(tracker_xy, (int, float)):
-            if planner_xy > tracker_xy:
+            if planner_xy > tracker_xy and not snap_xy:
                hints.append(
                    "planner 的 goal_xy_tolerance 大于 tracker 的 goal_tolerance；"
                    "planner 可能发布一个离原始目标较远的路径终点。"
--- a/src/planner/scripts/preflight_check.py
+++ b/src/planner/scripts/preflight_check.py
@@ -32,7 +32,7 @@ class PreflightCheck(Node):
        self.declare_parameter("map_topic", "/map")
        self.declare_parameter("odom_topic", "/odom_combined")
        self.declare_parameter("scan_topic", "/scan")
-        self.declare_parameter("cmd_vel_topic", "/cmd_vel")
+        self.declare_parameter("cmd_vel_topic", "/planner_cmd_vel")
        self.declare_parameter("expected_scan_frame", "laser_link")
        self.declare_parameter("tf_wait_sec", 3.0)
        self.declare_parameter("drive_command_topic", "/ackermann_cmd")
--- a/src/planner/scripts/send_goal.py
+++ b/src/planner/scripts/send_goal.py
@@ -39,8 +39,8 @@ def main():
    parser.add_argument("yaw", type=float, nargs="?", default=0.0)
    parser.add_argument("--topic", default="/goal_pose")
    parser.add_argument("--frame", default="map")
-    parser.add_argument("--timeout", type=float, default=5.0)
-    parser.add_argument("--repeat", type=int, default=1)
+    parser.add_argument("--timeout", type=float, default=8.0)
+    parser.add_argument("--repeat", type=int, default=3)
    args = parser.parse_args()

    rclpy.init()
--- a/src/planner/scripts/topology_pure_pursuit_node.py
+++ b/src/planner/scripts/topology_pure_pursuit_node.py
@@ -62,12 +62,22 @@ class TopologyPurePursuit(Node):
        self.declare_parameter("path_resolution", 0.10)
        self.declare_parameter("nearest_node_max_distance", 2.0)
        self.declare_parameter("lookahead_distance", 0.55)
+        self.declare_parameter("min_lookahead_distance", 0.16)
+        self.declare_parameter("lookahead_curvature_gain", 0.50)
+        self.declare_parameter("lookahead_error_gain", 1.20)
+        self.declare_parameter("heading_correction_gain", 0.55)
+        self.declare_parameter("cross_track_correction_gain", 0.80)
+        self.declare_parameter("cross_track_speed_gain", 1.20)
+        self.declare_parameter("min_tracking_speed_ratio", 0.40)
+        self.declare_parameter("path_reacquire_distance", 0.50)
        self.declare_parameter("goal_tolerance", 0.25)
        self.declare_parameter("linear_speed", 0.30)
        self.declare_parameter("min_linear_speed", 0.08)
        self.declare_parameter("reverse_speed", 0.12)
        self.declare_parameter("slowdown_distance", 0.80)
        self.declare_parameter("curvature_slowdown_gain", 0.18)
+        self.declare_parameter("min_curvature_speed_ratio", 0.35)
+        self.declare_parameter("min_goal_slowdown_ratio", 0.25)
        self.declare_parameter("max_angular_speed", 1.5)
        self.declare_parameter("min_turning_radius", 0.35)
        self.declare_parameter("goal_yaw_tolerance", 0.10)
@@ -99,6 +109,35 @@ class TopologyPurePursuit(Node):
        self.lookahead_distance = float(
            self.get_parameter("lookahead_distance").value
        )
+        self.min_lookahead_distance = max(
+            0.05,
+            min(
+                self.lookahead_distance,
+                float(self.get_parameter("min_lookahead_distance").value),
+            ),
+        )
+        self.lookahead_curvature_gain = max(
+            0.0, float(self.get_parameter("lookahead_curvature_gain").value)
+        )
+        self.lookahead_error_gain = max(
+            0.0, float(self.get_parameter("lookahead_error_gain").value)
+        )
+        self.heading_correction_gain = max(
+            0.0, float(self.get_parameter("heading_correction_gain").value)
+        )
+        self.cross_track_correction_gain = max(
+            0.0, float(self.get_parameter("cross_track_correction_gain").value)
+        )
+        self.cross_track_speed_gain = max(
+            0.0, float(self.get_parameter("cross_track_speed_gain").value)
+        )
+        self.min_tracking_speed_ratio = max(
+            0.0,
+            min(1.0, float(self.get_parameter("min_tracking_speed_ratio").value)),
+        )
+        self.path_reacquire_distance = max(
+            0.0, float(self.get_parameter("path_reacquire_distance").value)
+        )
        self.goal_tolerance = float(self.get_parameter("goal_tolerance").value)
        self.linear_speed = float(self.get_parameter("linear_speed").value)
        self.min_linear_speed = float(self.get_parameter("min_linear_speed").value)
@@ -107,6 +146,14 @@ class TopologyPurePursuit(Node):
        self.curvature_slowdown_gain = float(
            self.get_parameter("curvature_slowdown_gain").value
        )
+        self.min_curvature_speed_ratio = max(
+            0.0,
+            min(1.0, float(self.get_parameter("min_curvature_speed_ratio").value)),
+        )
+        self.min_goal_slowdown_ratio = max(
+            0.0,
+            min(1.0, float(self.get_parameter("min_goal_slowdown_ratio").value)),
+        )
        self.max_angular_speed = float(
            self.get_parameter("max_angular_speed").value
        )
@@ -151,6 +198,7 @@ class TopologyPurePursuit(Node):
        self.tf_listener = TransformListener(self.tf_buffer, self)

        self.latest_odom = None
+        self.latest_odom_received_ns = 0
        self.active_path = []
        self.active_node_path = []
        self.active_goal_node = ""
@@ -162,6 +210,8 @@ class TopologyPurePursuit(Node):
        self.last_cmd_linear = 0.0
        self.last_cmd_angular = 0.0

+        self.plan_pub = None
+        if self.enable_topology_planning:
            self.plan_pub = self.create_publisher(
                NavPath, self.get_parameter("plan_topic").value, 10
            )
@@ -272,6 +322,7 @@ class TopologyPurePursuit(Node):

    def _odom_callback(self, msg):
        self.latest_odom = msg
+        self.latest_odom_received_ns = self.get_clock().now().nanoseconds
        if (
            self.enable_topology_planning
            and self.autoplan_default_goal
@@ -462,6 +513,8 @@ class TopologyPurePursuit(Node):
        return updated

    def _publish_path(self):
+        if self.plan_pub is None:
+            return
        path_msg = NavPath()
        path_msg.header.frame_id = self.map_frame
        path_msg.header.stamp = self.get_clock().now().to_msg()
@@ -474,6 +527,12 @@ class TopologyPurePursuit(Node):
    def _current_pose(self, compensate=True):
        if self.latest_odom is None:
            return None
+        now_ns = self.get_clock().now().nanoseconds
+        if self.latest_odom_received_ns <= 0:
+            return None
+        age = max(0.0, (now_ns - self.latest_odom_received_ns) * 1e-9)
+        if age > self.max_odom_age:
+            return None

        msg = self.latest_odom
        source_frame = msg.header.frame_id or self.map_frame
@@ -483,16 +542,6 @@ class TopologyPurePursuit(Node):
        vx = msg.twist.twist.linear.x
        wz = msg.twist.twist.angular.z

-        stamp = Time.from_msg(msg.header.stamp)
-        if msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0:
-            age = 0.0
-        else:
-            age = (self.get_clock().now() - stamp).nanoseconds * 1e-9
-        if age < 0.0:
-            age = 0.0
-        if age > self.max_odom_age:
-            return None
-
        if compensate and self.latency_compensation:
            horizon = min(age, self.max_latency_compensation)
            if abs(wz) < 1e-4:
@@ -587,28 +636,53 @@ class TopologyPurePursuit(Node):
            )
            return

-        nearest_index = self._nearest_path_index(x, y)
+        projection = self._nearest_path_projection(x, y)
+        nearest_index = projection["index"]
        self.current_path_index = nearest_index
-        target_index = self._lookahead_index(x, y, nearest_index)
-        target_x, target_y, target_path_yaw = self.active_path[target_index]
-        nearest_x, nearest_y, _ = self.active_path[nearest_index]
-        cross_track_error = math.hypot(nearest_x - x, nearest_y - y)
+        cross_track_signed = projection["signed_error"]
+        cross_track_error = abs(cross_track_signed)
+        path_curvature = self._path_curvature(nearest_index)
+        lookahead_threshold = self._adaptive_lookahead(
+            path_curvature, cross_track_error, goal_distance
+        )
+        target = self._path_target_from_projection(projection, lookahead_threshold)
+        target_index = target["index"]
+        target_x = target["x"]
+        target_y = target["y"]
+        target_path_yaw = target["yaw"]

        dx = target_x - x
        dy = target_y - y
-        path_heading = self._segment_heading(target_index, x, y)
+        path_heading = target["path_heading"]
        reverse = math.cos(normalize_angle(target_path_yaw - path_heading)) < 0.0
        control_yaw = normalize_angle(yaw + math.pi) if reverse else yaw
+        # Track the XY curve.  The path pose yaw is still used to detect reverse
+        # segments, but heading correction should follow the segment tangent so
+        # a noisy or intentionally ignored goal yaw does not pull the vehicle
+        # away from the planned line.
+        heading_error = normalize_angle(path_heading - control_yaw)
        local_x = math.cos(control_yaw) * dx + math.sin(control_yaw) * dy
        local_y = -math.sin(control_yaw) * dx + math.cos(control_yaw) * dy
        lookahead = max(0.05, math.hypot(local_x, local_y))

        curvature = 2.0 * local_y / (lookahead * lookahead)
+        curvature += (
+            self.heading_correction_gain
+            * math.sin(heading_error)
+            / max(self.min_lookahead_distance, lookahead)
+        )
+        curvature += (
+            -self.cross_track_correction_gain
+            * cross_track_signed
+            / max(self.min_lookahead_distance * self.min_lookahead_distance, lookahead * lookahead)
+        )
        if self.min_turning_radius > 1e-3:
            max_curvature = 1.0 / self.min_turning_radius
            curvature = max(-max_curvature, min(max_curvature, curvature))

-        speed = self._target_speed(abs(curvature), goal_distance, reverse)
+        speed = self._target_speed(
+            abs(curvature), goal_distance, reverse, cross_track_error
+        )
        angular = speed * curvature
        angular = max(-self.max_angular_speed, min(self.max_angular_speed, angular))
        self._publish_cmd(speed, angular)
@@ -620,9 +694,13 @@ class TopologyPurePursuit(Node):
            nearest_index=nearest_index,
            target_index=target_index,
            cross_track_error=cross_track_error,
+            cross_track_signed=cross_track_signed,
            reverse=reverse,
            curvature=curvature,
            lookahead=lookahead,
+            lookahead_threshold=lookahead_threshold,
+            path_curvature=path_curvature,
+            heading_error=heading_error,
        )

    def _align_final_yaw(self, x, y, yaw, goal_x, goal_y, goal_yaw, goal_yaw_error):
@@ -670,15 +748,177 @@ class TopologyPurePursuit(Node):
                best_dist = dist
        return best_index

-    def _lookahead_index(self, x, y, start_index):
+    def _nearest_path_projection(self, x, y):
+        if len(self.active_path) < 2:
+            px, py, pyaw = self.active_path[0]
+            return {
+                "index": 0,
+                "t": 0.0,
+                "x": px,
+                "y": py,
+                "yaw": pyaw,
+                "path_heading": pyaw,
+                "signed_error": 0.0,
+                "distance": math.hypot(x - px, y - py),
+            }
+
+        if self.path_reacquire_distance > 0.0:
+            nearest_vertex = self._nearest_path_index(x, y)
+            vx, vy, _ = self.active_path[nearest_vertex]
+            vertex_error = math.hypot(vx - x, vy - y)
+            if vertex_error > self.path_reacquire_distance:
+                start = 0
+            else:
+                start = max(0, self.current_path_index - 8)
+        else:
+            start = max(0, self.current_path_index - 8)
+
+        best = None
+        best_dist_sq = float("inf")
+        for index in range(start, len(self.active_path) - 1):
+            x0, y0, yaw0 = self.active_path[index]
+            x1, y1, yaw1 = self.active_path[index + 1]
+            dx = x1 - x0
+            dy = y1 - y0
+            seg_len_sq = dx * dx + dy * dy
+            if seg_len_sq <= 1e-10:
+                continue
+            t = ((x - x0) * dx + (y - y0) * dy) / seg_len_sq
+            t = max(0.0, min(1.0, t))
+            px = x0 + t * dx
+            py = y0 + t * dy
+            err_x = x - px
+            err_y = y - py
+            dist_sq = err_x * err_x + err_y * err_y
+            if dist_sq >= best_dist_sq:
+                continue
+            seg_len = math.sqrt(seg_len_sq)
+            ux = dx / seg_len
+            uy = dy / seg_len
+            signed_error = ux * err_y - uy * err_x
+            heading = math.atan2(dy, dx)
+            yaw = yaw0 if t < 0.5 else yaw1
+            best = {
+                "index": index,
+                "t": t,
+                "x": px,
+                "y": py,
+                "yaw": yaw,
+                "path_heading": heading,
+                "signed_error": signed_error,
+                "distance": math.sqrt(dist_sq),
+            }
+            best_dist_sq = dist_sq
+
+        if best is not None:
+            return best
+
+        index = min(max(0, self.current_path_index), len(self.active_path) - 1)
+        px, py, pyaw = self.active_path[index]
+        return {
+            "index": index,
+            "t": 0.0,
+            "x": px,
+            "y": py,
+            "yaw": pyaw,
+            "path_heading": pyaw,
+            "signed_error": 0.0,
+            "distance": math.hypot(x - px, y - py),
+        }
+
+    def _path_target_from_projection(self, projection, lookahead_distance):
+        remaining = max(0.05, float(lookahead_distance))
+        index = min(projection["index"], len(self.active_path) - 2)
+        t = max(0.0, min(1.0, projection.get("t", 0.0)))
+
+        while index < len(self.active_path) - 1:
+            x0, y0, yaw0 = self.active_path[index]
+            x1, y1, yaw1 = self.active_path[index + 1]
+            dx = x1 - x0
+            dy = y1 - y0
+            seg_len = math.hypot(dx, dy)
+            if seg_len <= 1e-9:
+                index += 1
+                t = 0.0
+                continue
+            available = (1.0 - t) * seg_len
+            if remaining <= available:
+                ratio = t + remaining / seg_len
+                x = x0 + ratio * dx
+                y = y0 + ratio * dy
+                heading = math.atan2(dy, dx)
+                yaw = yaw0 if ratio < 0.5 else yaw1
+                return {
+                    "index": index + 1,
+                    "x": x,
+                    "y": y,
+                    "yaw": yaw,
+                    "path_heading": heading,
+                }
+            remaining -= available
+            index += 1
+            t = 0.0
+
+        x, y, yaw = self.active_path[-1]
+        heading = self._segment_heading(len(self.active_path) - 1)
+        return {
+            "index": len(self.active_path) - 1,
+            "x": x,
+            "y": y,
+            "yaw": yaw,
+            "path_heading": heading,
+        }
+
+    def _lookahead_index(self, x, y, start_index, lookahead_distance=None):
+        threshold = (
+            self.lookahead_distance
+            if lookahead_distance is None
+            else max(0.05, float(lookahead_distance))
+        )
        target_index = len(self.active_path) - 1
        for index in range(start_index, len(self.active_path)):
            px, py, _ = self.active_path[index]
-            if math.hypot(px - x, py - y) >= self.lookahead_distance:
+            if math.hypot(px - x, py - y) >= threshold:
                target_index = index
                break
        return target_index

+    def _adaptive_lookahead(self, path_curvature, cross_track_error, goal_distance):
+        scale = (
+            1.0
+            + self.lookahead_curvature_gain * abs(path_curvature)
+            + self.lookahead_error_gain * max(0.0, cross_track_error)
+        )
+        lookahead = self.lookahead_distance / max(1.0, scale)
+        if goal_distance < self.lookahead_distance:
+            lookahead = min(lookahead, max(self.min_lookahead_distance, goal_distance))
+        return max(self.min_lookahead_distance, min(self.lookahead_distance, lookahead))
+
+    def _path_curvature(self, index):
+        if len(self.active_path) < 3:
+            return 0.0
+        i0 = max(0, index - 1)
+        i1 = max(0, min(index, len(self.active_path) - 1))
+        i2 = min(len(self.active_path) - 1, index + 1)
+        if i0 == i1:
+            i2 = min(len(self.active_path) - 1, i1 + 2)
+        if i1 == i2:
+            i0 = max(0, i1 - 2)
+        if i0 == i1 or i1 == i2:
+            return 0.0
+
+        x0, y0, _ = self.active_path[i0]
+        x1, y1, _ = self.active_path[i1]
+        x2, y2, _ = self.active_path[i2]
+        a = math.hypot(x1 - x0, y1 - y0)
+        b = math.hypot(x2 - x1, y2 - y1)
+        c = math.hypot(x2 - x0, y2 - y0)
+        denominator = a * b * c
+        if denominator <= 1e-6:
+            return 0.0
+        cross = (x1 - x0) * (y2 - y0) - (y1 - y0) * (x2 - x0)
+        return 2.0 * cross / denominator
+
    def _segment_heading(self, index, current_x=None, current_y=None):
        if len(self.active_path) < 2:
            return 0.0
@@ -716,9 +956,13 @@ class TopologyPurePursuit(Node):
        nearest_index=None,
        target_index=None,
        cross_track_error=None,
+        cross_track_signed=None,
        reverse=False,
        curvature=None,
        lookahead=None,
+        lookahead_threshold=None,
+        path_curvature=None,
+        heading_error=None,
    ):
        if self.state_log_period <= 0.0:
            return
@@ -731,10 +975,34 @@ class TopologyPurePursuit(Node):
        parts = [f"tracker_state mode={mode}"]
        if pose is None:
            parts.append("pose=unavailable")
+            if self.latest_odom is None:
+                parts.append("odom=none")
+            elif self.latest_odom_received_ns > 0:
+                receive_age = max(
+                    0.0,
+                    (now_ns - self.latest_odom_received_ns) * 1e-9,
+                )
+                parts.append(f"odom_receive_age={receive_age:.3f}")
        else:
            x, y, yaw, vx, wz = pose
            parts.append(f"pose=({x:.3f},{y:.3f},{yaw:.3f})")
            parts.append(f"odom_twist=({vx:.3f},{wz:.3f})")
+            if self.latest_odom_received_ns > 0:
+                receive_age = max(
+                    0.0,
+                    (now_ns - self.latest_odom_received_ns) * 1e-9,
+                )
+                parts.append(f"odom_receive_age={receive_age:.3f}")
+        if self.latest_odom is not None:
+            stamp = Time.from_msg(self.latest_odom.header.stamp)
+            if (
+                self.latest_odom.header.stamp.sec != 0
+                or self.latest_odom.header.stamp.nanosec != 0
+            ):
+                stamp_age = max(
+                    0.0, (self.get_clock().now() - stamp).nanoseconds * 1e-9
+                )
+                parts.append(f"odom_stamp_age={stamp_age:.3f}")

        parts.append(f"path_points={len(self.active_path)}")
        parts.append(f"completed={self.completed}")
@@ -748,21 +1016,41 @@ class TopologyPurePursuit(Node):
            parts.append(f"target_index={target_index}")
        if cross_track_error is not None:
            parts.append(f"cross_track={cross_track_error:.3f}")
+        if cross_track_signed is not None:
+            parts.append(f"cross_track_signed={cross_track_signed:.3f}")
        if curvature is not None:
            parts.append(f"curvature={curvature:.3f}")
        if lookahead is not None:
            parts.append(f"lookahead={lookahead:.3f}")
+        if lookahead_threshold is not None:
+            parts.append(f"lookahead_threshold={lookahead_threshold:.3f}")
+        if path_curvature is not None:
+            parts.append(f"path_curvature={path_curvature:.3f}")
+        if heading_error is not None:
+            parts.append(f"heading_error={heading_error:.3f}")
        parts.append(f"reverse={reverse}")
        parts.append(f"cmd=({self.last_cmd_linear:.3f},{self.last_cmd_angular:.3f})")
        self.get_logger().info(" ".join(parts))

-    def _target_speed(self, abs_curvature, goal_distance, reverse):
+    def _target_speed(self, abs_curvature, goal_distance, reverse, cross_track_error=0.0):
        base_speed = self.reverse_speed if reverse else self.linear_speed
        speed = base_speed
-        speed *= max(0.35, 1.0 - self.curvature_slowdown_gain * abs_curvature)
+        speed *= max(
+            self.min_curvature_speed_ratio,
+            1.0 - self.curvature_slowdown_gain * abs_curvature,
+        )
        if goal_distance < self.slowdown_distance:
-            ratio = max(0.25, goal_distance / max(0.05, self.slowdown_distance))
+            ratio = max(
+                self.min_goal_slowdown_ratio,
+                goal_distance / max(0.05, self.slowdown_distance),
+            )
            speed *= ratio
+        if cross_track_error > 0.0 and self.cross_track_speed_gain > 0.0:
+            tracking_ratio = max(
+                self.min_tracking_speed_ratio,
+                1.0 - self.cross_track_speed_gain * cross_track_error,
+            )
+            speed *= tracking_ratio
        speed = max(self.min_linear_speed, min(base_speed, speed))
        return -speed if reverse else speed

--- a/src/vlm_detect/config/vlm_detect.yaml
+++ b/src/vlm_detect/config/vlm_detect.yaml
@@ -1,36 +1,18 @@
 # 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
@@ -1,13 +1,15 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
-vlm_detect 联合启动文件
-同时启动 vlm_node (图生文) 和 tts_node (语音播报)
+vlm_detect 启动文件
+同时启动 vlm_node (图生文) + tts_server (语音播报服务) + qr_tts_bridge (二维码播报)

 用法:
-  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
+  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           # 关闭语音播报
+  ros2 launch vlm_detect vlm_detect.launch.py use_qr_tts:=false        # 关闭二维码播报桥接
+  ros2 launch vlm_detect vlm_detect.launch.py use_vlm:=false           # 只启动语音服务
 """

 import os
@@ -23,7 +25,9 @@ from launch_ros.actions import Node
 def generate_launch_description():

    # ==================== Launch 参数 ====================
+    use_vlm = LaunchConfiguration('use_vlm')
    use_tts = LaunchConfiguration('use_tts')
+    use_qr_tts = LaunchConfiguration('use_qr_tts')

    config_file = LaunchConfiguration('config_file')

@@ -37,15 +41,22 @@ def generate_launch_description():
    prompt_text = LaunchConfiguration('prompt_text')
    max_tokens = LaunchConfiguration('max_tokens')

-    # tts_node 可覆盖参数
+    # tts_server 可覆盖参数
    audio_sink = LaunchConfiguration('audio_sink')
-    tts_voice = LaunchConfiguration('tts_voice')
    tts_speed = LaunchConfiguration('tts_speed')

-    # ==================== 声明参数 ====================
+    # ==================== 参数声明 ====================
+    declare_use_vlm = DeclareLaunchArgument(
+        'use_vlm', default_value='true',
+        description='启动 VLM 图生文节点')
+
    declare_use_tts = DeclareLaunchArgument(
        'use_tts', default_value='true',
-        description='是否同时启动 TTS 语音播报节点')
+        description='启动 TTS 语音播报服务')
+
+    declare_use_qr_tts = DeclareLaunchArgument(
+        'use_qr_tts', default_value='true',
+        description='启动二维码 → TTS 桥接节点')

    declare_config_file = DeclareLaunchArgument(
        'config_file',
@@ -57,47 +68,46 @@ def generate_launch_description():
    # vlm_node 参数
    declare_vlm_host = DeclareLaunchArgument(
        'vlm_host', default_value='http://192.168.10.189:8000',
-        description='VLM 推理服务地址')
+        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='订阅的压缩图像话题')
+        description='输入的压缩图像话题')
    declare_trigger_topic = DeclareLaunchArgument(
        'trigger_topic', default_value='/sign4return',
-        description='订阅的触发信号话题')
+        description='输入的触发信号话题')
    declare_trigger_sign = DeclareLaunchArgument(
        'trigger_sign', default_value='9',
        description='触发信号值 (Int32)')
    declare_result_topic = DeclareLaunchArgument(
        'result_topic', default_value='/vlm_result',
-        description='发布 VLM 结果的话题')
+        description='输出 VLM 结果的话题')
    declare_prompt_text = DeclareLaunchArgument(
-        'prompt_text', default_value='描述图片中有一个病人的特征，字数控制在20字以内。',
+        'prompt_text', default_value='请描述这张图片的内容，用一句简短的话概括，不超过20个字。',
        description='发送给 VLM 的提示词')
    declare_max_tokens = DeclareLaunchArgument(
        'max_tokens', default_value='100',
-        description='最大输出 token 数')
+        description='最大生成 token 数')

-    # tts_node 参数
+    # tts_server 参数
    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)')
+        description='语速倍率 (0.5~2.0)')

    # ==================== 节点 ====================
+    # VLM 图生文节点 (内部自带 TTS 服务客户端)
    vlm_node = Node(
        package='vlm_detect',
        executable='vlm_node',
        name='vlm_detect',
        output='screen',
+        condition=IfCondition(use_vlm),
        parameters=[config_file,
            {
                'vlm_host': vlm_host,
@@ -111,26 +121,35 @@ def generate_launch_description():
            }],
    )

-    tts_node = Node(
+    # TTS 语音播报服务端
+    tts_server = Node(
        package='vlm_detect',
-        executable='tts_node',
-        name='tts_node',
+        executable='tts_server',
+        name='tts_server',
        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,
            }],
    )

-    # ==================== 启动描述 ====================
+    # 二维码 → TTS 桥接 (订阅 qr_results，调用 /tts/speak)
+    qr_tts_bridge = Node(
+        package='vlm_detect',
+        executable='qr_tts_bridge',
+        name='qr_tts_bridge',
+        output='screen',
+        condition=IfCondition(use_qr_tts),
+    )
+
+    # ==================== 组装 ====================
    return LaunchDescription([
        # 参数声明
+        declare_use_vlm,
        declare_use_tts,
+        declare_use_qr_tts,
        declare_config_file,
        declare_vlm_host,
        declare_vlm_model,
@@ -141,12 +160,13 @@ def generate_launch_description():
        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]),
+        LogInfo(msg=['TTS 服务: ', use_tts]),
+        LogInfo(msg=['QR-TTS 桥接: ', use_qr_tts]),
        vlm_node,
-        tts_node,
+        tts_server,
+        qr_tts_bridge,
    ])
--- a/src/vlm_detect/setup.py
+++ b/src/vlm_detect/setup.py
@@ -27,6 +27,8 @@ setup(
            'vlm_node = vlm_detect.vlm_node:main',
            'test_publisher = vlm_detect.test_publisher:main',
            'tts_node = vlm_detect.tts_node:main',
+            'tts_server = vlm_detect.tts_server:main',
+            'qr_tts_bridge = vlm_detect.qr_tts_bridge:main',
        ],
    },
 )
--- 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/qr_tts_bridge.py
+++ b/src/vlm_detect/vlm_detect/qr_tts_bridge.py
@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+"""
+QR 识别 → TTS 语音播报桥接节点
+订阅 qr_results，调用 /tts/speak 服务
+"""
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from origincar_msg.srv import Speak
+
+
+class QrTtsBridge(Node):
+    def __init__(self):
+        super().__init__('qr_tts_bridge')
+
+        # TTS 服务客户端
+        self.tts_client = self.create_client(Speak, '/tts/speak')
+        while not self.tts_client.wait_for_service(timeout_sec=5.0):
+            self.get_logger().info('Waiting for TTS service...')
+        self.get_logger().info('TTS service connected')
+
+        # 订阅 QR 识别结果
+        self.sub = self.create_subscription(
+            String, 'qr_results', self.callback, 10)
+
+        self.get_logger().info('QR-TTS Bridge ready, listening on qr_results')
+
+    def callback(self, msg):
+        text = msg.data.strip()
+        if not text:
+            return
+
+        self.get_logger().info(f'QR result: {text}')
+
+        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(f'TTS failed: {resp.message}')
+        except Exception as e:
+            self.get_logger().error(f'TTS call error: {e}')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = QrTtsBridge()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
--- a/src/vlm_detect/vlm_detect/test_publisher.py
+++ b/src/vlm_detect/vlm_detect/test_publisher.py
@@ -2,7 +2,7 @@
 # -*- coding: utf-8 -*-
 """
 测试发布者：发送图片和触发信号给 VLM 节点
-用法: ros2 run vlm_detect test_publisher --ros-args -p image_path:="/path/to/image.jpg"
+用法: ros2 run vlm_detect test_publisher --ros-args -p image_path:="/home/sunrise/yiliao_ws/my_model/image.png"
 """

 import rclpy
--- a/src/vlm_detect/vlm_detect/tts_node.py
+++ b/src/vlm_detect/vlm_detect/tts_node.py
@@ -1,60 +1,50 @@
 #!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-import rclpy, subprocess, requests, os
+import rclpy, subprocess, os, wave
 from rclpy.node import Node
 from std_msgs.msg import String
+from piper import PiperVoice
+from piper.config import SynthesisConfig
+
+MODEL_PATH = '/home/sunrise/tts_model/zh_CN-huayan-medium.onnx'

 class TTSNode(Node):
    def __init__(self):
-        super().__init__("tts_node")
-
-        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')
+        super().__init__('tts_node')
+        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
+        self.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}
+        tts_speed = self.get_parameter('tts_speed').value
+        self.length_scale = 1.0 / tts_speed
+        self.espeak_speed = int(175 * tts_speed)  # espeak default=175wpm, scale with tts_speed

        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"
-        )
+        self.get_logger().info(f'Piper TTS started | voice=zh_CN-huayan | speed={tts_speed}x | sink={self.audio_sink}')
+        self.get_logger().info(f'Loading model: {MODEL_PATH}')
+        self.voice = PiperVoice.load(MODEL_PATH)
+        self.get_logger().info('Model loaded OK')

    def callback(self, msg):
-        text = msg.data
-        self.get_logger().info(f"语音播报: {text}")
+        text = msg.data.strip()
+        if not text:
+            return
+        self.get_logger().info(f'TTS: {text}')
        try:
-            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)
-            speed_str = f"atempo={self.tts_speed}"
+            syn_config = SynthesisConfig(length_scale=self.length_scale)
+            wav_path = '/tmp/tts_out.wav'
+            with wave.open(wav_path, 'wb') as wf:
+                self.voice.synthesize_wav(text, wf, syn_config=syn_config)
            subprocess.Popen(
-                ["ffplay", "-nodisp", "-autoexit", "-af", speed_str, self.tmp_mp3],
-                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
-                env=self.audio_env
-            )
+                ['paplay', f'--device={self.audio_sink}', wav_path],
+                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
        except Exception as e:
-            self.get_logger().error(f"TTS 失败, 降级 espeak: {e}")
+            self.get_logger().error(f'Piper TTS failed, fallback espeak: {e}')
            subprocess.Popen(
-                ["espeak-ng", "-v", "zh", "-s", "150", text],
-                env=self.audio_env
-            )
+                ['espeak-ng', '-v', 'cmn', '-s', str(self.espeak_speed), text],
+                env={**os.environ, 'PULSE_SINK': self.audio_sink},
+                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)

 def main(args=None):
    rclpy.init(args=args)
@@ -67,5 +57,5 @@ def main(args=None):
        node.destroy_node()
        rclpy.shutdown()

-if __name__ == "__main__":
+if __name__ == '__main__':
    main()
--- a/src/vlm_detect/vlm_detect/tts_server.py
+++ b/src/vlm_detect/vlm_detect/tts_server.py
@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+"""
+TTS 语音播报服务端 (ROS2 Service Server)
+服务类型: origincar_msg/srv/Speak
+主 TTS: Piper 离线模型  降级: espeak-ng
+"""
+import rclpy
+import subprocess
+import os
+import wave
+
+from rclpy.node import Node
+from origincar_msg.srv import Speak
+
+from piper import PiperVoice
+from piper.config import SynthesisConfig
+
+MODEL_PATH = '/home/sunrise/tts_model/zh_CN-huayan-medium.onnx'
+
+
+class TTSServer(Node):
+    def __init__(self):
+        super().__init__('tts_server')
+
+        self.declare_parameter('audio_sink',
+                               'alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
+        self.declare_parameter('tts_speed', 1.5)
+
+        self.audio_sink = self.get_parameter('audio_sink').value
+        tts_speed = self.get_parameter('tts_speed').value
+        self.length_scale = 1.0 / tts_speed
+        self.espeak_speed = int(175 * tts_speed)
+
+        self.srv = self.create_service(Speak, '/tts/speak', self.handle_speak)
+
+        self.get_logger().info(f'TTS Server ready | voice=zh_CN-huayan | speed={tts_speed}x | sink={self.audio_sink}')
+        self.get_logger().info(f'Loading model: {MODEL_PATH}')
+        self.voice = PiperVoice.load(MODEL_PATH)
+        self.get_logger().info('Model loaded OK')
+
+    def handle_speak(self, request, response):
+        text = request.text.strip()
+        if not text:
+            response.success = False
+            response.message = 'empty text'
+            return response
+
+        self.get_logger().info(f'TTS: {text}')
+
+        try:
+            syn_config = SynthesisConfig(length_scale=self.length_scale)
+            wav_path = '/tmp/tts_out.wav'
+            with wave.open(wav_path, 'wb') as wf:
+                self.voice.synthesize_wav(text, wf, syn_config=syn_config)
+            subprocess.Popen(
+                ['paplay', f'--device={self.audio_sink}', wav_path],
+                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
+            response.success = True
+            response.message = 'ok'
+        except Exception as e:
+            self.get_logger().error(f'Piper TTS failed, fallback espeak: {e}')
+            try:
+                subprocess.Popen(
+                    ['espeak-ng', '-v', 'cmn', '-s', str(self.espeak_speed), text],
+                    env={**os.environ, 'PULSE_SINK': self.audio_sink},
+                    stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
+                response.success = True
+                response.message = 'ok (espeak fallback)'
+            except Exception as e2:
+                self.get_logger().error(f'espeak also failed: {e2}')
+                response.success = False
+                response.message = str(e2)
+
+        return response
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = TTSServer()
+    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,5 +1,8 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
+"""
+VLM 图生文节点 —— 收到触发信号后拍图发给 VLM 服务，结果调用 TTS 服务播报
+"""
 import rclpy
 from rclpy.node import Node
 from std_msgs.msg import Int32, String
@@ -13,6 +16,9 @@ import os
 import time
 import numpy as np

+from origincar_msg.srv import Speak
+
+
 class VLMProcessor(Node):
    def __init__(self):
        super().__init__('vlm_detect')
@@ -24,7 +30,7 @@ class VLMProcessor(Node):
        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('prompt_text', '请描述这张图片的内容，用一句简短的话概括，不超过20个字。')
        self.declare_parameter('max_tokens', 100)

        vlm_host = self.get_parameter('vlm_host').value
@@ -43,7 +49,7 @@ class VLMProcessor(Node):
        )
        self.vlm_model = vlm_model

-        # ROS2 组件
+        # ROS2 通信
        self.bridge = CvBridge()
        self.latest_image = None
        self.image_lock = threading.Lock()
@@ -56,8 +62,13 @@ class VLMProcessor(Node):
        )
        self.result_pub = self.create_publisher(String, result_topic, 10)

+        # TTS 服务客户端
+        self.tts_client = self.create_client(Speak, '/tts/speak')
+        while not self.tts_client.wait_for_service(timeout_sec=5.0):
+            self.get_logger().info('Waiting for TTS service...')
+
        self.get_logger().info(
-            f"VLM Processor 启动 | host={vlm_host} | model={vlm_model} | "
+            f"VLM Processor 就绪 | host={vlm_host} | model={vlm_model} | "
            f"image={image_topic} | trigger={trigger_topic}(sign={self.trigger_sign})"
        )

@@ -73,7 +84,7 @@ class VLMProcessor(Node):

    def sign_callback(self, msg):
        if msg.data == self.trigger_sign:
-            self.get_logger().info(f"收到触发信号 ({msg.data}), 开始处理...")
+            self.get_logger().info(f"收到触发信号 ({msg.data}), 开始推理...")
            with self.image_lock:
                if self.latest_image is None:
                    self.get_logger().warning("无可用图片")
@@ -85,12 +96,34 @@ class VLMProcessor(Node):
            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)
+
+                # 调用 TTS 服务播报
+                if self.tts_client.service_is_ready():
+                    req = Speak.Request()
+                    req.text = description
+                    future = self.tts_client.call_async(req)
+                    future.add_done_callback(self._tts_done_callback)
+                else:
+                    self.get_logger().warning('TTS service not available')
+
                os.remove(temp_path)
            except Exception as e:
-                self.get_logger().error(f"图像处理出错: {e}")
+                self.get_logger().error(f"图像推理失败: {e}")
+
+    def _tts_done_callback(self, future):
+        try:
+            resp = future.result()
+            if resp.success:
+                self.get_logger().debug(f'TTS OK: {resp.message}')
+            else:
+                self.get_logger().warning(f'TTS failed: {resp.message}')
+        except Exception as e:
+            self.get_logger().error(f'TTS call error: {e}')

    def process_image(self, image_path):
        with open(image_path, "rb") as image_file:
@@ -109,10 +142,12 @@ class VLMProcessor(Node):
                ]
            }],
            max_tokens=self.max_tokens,
+            timeout=30,
        )
        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()
@@ -124,5 +159,6 @@ def main(args=None):
        node.destroy_node()
        rclpy.shutdown()

+
 if __name__ == '__main__':
    main()
--- a/test_results/goal_reach_debug_20260722_202420.json
+++ b/test_results/goal_reach_debug_20260722_202420.json
--- a/tf_bag/metadata.yaml
+++ b/tf_bag/metadata.yaml
@@ -0,0 +1,26 @@
+rosbag2_bagfile_information:
+  version: 5
+  storage_identifier: sqlite3
+  duration:
+    nanoseconds: 71999265360
+  starting_time:
+    nanoseconds_since_epoch: 1784703364769624477
+  message_count: 598
+  topics_with_message_count:
+    - topic_metadata:
+        name: /tf
+        type: tf2_msgs/msg/TFMessage
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 598
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - tf_bag_0.db3
+  files:
+    - path: tf_bag_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 1784703364769624477
+      duration:
+        nanoseconds: 71999265360
+      message_count: 598
--- a/tf_bag/tf_bag_0.db3
+++ b/tf_bag/tf_bag_0.db3