vlm和tts

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
+
+

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
+
+# 启动图生文
+
 ```

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(
+    usb_camera = IncludeLaunchDescription(
-        package='usb_cam',
+        PythonLaunchDescriptionSource([
-        executable='usb_cam_node_exe',
+            FindPackageShare('car_usb_cam'), '/launch', '/hobot_usb_cam.launch.py'
-        name='usb_cam',
+        ]),
-        output='screen',
         condition=IfCondition(use_qr),
-        parameters=[{
+        launch_arguments={
-            'video_device': camera_device,
+            'usb_video_device': camera_device,
-            'image_size': [640, 480],
+            'usb_image_width': '640',
-            'pixel_format': 'YUYV',
+            'usb_image_height': '480',
-            'framerate': 30.0,
+            'usb_framerate': '30',
-        }],
+            'usb_pixel_format': 'yuyv2rgb',
+        }.items(),
     )
 
     qr_detect = Node(

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}
 )
 

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
 )

src/origincar_msg/srv/Speak.srv

@@ -0,0 +1,4 @@
+string text
+---
+bool success
+string message

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

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
+    linear_speed: 0.45
-    min_linear_speed: 0.08
+    min_linear_speed: 0.06
-    reverse_speed: 0.25
+    reverse_speed: 0.18
-    slowdown_distance: 0.35
+    slowdown_distance: 0.45
-    curvature_slowdown_gain: 0.10
+    curvature_slowdown_gain: 0.06
-    max_angular_speed: 2.80
+    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

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

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
+    curvature_slowdown_gain: 0.12
-    max_angular_speed: 1.20
+    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

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_lookahead_distance", default_value="0.10"),
-        DeclareLaunchArgument("min_linear_speed", default_value="0.08"),
+        DeclareLaunchArgument("lookahead_curvature_gain", default_value="0.80"),
-        DeclareLaunchArgument("reverse_speed", default_value="0.25"),
+        DeclareLaunchArgument("lookahead_error_gain", default_value="2.00"),
-        DeclareLaunchArgument("slowdown_distance", default_value="0.35"),
+        DeclareLaunchArgument("heading_correction_gain", default_value="0.80"),
-        DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.10"),
+        DeclareLaunchArgument("cross_track_correction_gain", default_value="1.20"),
-        DeclareLaunchArgument("max_angular_speed", default_value="2.80"),
+        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",

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",

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
+            stamp = Time.from_msg(msg.header.stamp)
-        if age < 0.0:
+            stamp_age = max(0.0, (self.get_clock().now() - stamp).nanoseconds * 1e-9)
-            age = 0.0
+        if receive_age is not None and receive_age > self.max_odom_age:
-        if 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 可能发布一个离原始目标较远的路径终点。"

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")

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("--timeout", type=float, default=8.0)
-    parser.add_argument("--repeat", type=int, default=1)
+    parser.add_argument("--repeat", type=int, default=3)
     args = parser.parse_args()
 
     rclpy.init()

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)
+        cross_track_signed = projection["signed_error"]
-        target_x, target_y, target_path_yaw = self.active_path[target_index]
+        cross_track_error = abs(cross_track_signed)
-        nearest_x, nearest_y, _ = self.active_path[nearest_index]
+        path_curvature = self._path_curvature(nearest_index)
-        cross_track_error = math.hypot(nearest_x - x, nearest_y - y)
+        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
 

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

src/vlm_detect/launch/vlm_detect.launch.py

@@ -1,13 +1,15 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
-vlm_detect 联合启动文件
+vlm_detect 启动文件
-同时启动 vlm_node (图生文) 和 tts_node (语音播报)
+同时启动 vlm_node (图生文) + tts_server (语音播报服务) + qr_tts_bridge (二维码播报)
 
 用法:
-  ros2 launch vlm_detect vlm_detect.launch.py                          # 默认配置
+  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 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 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',
+        executable='tts_server',
-        name='tts_node',
+        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,
     ])

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',
         ],
     },
 )

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()

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

src/vlm_detect/vlm_detect/tts_node.py

@@ -1,60 +1,50 @@
 #!/usr/bin/env python3
-# -*- coding: utf-8 -*-
+import rclpy, subprocess, os, wave
-import rclpy, subprocess, requests, os
 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")
+        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('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
+        self.audio_sink = self.get_parameter('audio_sink').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
+        tts_speed = self.get_parameter('tts_speed').value
-        self.tmp_mp3 = self.get_parameter('tmp_mp3_path').value
+        self.length_scale = 1.0 / tts_speed
-        self.tts_speed = self.get_parameter('tts_speed').value
+        self.espeak_speed = int(175 * tts_speed)  # espeak default=175wpm, scale with tts_speed
-
-        self.audio_env = {**os.environ, "PULSE_SINK": audio_sink}
 
         self.sub = self.create_subscription(String, result_topic, self.callback, 10)
-        self.get_logger().info(
+        self.get_logger().info(f'Piper TTS started | voice=zh_CN-huayan | speed={tts_speed}x | sink={self.audio_sink}')
-            f"TTS 节点启动 | host={self.vlm_host} | sink={audio_sink} | "
+        self.get_logger().info(f'Loading model: {MODEL_PATH}')
-            f"voice={self.tts_voice} | speed={self.tts_speed}x"
+        self.voice = PiperVoice.load(MODEL_PATH)
-        )
+        self.get_logger().info('Model loaded OK')
 
     def callback(self, msg):
-        text = msg.data
+        text = msg.data.strip()
-        self.get_logger().info(f"语音播报: {text}")
+        if not text:
+            return
+        self.get_logger().info(f'TTS: {text}')
         try:
-            resp = requests.post(
+            syn_config = SynthesisConfig(length_scale=self.length_scale)
-                f"{self.vlm_host}/v1/tts",
+            wav_path = '/tmp/tts_out.wav'
-                json={"text": text, "voice": self.tts_voice},
+            with wave.open(wav_path, 'wb') as wf:
-                timeout=60
+                self.voice.synthesize_wav(text, wf, syn_config=syn_config)
-            )
-            resp.raise_for_status()
-            with open(self.tmp_mp3, "wb") as f:
-                f.write(resp.content)
-            speed_str = f"atempo={self.tts_speed}"
             subprocess.Popen(
-                ["ffplay", "-nodisp", "-autoexit", "-af", speed_str, self.tmp_mp3],
+                ['paplay', f'--device={self.audio_sink}', wav_path],
-                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
+                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
-                env=self.audio_env
-            )
         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],
+                ['espeak-ng', '-v', 'cmn', '-s', str(self.espeak_speed), text],
-                env=self.audio_env
+                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()

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()

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()

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