test for gc

src/gc/项目总结_yiliao_ws.md

@@ -12,7 +12,7 @@
 ## 一、项目定位
 
 本项目是 RDK X5 机器人上的 ROS 2 Humble 工作区，服务于第21届全国大学生智能汽车竞赛的**医疗赛道**（智慧医疗）。整体采用**调度型架构**：底盘、雷达、相机、二维码、Nav2 导航、VLM 图生文、TTS 语音等功能由独立模块完成，`racing_control` 包作为总调度协调各模块完成比赛流程。
-
+    
 ---
 
 ## 二、项目目录结构总览

src/map/nav2_costmap_map.yaml

@@ -1,4 +1,4 @@
-image: nav2_costmap_binary.png
+image: nav2_costmap_binary_01.png
 mode: trinary
 resolution: 0.01
 origin: [0.0, 0.0, 0.0]

src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260811

@@ -0,0 +1,340 @@
+# ============================================================================
+# nav2_params.yaml — Odometry-only obstacle navigation
+#
+# No static map, no AMCL, no SLAM.
+# Both costmaps are rolling windows in odom. The launch file can rewrite every
+# global_frame leaf when a different connected odometry frame is required.
+# Global planner: Smac Hybrid A* (Reeds-Shepp)
+# Local controller: MPPI (Ackermann)
+# 改了控制频率，微调了一些参数让它不那么喜欢倒车
+# ============================================================================
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom_combined
+    bt_loop_duration: 50
+    default_server_timeout: 20
+    # Injected by obstacle_nav2.launch.py from this package's share directory.
+    default_nav_to_pose_bt_xml: ""
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 15.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 40
+      model_dt: 0.06666666666666666
+      batch_size: 900
+      vx_std: 0.24
+      vy_std: 0.0
+      wz_std: 0.48
+      vx_max: 1.00
+      vx_min: -0.75
+      vy_max: 0.0
+      wz_max: 1.5
+      iteration_count: 1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Ackermann"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.6
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: false
+        cost_power: 1
+        cost_weight: 11.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 100000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 11.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 10
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 5
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: false
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      transform_tolerance: 0.5
+      global_frame: odom
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      track_unknown_space: false
+      plugins: ["obstacle_array_layer", "inflation_layer"]
+      obstacle_array_layer:
+        plugin: "obstacle_nav2::ObstacleArrayLayer"
+        enabled: true
+        topic: /obstacles
+        obstacle_timeout: 0.5
+        transform_tolerance: 0.2
+        default_obstacle_radius: 0.05
+        minimum_obstacle_radius: 0.02
+        maximum_obstacle_radius: 0.50
+        extra_inflation: 0.02
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.20
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      transform_tolerance: 0.5
+      global_frame: map
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: false
+      width: 8
+      height: 8
+      resolution: 0.05
+      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        enabled: true
+        map_subscribe_transient_local: true
+        subscribe_to_updates: false
+      obstacle_array_layer:
+        plugin: "obstacle_nav2::ObstacleArrayLayer"
+        enabled: true
+        topic: /obstacles
+        obstacle_timeout: 0.5
+        transform_tolerance: 0.2
+        default_obstacle_radius: 0.05
+        minimum_obstacle_radius: 0.02
+        maximum_obstacle_radius: 0.50
+        extra_inflation: 0.02
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.35
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: False
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      downsample_costmap: false
+      downsampling_factor: 1
+      tolerance: 0.15
+      allow_unknown: false
+      max_iterations: 1000000
+      max_on_approach_iterations: 1000
+      max_planning_time: 25.0
+      motion_model_for_search: "REEDS_SHEPP"
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 3.5
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.45
+      reverse_penalty: 4.0
+      change_penalty: 1.2
+      non_straight_penalty: 0.7
+      cost_penalty: 3.0
+      retrospective_penalty: 0.015
+      # 5 m covers the rolling planning horizon without the startup and memory
+      # cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
+      lookup_table_size: 5.0
+      cache_obstacle_heuristic: false
+      viz_expansions: false
+      smooth_path: True
+      smoother:
+        max_iterations: 700
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 2
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: False
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin","wait","backup"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+      backup_dist: 0.8
+      backup_speed: 0.18
+    wait:
+      plugin: "nav2_behaviors/Wait"
+      wait_duration: 0.5
+    global_frame: map
+    robot_base_frame: base_footprint
+    transform_tolerance: 0.5
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: False
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: False
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [1.00, 0.0, 2.0]
+    min_velocity: [-0.75, 0.0, -2.0]
+    max_accel: [3.73, 0.0, 3.2]
+    max_decel: [-1.1, 0.0, -4.5]
+    odom_topic: /odom_combined
+    odom_duration: 0.1
+    deadband_velocity: [0.03, 0.0, 0.03]
+    velocity_timeout: 1.0

src/navigation/obstacle_nav2/config/nav2_profile_10.yaml

@@ -71,13 +71,13 @@ bt_navigator_rclcpp_node:
 controller_server:
   ros__parameters:
     use_sim_time: False
-    controller_frequency: 15.0
+    controller_frequency: 10.0
     FollowPath:
       plugin: "nav2_mppi_controller::MPPIController"
-      time_steps: 40
+      time_steps: 36
-      model_dt: 0.06666666666666666
+      model_dt: 0.10
-      batch_size: 900
+      batch_size: 700
-      vx_std: 0.25
+      vx_std: 0.22
       vy_std: 0.0
       wz_std: 0.45
       vx_max: 1.00
@@ -112,12 +112,12 @@ controller_server:
       PreferForwardCritic:
         enabled: false
         cost_power: 1
-        cost_weight: 9.0
+        cost_weight: 4.0
         threshold_to_consider: 0.5
       CostCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 5.0
+        cost_weight: 3.81
         critical_cost: 300.0
         consider_footprint: true
         collision_cost: 100000.0
@@ -135,7 +135,7 @@ controller_server:
       PathFollowCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 5.0
+        cost_weight: 4.0
         offset_from_furthest: 10
         threshold_to_consider: 1.4
       PathAngleCritic:
@@ -181,7 +181,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.20
+        inflation_radius: 0.35
       always_send_full_costmap: True
   local_costmap_client:
     ros__parameters:
@@ -224,8 +224,8 @@ global_costmap:
         extra_inflation: 0.02
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
+        cost_scaling_factor: 2.0
-        inflation_radius: 0.35
+        inflation_radius: 0.3
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:
@@ -246,16 +246,16 @@ planner_server:
       allow_unknown: false
       max_iterations: 1000000
       max_on_approach_iterations: 1000
-      max_planning_time: 25.0
+      max_planning_time: 5.0
       motion_model_for_search: "REEDS_SHEPP"
       angle_quantization_bins: 72
       analytic_expansion_ratio: 3.5
       analytic_expansion_max_length: 3.0
-      minimum_turning_radius: 0.45
+      minimum_turning_radius: 0.40
-      reverse_penalty: 4.0
+      reverse_penalty: 3.0
-      change_penalty: 1.0
+      change_penalty: 0.0
-      non_straight_penalty: 1.0
+      non_straight_penalty: 1.2
-      cost_penalty: 3.0
+      cost_penalty: 2.0
       retrospective_penalty: 0.015
       # 5 m covers the rolling planning horizon without the startup and memory
       # cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
@@ -264,7 +264,7 @@ planner_server:
       viz_expansions: false
       smooth_path: True
       smoother:
-        max_iterations: 700
+        max_iterations: 1000
         w_smooth: 0.3
         w_data: 0.2
         tolerance: 1.0e-10

src/racing_control/CMakeLists.txt

@@ -1,6 +1,9 @@
 cmake_minimum_required(VERSION 3.8)
 project(racing_control)
 
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
@@ -15,8 +18,21 @@ find_package(rclcpp_action REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 
+add_library(racing_control_core
+  src/candidate_waypoint_selector.cpp
+)
+target_include_directories(racing_control_core PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(racing_control_core
+  geometry_msgs
+  nav_msgs
+)
+
 add_executable(racing_control
   src/racing_control.cpp
+  src/candidate_waypoint_selector.cpp
 )
 target_include_directories(racing_control PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -33,6 +49,7 @@ ament_target_dependencies(racing_control
 )
 
 install(TARGETS
+  racing_control_core
   racing_control
   DESTINATION lib/${PROJECT_NAME}
 )
@@ -63,8 +80,12 @@ if(BUILD_TESTING)
   )
   ament_target_dependencies(test_racing_control_helpers
     geometry_msgs
+    nav_msgs
     rclcpp
   )
+  target_sources(test_racing_control_helpers PRIVATE
+    src/candidate_waypoint_selector.cpp
+  )
 
   ament_lint_auto_find_test_dependencies()
 endif()

src/racing_control/config/racing_control.yaml

@@ -8,6 +8,7 @@ racing_control:
     enable_vlm_image_relay: false
     enable_dynamic_replanning: false
     enable_recovery: true
+    enable_candidate_waypoint_selection: true
     vlm_image_input_topic: /image
     vlm_image_output_topic: /vlm_image
 
@@ -17,6 +18,7 @@ racing_control:
     vlm_result_topic: /vlm_result
     odom_topic: /odom_combined
     recovery_cmd_vel_topic: /cmd_vel
+    global_costmap_topic: /global_costmap/costmap
     trajectory_guard_input_topic: /trajectory_guard/input_path
 
     # Nav2 actions and plugin IDs
@@ -42,6 +44,15 @@ racing_control:
     recovery_backup_speed: -0.2
     recovery_backup_distance: 0.04
     recovery_backup_timeout_sec: 0.2
+    # Used only when the initial ComputePathThroughPoses request fails.
+    planning_failure_backup_speed: -0.1
+    planning_failure_backup_distance: 0.03
+    planning_failure_backup_timeout_sec: 0.3
+    recovery_clear_wait_sec: 1.0
+    recovery_search_radius_m: 1.0
+    recovery_rear_clear_distance_m: 0.5
+    global_costmap_clear_service: global_costmap/clear_entirely_global_costmap
+    local_costmap_clear_service: local_costmap/clear_entirely_local_costmap
     max_recovery_attempts: 2
     circle_goal_tolerance: 0.50
 
@@ -57,6 +68,17 @@ racing_control:
     sign_vlm_trigger: 9
     sign_profile_normal: 10
     sign_profile_task2: 11
+    vlm_trigger_repeat_count: 5
+    vlm_trigger_interval_sec: 0.5
+
+    # Candidate waypoint JSONs reuse the saved-point format.
+    # home is intentionally never replaced.
+    candidate_waypoint_json_dir: /home/sunrise/yiliao_ws/src/racing_control/config/waypoints
+    qr_candidate_group_name: qr
+    entry_candidate_group_name: entry
+    vlm_candidate_group_name: goal_005
+    clockwise_candidate_source_file: main_1.json
+    counterclockwise_candidate_source_file: main_2.json
 
     # Pose parameters are flat x/y/yaw-radians triples in frame_id.
     qr_pose: [4.333107888975101, 1.028867429995691, 1.1383894869544392]

src/racing_control/config/waypoints/main.json

@@ -0,0 +1,106 @@
+{
+  "topic": "/odom_combined",
+  "message_type": "nav_msgs/msg/Odometry",
+  "saved_at": "2026-08-07T14:42:49.402Z",
+  "count": 2,
+  "points": [
+    {
+      "name": "qr",
+      "captured_at": "2026-08-07T14:42:33.118Z",
+      "received_at": null,
+      "yaw_degrees": 65.22491304455252,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 4.333107888975101,
+              "y": 1.028867429995691,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.5389539275964809,
+              "w": 0.8423352443821446
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 65.22491304455252
+      }
+    },
+    {
+      "name": "entry",
+      "captured_at": "2026-08-07T14:42:45.961Z",
+      "received_at": null,
+      "yaw_degrees": 87.09617569507752,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 2.504811372926262,
+              "y": 2.2798075935366624,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.6889631335573172,
+              "w": 0.7247963856138373
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 87.09617569507752
+      }
+    }
+  ]
+}

src/racing_control/config/waypoints/main_1.json

@@ -0,0 +1,155 @@
+{
+  "topic": "/odom_combined",
+  "message_type": "nav_msgs/msg/Odometry",
+  "saved_at": "2026-08-07T14:44:10.775Z",
+  "count": 3,
+  "points": [
+    {
+      "name": "goal_004",
+      "captured_at": "2026-08-07T14:43:55.328Z",
+      "received_at": null,
+      "yaw_degrees": 89.33764149250207,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 0.7534956931109804,
+              "y": 3.766501234923627,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.7030077953706314,
+              "w": 0.7111821423855668
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 89.33764149250207
+      }
+    },
+    {
+      "name": "goal_005",
+      "captured_at": "2026-08-07T14:44:02.099Z",
+      "received_at": null,
+      "yaw_degrees": 2.2906028734862383,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 3.8567885105264077,
+              "y": 4.329424195458402,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.019987949834902125,
+              "w": 0.9998002209748693
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 2.2906028734862383
+      }
+    },
+    {
+      "name": "goal_006",
+      "captured_at": "2026-08-07T14:44:09.250Z",
+      "received_at": null,
+      "yaw_degrees": -92.25458353863968,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 2.5144339572664096,
+              "y": 2.4337692660037766,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": -0.720881318872454,
+              "w": 0.6930585286256215
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": -92.25458353863968
+      }
+    }
+  ]
+}

src/racing_control/config/waypoints/main_2.json

@@ -0,0 +1,155 @@
+{
+  "topic": "/odom_combined",
+  "message_type": "nav_msgs/msg/Odometry",
+  "saved_at": "2026-08-07T14:43:29.485Z",
+  "count": 3,
+  "points": [
+    {
+      "name": "goal_001",
+      "captured_at": "2026-08-07T14:43:14.866Z",
+      "received_at": null,
+      "yaw_degrees": 88.52360610794565,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 4.2128251001861265,
+              "y": 3.7376334819031842,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.6979380047775942,
+              "w": 0.7161581818893582
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 88.52360610794565
+      }
+    },
+    {
+      "name": "goal_002",
+      "captured_at": "2026-08-07T14:43:20.968Z",
+      "received_at": null,
+      "yaw_degrees": 178.26427158937722,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 1.1720794040064113,
+              "y": 4.319801449605877,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": 0.99988528505826,
+              "w": 0.015146508639358486
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": 178.26427158937722
+      }
+    },
+    {
+      "name": "goal_003",
+      "captured_at": "2026-08-07T14:43:28.294Z",
+      "received_at": null,
+      "yaw_degrees": -88.21009502116203,
+      "odom": {
+        "header": {
+          "frame_id": "odom",
+          "stamp": {
+            "sec": 0,
+            "nanosec": 0
+          },
+          "stamp_iso": null
+        },
+        "child_frame_id": "base_link",
+        "pose": {
+          "pose": {
+            "position": {
+              "x": 2.4951887885861144,
+              "y": 2.318297930897253,
+              "z": 0
+            },
+            "orientation": {
+              "x": 0,
+              "y": 0,
+              "z": -0.6959760577153663,
+              "w": 0.7180649880665239
+            }
+          },
+          "covariance": []
+        },
+        "twist": {
+          "twist": {
+            "linear": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            },
+            "angular": {
+              "x": 0,
+              "y": 0,
+              "z": 0
+            }
+          },
+          "covariance": []
+        },
+        "yaw_degrees": -88.21009502116203
+      }
+    }
+  ]
+}

src/racing_control/include/racing_control/candidate_waypoint_selector.hpp

@@ -0,0 +1,89 @@
+#ifndef RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_
+#define RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_
+
+#include <optional>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
+namespace racing_control
+{
+
+struct CandidateWaypoint
+{
+  std::string group_name;
+  std::string source_file;
+  std::string point_name;
+  geometry_msgs::msg::PoseStamped pose;
+};
+
+using CandidateWaypointGroups = std::unordered_map<std::string, std::vector<CandidateWaypoint>>;
+
+struct RecoveryCommand
+{
+  double linear_x{0.0};
+  double angular_z{0.0};
+  double duration_sec{0.0};
+  std::string label;
+};
+
+CandidateWaypointGroups loadCandidateWaypointGroups(
+  const std::string & json_dir,
+  const std::string & frame_id);
+
+std::optional<RecoveryCommand> nearestFreeRecoveryCommand(
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const geometry_msgs::msg::PoseStamped & current,
+  int occupied_threshold,
+  bool treat_unknown_as_occupied,
+  double search_radius_m = 1.0);
+
+std::optional<RecoveryCommand> rearClearRecoveryCommand(
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const geometry_msgs::msg::PoseStamped & current,
+  int occupied_threshold,
+  bool treat_unknown_as_occupied,
+  double rear_clear_distance_m);
+
+class CandidateWaypointSelector
+{
+public:
+  void setGroups(CandidateWaypointGroups groups);
+  void setCostmap(const nav_msgs::msg::OccupancyGrid & costmap);
+
+  std::optional<CandidateWaypoint> select(
+    const std::string & group_name,
+    int occupied_threshold,
+    bool treat_unknown_as_occupied,
+    const std::string & preferred_source_file = "") const;
+
+  std::optional<RecoveryCommand> nearestFreeRecoveryCommand(
+    const geometry_msgs::msg::PoseStamped & current,
+    int occupied_threshold,
+    bool treat_unknown_as_occupied,
+    double search_radius_m = 1.0) const;
+
+  bool isPoseOccupied(
+    const geometry_msgs::msg::PoseStamped & pose,
+    int occupied_threshold,
+    bool treat_unknown_as_occupied) const;
+
+private:
+  bool isCandidateOccupied(
+    const geometry_msgs::msg::PoseStamped & pose,
+    int occupied_threshold,
+    bool treat_unknown_as_occupied) const;
+
+  std::optional<RecoveryCommand> commandForRecoveryAngle(double angle_rad) const;
+
+  CandidateWaypointGroups groups_;
+  nav_msgs::msg::OccupancyGrid costmap_;
+  bool has_costmap_{false};
+};
+
+}  // namespace racing_control
+
+#endif  // RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_

src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup.md5~

@@ -0,0 +1 @@
+4773a55f2a69d3a5f983b4f5cbe49312  candidate_waypoint_selector.hpp

src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup~

@@ -0,0 +1,190 @@
+#ifndef RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_
+#define RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_
+
+#include <cmath>
+#include <filesystem>
+#include <fstream>
+#include <map>
+#include <optional>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include <nlohmann/json.hpp>
+
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "racing_control/racing_control.hpp"
+
+namespace racing_control
+{
+
+struct CandidateWaypoint
+{
+  std::string group;
+  std::string source_file;
+  std::string point_name;
+  geometry_msgs::msg::PoseStamped pose;
+};
+
+struct RecoveryCommand
+{
+  double linear_x{0.0};
+  double angular_z{0.0};
+};
+
+using CandidateWaypointGroups = std::map<std::string, std::vector<CandidateWaypoint>>;
+
+inline geometry_msgs::msg::PoseStamped candidatePoseFromJson(
+  const nlohmann::json & point, const std::string & frame_id)
+{
+  const auto & pose = point.at("odom").at("pose").at("pose");
+  const auto & position = pose.at("position");
+  const auto & orientation = pose.at("orientation");
+  geometry_msgs::msg::PoseStamped result;
+  result.header.frame_id = frame_id;
+  result.pose.position.x = position.value("x", 0.0);
+  result.pose.position.y = position.value("y", 0.0);
+  result.pose.position.z = position.value("z", 0.0);
+  result.pose.orientation.x = orientation.value("x", 0.0);
+  result.pose.orientation.y = orientation.value("y", 0.0);
+  result.pose.orientation.z = orientation.value("z", 0.0);
+  result.pose.orientation.w = orientation.value("w", 1.0);
+  if (point.contains("yaw_degrees")) {
+    const auto yaw = point.at("yaw_degrees").get<double>() * M_PI / 180.0;
+    result.pose.orientation.z = std::sin(yaw * 0.5);
+    result.pose.orientation.w = std::cos(yaw * 0.5);
+  }
+  return result;
+}
+
+inline CandidateWaypointGroups loadCandidateWaypointGroups(
+  const std::string & directory, const std::string & frame_id)
+{
+  CandidateWaypointGroups groups;
+  for (const auto & entry : std::filesystem::directory_iterator(directory)) {
+    if (!entry.is_regular_file() || entry.path().extension() != ".json") {
+      continue;
+    }
+    std::ifstream input(entry.path());
+    const auto document = nlohmann::json::parse(input);
+    for (const auto & point : document.value("points", nlohmann::json::array())) {
+      const auto point_name = point.value("name", std::string{});
+      if (point_name.empty()) {
+        continue;
+      }
+      groups[point_name].push_back(
+        CandidateWaypoint{
+          point_name, entry.path().filename().string(), point_name,
+          candidatePoseFromJson(point, frame_id)});
+    }
+  }
+  return groups;
+}
+
+class CandidateWaypointSelector
+{
+public:
+  void setGroups(CandidateWaypointGroups groups)
+  {
+    groups_ = std::move(groups);
+  }
+
+  void setCostmap(const nav_msgs::msg::OccupancyGrid & costmap)
+  {
+    costmap_ = costmap;
+  }
+
+  std::optional<CandidateWaypoint> select(
+    const std::string & group, const int occupied_threshold, const bool allow_unknown) const
+  {
+    const auto it = groups_.find(group);
+    if (it == groups_.end()) {
+      return std::nullopt;
+    }
+    for (const auto & candidate : it->second) {
+      if (isFree(candidate.pose, occupied_threshold, allow_unknown)) {
+        return candidate;
+      }
+    }
+    return std::nullopt;
+  }
+
+private:
+  bool isFree(
+    const geometry_msgs::msg::PoseStamped & pose,
+    const int occupied_threshold,
+    const bool allow_unknown) const
+  {
+    if (costmap_.info.resolution <= 0.0 || costmap_.info.width == 0 || costmap_.info.height == 0) {
+      return true;
+    }
+    const auto mx = static_cast<int>(std::floor(
+      (pose.pose.position.x - costmap_.info.origin.position.x) / costmap_.info.resolution));
+    const auto my = static_cast<int>(std::floor(
+      (pose.pose.position.y - costmap_.info.origin.position.y) / costmap_.info.resolution));
+    if (mx < 0 || my < 0 || mx >= static_cast<int>(costmap_.info.width) ||
+      my >= static_cast<int>(costmap_.info.height))
+    {
+      return false;
+    }
+    const auto value = costmap_.data[
+      static_cast<std::size_t>(my) * costmap_.info.width + static_cast<std::size_t>(mx)];
+    return value < 0 ? allow_unknown : value < occupied_threshold;
+  }
+
+  CandidateWaypointGroups groups_;
+  nav_msgs::msg::OccupancyGrid costmap_;
+};
+
+inline std::optional<RecoveryCommand> nearestFreeRecoveryCommand(
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const geometry_msgs::msg::PoseStamped & current,
+  const int occupied_threshold,
+  const bool allow_unknown,
+  const double max_distance)
+{
+  if (costmap.info.resolution <= 0.0 || costmap.info.width == 0 || costmap.info.height == 0) {
+    return std::nullopt;
+  }
+
+  const auto yaw = std::atan2(
+    2.0 * (current.pose.orientation.w * current.pose.orientation.z),
+    1.0 - 2.0 * current.pose.orientation.z * current.pose.orientation.z);
+  const auto cos_yaw = std::cos(yaw);
+  const auto sin_yaw = std::sin(yaw);
+  double best_distance = max_distance;
+  double best_forward = 0.0;
+  bool found = false;
+  for (std::size_t my = 0; my < costmap.info.height; ++my) {
+    for (std::size_t mx = 0; mx < costmap.info.width; ++mx) {
+      const auto value = costmap.data[my * costmap.info.width + mx];
+      if (value < 0 ? !allow_unknown : value >= occupied_threshold) {
+        continue;
+      }
+      const auto world_x = costmap.info.origin.position.x +
+        (static_cast<double>(mx) + 0.5) * costmap.info.resolution;
+      const auto world_y = costmap.info.origin.position.y +
+        (static_cast<double>(my) + 0.5) * costmap.info.resolution;
+      const auto dx = world_x - current.pose.position.x;
+      const auto dy = world_y - current.pose.position.y;
+      const auto forward = cos_yaw * dx + sin_yaw * dy;
+      const auto lateral = -sin_yaw * dx + cos_yaw * dy;
+      const auto distance = std::hypot(forward, lateral);
+      if (distance <= 0.0 || distance > max_distance || distance >= best_distance) {
+        continue;
+      }
+      best_distance = distance;
+      best_forward = forward;
+      found = true;
+    }
+  }
+  if (!found) {
+    return std::nullopt;
+  }
+  return RecoveryCommand{best_forward >= 0.0 ? 0.2 : -0.2, 0.0};
+}
+
+}  // namespace racing_control
+
+#endif  // RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_

src/racing_control/include/racing_control/racing_control.hpp

@@ -33,6 +33,64 @@ enum class QrTransitTarget
   PostQr
 };
 
+enum class RouteRecoveryPhase
+{
+  Initial,
+  AfterPlanningBackup,
+  AfterLocalClearReplan,
+  AfterFinalBackupReplan
+};
+
+enum class RouteFailureKind
+{
+  Planning,
+  Following
+};
+
+enum class RouteRecoveryAction
+{
+  Fail,
+  PlanningBackup,
+  ClearLocalAndReplan,
+  FinalBackupReplan
+};
+
+inline RouteRecoveryAction routeRecoveryAction(
+  const RouteRecoveryPhase phase,
+  const RouteFailureKind failure,
+  const int final_recovery_attempts,
+  const int max_final_recovery_attempts)
+{
+  const auto finalBackupOrFail = [&]() {
+      return final_recovery_attempts < max_final_recovery_attempts ?
+             RouteRecoveryAction::FinalBackupReplan : RouteRecoveryAction::Fail;
+    };
+
+  if (failure == RouteFailureKind::Planning) {
+    if (phase == RouteRecoveryPhase::Initial) {
+      return RouteRecoveryAction::PlanningBackup;
+    }
+    if (phase == RouteRecoveryPhase::AfterLocalClearReplan ||
+      phase == RouteRecoveryPhase::AfterFinalBackupReplan)
+    {
+      return finalBackupOrFail();
+    }
+    return RouteRecoveryAction::Fail;
+  }
+
+  if (phase == RouteRecoveryPhase::Initial ||
+    phase == RouteRecoveryPhase::AfterPlanningBackup)
+  {
+    return RouteRecoveryAction::ClearLocalAndReplan;
+  }
+  if (phase == RouteRecoveryPhase::AfterLocalClearReplan ||
+    phase == RouteRecoveryPhase::AfterFinalBackupReplan)
+  {
+    return finalBackupOrFail();
+  }
+  return RouteRecoveryAction::Fail;
+}
+
 inline VlmCaptureMode vlmCaptureModeFromString(const std::string & value)
 {
   std::string normalized;

src/racing_control/src/candidate_waypoint_selector.cpp

@@ -0,0 +1,314 @@
+#include "racing_control/candidate_waypoint_selector.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+#include <filesystem>
+#include <fstream>
+#include <limits>
+#include <nlohmann/json.hpp>
+
+namespace racing_control
+{
+namespace
+{
+
+double normalizeAngle(double angle)
+{
+  while (angle > M_PI) {
+    angle -= 2.0 * M_PI;
+  }
+  while (angle < -M_PI) {
+    angle += 2.0 * M_PI;
+  }
+  return angle;
+}
+
+double yawFromQuaternionInternal(
+  const double x, const double y, const double z, const double w)
+{
+  return std::atan2(
+    2.0 * (w * z + x * y),
+    1.0 - 2.0 * (y * y + z * z));
+}
+
+std::vector<std::filesystem::path> sortedJsonFiles(const std::filesystem::path & json_dir)
+{
+  std::vector<std::filesystem::path> files;
+  for (const auto & entry : std::filesystem::directory_iterator(json_dir)) {
+    if (entry.is_regular_file() && entry.path().extension() == ".json") {
+      files.push_back(entry.path());
+    }
+  }
+  std::sort(
+    files.begin(), files.end(), [](const auto & a, const auto & b) {
+      return a.filename().string() < b.filename().string();
+    });
+  return files;
+}
+
+geometry_msgs::msg::PoseStamped poseFromJsonPoint(
+  const nlohmann::json & point,
+  const std::string & frame_id)
+{
+  const auto & odom = point.at("odom");
+  const auto & pose = odom.at("pose").at("pose");
+  const auto & position = pose.at("position");
+  const auto & orientation = pose.at("orientation");
+
+  const double x = position.at("x").get<double>();
+  const double y = position.at("y").get<double>();
+  const double yaw = point.contains("yaw_degrees") ?
+    point.at("yaw_degrees").get<double>() * M_PI / 180.0 :
+    yawFromQuaternionInternal(
+    orientation.at("x").get<double>(), orientation.at("y").get<double>(),
+    orientation.at("z").get<double>(), orientation.at("w").get<double>());
+
+  geometry_msgs::msg::PoseStamped stamped;
+  stamped.header.frame_id = frame_id;
+  stamped.pose.position.x = x;
+  stamped.pose.position.y = y;
+  stamped.pose.position.z = 0.0;
+  stamped.pose.orientation.z = std::sin(yaw * 0.5);
+  stamped.pose.orientation.w = std::cos(yaw * 0.5);
+  return stamped;
+}
+
+bool fileContainsGroup(
+  const std::string & source_file,
+  const std::string & preferred_source_file)
+{
+  if (preferred_source_file.empty()) {
+    return false;
+  }
+  return source_file == preferred_source_file;
+}
+
+}  // namespace
+
+CandidateWaypointGroups loadCandidateWaypointGroups(
+  const std::string & json_dir,
+  const std::string & frame_id)
+{
+  CandidateWaypointGroups groups;
+  const auto root = std::filesystem::path(json_dir);
+  if (json_dir.empty() || !std::filesystem::exists(root)) {
+    return groups;
+  }
+
+  for (const auto & path : sortedJsonFiles(root)) {
+    std::ifstream input(path);
+    if (!input) {
+      continue;
+    }
+    nlohmann::json data;
+    try {
+      input >> data;
+    } catch (...) {
+      continue;
+    }
+
+    const auto points = data.is_array() ? data : data.value("points", nlohmann::json::array());
+    if (!points.is_array()) {
+      continue;
+    }
+
+    for (const auto & point : points) {
+      if (!point.is_object() || !point.contains("odom") || !point["odom"].is_object()) {
+        continue;
+      }
+      CandidateWaypoint candidate;
+      candidate.source_file = path.filename().string();
+      candidate.point_name = point.value("name", path.stem().string());
+      candidate.group_name = candidate.point_name;
+      candidate.pose = poseFromJsonPoint(point, frame_id);
+      groups[candidate.group_name].push_back(std::move(candidate));
+    }
+  }
+
+  return groups;
+}
+
+void CandidateWaypointSelector::setGroups(CandidateWaypointGroups groups)
+{
+  groups_ = std::move(groups);
+}
+
+void CandidateWaypointSelector::setCostmap(const nav_msgs::msg::OccupancyGrid & costmap)
+{
+  costmap_ = costmap;
+  has_costmap_ = true;
+}
+
+std::optional<CandidateWaypoint> CandidateWaypointSelector::select(
+  const std::string & group_name,
+  const int occupied_threshold,
+  const bool treat_unknown_as_occupied,
+  const std::string & preferred_source_file) const
+{
+  const auto group_it = groups_.find(group_name);
+  if (group_it == groups_.end() || group_it->second.empty() || !has_costmap_) {
+    return std::nullopt;
+  }
+
+  std::vector<const CandidateWaypoint *> ordered;
+  ordered.reserve(group_it->second.size());
+  for (const auto & candidate : group_it->second) {
+    ordered.push_back(&candidate);
+  }
+  std::stable_sort(
+    ordered.begin(), ordered.end(), [&](const auto * lhs, const auto * rhs) {
+      const bool lhs_preferred = fileContainsGroup(lhs->source_file, preferred_source_file);
+      const bool rhs_preferred = fileContainsGroup(rhs->source_file, preferred_source_file);
+      if (lhs_preferred != rhs_preferred) {
+        return lhs_preferred;
+      }
+      return lhs->source_file < rhs->source_file;
+    });
+
+  for (const auto * candidate : ordered) {
+    if (!isCandidateOccupied(candidate->pose, occupied_threshold, treat_unknown_as_occupied)) {
+      return *candidate;
+    }
+  }
+  return std::nullopt;
+}
+
+std::optional<RecoveryCommand> nearestFreeRecoveryCommand(
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const geometry_msgs::msg::PoseStamped & current,
+  const int occupied_threshold,
+  const bool treat_unknown_as_occupied,
+  const double search_radius_m)
+{
+  CandidateWaypointSelector selector;
+  selector.setCostmap(costmap);
+  return selector.nearestFreeRecoveryCommand(
+    current, occupied_threshold, treat_unknown_as_occupied, search_radius_m);
+}
+
+std::optional<RecoveryCommand> rearClearRecoveryCommand(
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const geometry_msgs::msg::PoseStamped & current,
+  const int occupied_threshold,
+  const bool treat_unknown_as_occupied,
+  const double rear_clear_distance_m)
+{
+  CandidateWaypointSelector selector;
+  selector.setCostmap(costmap);
+  const double yaw = 2.0 * std::atan2(current.pose.orientation.z, current.pose.orientation.w);
+  geometry_msgs::msg::PoseStamped rear = current;
+  rear.pose.position.x -= std::cos(yaw) * rear_clear_distance_m;
+  rear.pose.position.y -= std::sin(yaw) * rear_clear_distance_m;
+  if (selector.isPoseOccupied(rear, occupied_threshold, treat_unknown_as_occupied)) {
+    return std::nullopt;
+  }
+  return RecoveryCommand{-0.25, 0.0, rear_clear_distance_m / 0.25, "rear-clear-backward"};
+}
+
+std::optional<RecoveryCommand> CandidateWaypointSelector::nearestFreeRecoveryCommand(
+  const geometry_msgs::msg::PoseStamped & current,
+  const int occupied_threshold,
+  const bool treat_unknown_as_occupied,
+  const double search_radius_m) const
+{
+  if (!has_costmap_) {
+    return std::nullopt;
+  }
+
+  const double yaw = 2.0 * std::atan2(current.pose.orientation.z, current.pose.orientation.w);
+  const std::vector<double> distances{0.25, 0.5, 0.75, std::max(0.25, search_radius_m)};
+  const std::vector<double> angles{0.0, M_PI / 4.0, -M_PI / 4.0, M_PI / 2.0, -M_PI / 2.0, M_PI};
+
+  double best_score = std::numeric_limits<double>::infinity();
+  std::optional<RecoveryCommand> best_command;
+  for (const double distance : distances) {
+    for (const double angle : angles) {
+      const double sample_angle = normalizeAngle(yaw + angle);
+      geometry_msgs::msg::PoseStamped sample = current;
+      sample.pose.position.x += std::cos(sample_angle) * distance;
+      sample.pose.position.y += std::sin(sample_angle) * distance;
+      if (isCandidateOccupied(sample, occupied_threshold, treat_unknown_as_occupied)) {
+        continue;
+      }
+      const double score = distance + std::abs(angle) * 0.1;
+      if (score < best_score) {
+        best_score = score;
+        best_command = commandForRecoveryAngle(angle);
+      }
+    }
+  }
+  return best_command;
+}
+
+bool CandidateWaypointSelector::isPoseOccupied(
+  const geometry_msgs::msg::PoseStamped & pose,
+  const int occupied_threshold,
+  const bool treat_unknown_as_occupied) const
+{
+  return isCandidateOccupied(pose, occupied_threshold, treat_unknown_as_occupied);
+}
+
+bool CandidateWaypointSelector::isCandidateOccupied(
+  const geometry_msgs::msg::PoseStamped & pose,
+  const int occupied_threshold,
+  const bool treat_unknown_as_occupied) const
+{
+  const auto & costmap = costmap_;
+  const auto & origin = costmap.info.origin;
+  const double resolution = costmap.info.resolution;
+  const auto width = static_cast<int>(costmap.info.width);
+  const auto height = static_cast<int>(costmap.info.height);
+  if (resolution <= 0.0 || width <= 0 || height <= 0) {
+    return true;
+  }
+
+  const double origin_yaw = yawFromQuaternionInternal(
+    origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
+  const double dx = pose.pose.position.x - origin.position.x;
+  const double dy = pose.pose.position.y - origin.position.y;
+  const double cos_yaw = std::cos(-origin_yaw);
+  const double sin_yaw = std::sin(-origin_yaw);
+  const double local_x = cos_yaw * dx - sin_yaw * dy;
+  const double local_y = sin_yaw * dx + cos_yaw * dy;
+  const auto mx = static_cast<int>(std::floor(local_x / resolution));
+  const auto my = static_cast<int>(std::floor(local_y / resolution));
+  if (mx < 0 || my < 0 || mx >= width || my >= height) {
+    return true;
+  }
+
+  const auto index = my * width + mx;
+  if (index < 0 || index >= static_cast<int>(costmap.data.size())) {
+    return true;
+  }
+
+  const auto cost = static_cast<int>(costmap.data[static_cast<std::size_t>(index)]);
+  if (cost < 0) {
+    return treat_unknown_as_occupied;
+  }
+  return cost >= occupied_threshold;
+}
+
+std::optional<RecoveryCommand> CandidateWaypointSelector::commandForRecoveryAngle(
+  const double angle_rad) const
+{
+  const double angle = normalizeAngle(angle_rad);
+  if (std::abs(angle) < M_PI / 8.0) {
+    return RecoveryCommand{0.25, 0.0, 0.6, "forward"};
+  }
+  if (std::abs(std::abs(angle) - M_PI) < M_PI / 8.0) {
+    return RecoveryCommand{-0.25, 0.0, 0.6, "backward"};
+  }
+  if (angle > 0.0 && angle < M_PI / 2.0) {
+    return RecoveryCommand{0.25, 0.7, 0.6, "forward-left"};
+  }
+  if (angle < 0.0 && angle > -M_PI / 2.0) {
+    return RecoveryCommand{0.25, -0.7, 0.6, "forward-right"};
+  }
+  if (angle >= M_PI / 2.0) {
+    return RecoveryCommand{0.0, 0.7, 0.4, "left"};
+  }
+  return RecoveryCommand{0.0, -0.7, 0.4, "right"};
+}
+
+}  // namespace racing_control

src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup.md5~

@@ -0,0 +1 @@
+38f483033e12bc75920274763b0c7e0b  candidate_waypoint_selector.cpp

src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup~

@@ -0,0 +1,312 @@
+#include "racing_control/candidate_waypoint_selector.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+#include <filesystem>
+#include <fstream>
+#include <limits>
+#include <nlohmann/json.hpp>
+
+namespace racing_control
+{
+namespace
+{
+
+double normalizeAngle(double angle)
+{
+  while (angle > M_PI) {
+    angle -= 2.0 * M_PI;
+  }
+  while (angle < -M_PI) {
+    angle += 2.0 * M_PI;
+  }
+  return angle;
+}
+
+double yawFromQuaternionInternal(
+  const double x, const double y, const double z, const double w)
+{
+  return std::atan2(
+    2.0 * (w * z + x * y),
+    1.0 - 2.0 * (y * y + z * z));
+}
+
+std::vector<std::filesystem::path> sortedJsonFiles(const std::filesystem::path & json_dir)
+{
+  std::vector<std::filesystem::path> files;
+  for (const auto & entry : std::filesystem::directory_iterator(json_dir)) {
+    if (entry.is_regular_file() && entry.path().extension() == ".json") {
+      files.push_back(entry.path());
+    }
+  }
+  std::sort(files.begin(), files.end(), [](const auto & a, const auto & b) {
+    return a.filename().string() < b.filename().string();
+  });
+  return files;
+}
+
+geometry_msgs::msg::PoseStamped poseFromJsonPoint(
+  const nlohmann::json & point,
+  const std::string & frame_id)
+{
+  const auto & odom = point.at("odom");
+  const auto & pose = odom.at("pose").at("pose");
+  const auto & position = pose.at("position");
+  const auto & orientation = pose.at("orientation");
+
+  const double x = position.at("x").get<double>();
+  const double y = position.at("y").get<double>();
+  const double yaw = point.contains("yaw_degrees") ?
+    point.at("yaw_degrees").get<double>() * M_PI / 180.0 :
+    yawFromQuaternionInternal(
+    orientation.at("x").get<double>(), orientation.at("y").get<double>(),
+    orientation.at("z").get<double>(), orientation.at("w").get<double>());
+
+  geometry_msgs::msg::PoseStamped stamped;
+  stamped.header.frame_id = frame_id;
+  stamped.pose.position.x = x;
+  stamped.pose.position.y = y;
+  stamped.pose.position.z = 0.0;
+  stamped.pose.orientation.z = std::sin(yaw * 0.5);
+  stamped.pose.orientation.w = std::cos(yaw * 0.5);
+  return stamped;
+}
+
+bool fileContainsGroup(
+  const std::string & source_file,
+  const std::string & preferred_source_file)
+{
+  if (preferred_source_file.empty()) {
+    return false;
+  }
+  return source_file == preferred_source_file;
+}
+
+}  // namespace
+
+CandidateWaypointGroups loadCandidateWaypointGroups(
+  const std::string & json_dir,
+  const std::string & frame_id)
+{
+  CandidateWaypointGroups groups;
+  const auto root = std::filesystem::path(json_dir);
+  if (json_dir.empty() || !std::filesystem::exists(root)) {
+    return groups;
+  }
+
+  for (const auto & path : sortedJsonFiles(root)) {
+    std::ifstream input(path);
+    if (!input) {
+      continue;
+    }
+    nlohmann::json data;
+    try {
+      input >> data;
+    } catch (...) {
+      continue;
+    }
+
+    const auto points = data.is_array() ? data : data.value("points", nlohmann::json::array());
+    if (!points.is_array()) {
+      continue;
+    }
+
+    for (const auto & point : points) {
+      if (!point.is_object() || !point.contains("odom") || !point["odom"].is_object()) {
+        continue;
+      }
+      CandidateWaypoint candidate;
+      candidate.source_file = path.filename().string();
+      candidate.point_name = point.value("name", path.stem().string());
+      candidate.group_name = candidate.point_name;
+      candidate.pose = poseFromJsonPoint(point, frame_id);
+      groups[candidate.group_name].push_back(std::move(candidate));
+    }
+  }
+
+  return groups;
+}
+
+void CandidateWaypointSelector::setGroups(CandidateWaypointGroups groups)
+{
+  groups_ = std::move(groups);
+}
+
+void CandidateWaypointSelector::setCostmap(const nav_msgs::msg::OccupancyGrid & costmap)
+{
+  costmap_ = costmap;
+  has_costmap_ = true;
+}
+
+std::optional<CandidateWaypoint> CandidateWaypointSelector::select(
+  const std::string & group_name,
+  const int occupied_threshold,
+  const bool treat_unknown_as_occupied,
+  const std::string & preferred_source_file) const
+{
+  const auto group_it = groups_.find(group_name);
+  if (group_it == groups_.end() || group_it->second.empty() || !has_costmap_) {
+    return std::nullopt;
+  }
+
+  std::vector<const CandidateWaypoint *> ordered;
+  ordered.reserve(group_it->second.size());
+  for (const auto & candidate : group_it->second) {
+    ordered.push_back(&candidate);
+  }
+  std::stable_sort(ordered.begin(), ordered.end(), [&](const auto * lhs, const auto * rhs) {
+    const bool lhs_preferred = fileContainsGroup(lhs->source_file, preferred_source_file);
+    const bool rhs_preferred = fileContainsGroup(rhs->source_file, preferred_source_file);
+    if (lhs_preferred != rhs_preferred) {
+      return lhs_preferred;
+    }
+    return lhs->source_file < rhs->source_file;
+  });
+
+  for (const auto * candidate : ordered) {
+    if (!isCandidateOccupied(candidate->pose, occupied_threshold, treat_unknown_as_occupied)) {
+      return *candidate;
+    }
+  }
+  return std::nullopt;
+}
+
+std::optional<RecoveryCommand> nearestFreeRecoveryCommand(
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const geometry_msgs::msg::PoseStamped & current,
+  const int occupied_threshold,
+  const bool treat_unknown_as_occupied,
+  const double search_radius_m)
+{
+  CandidateWaypointSelector selector;
+  selector.setCostmap(costmap);
+  return selector.nearestFreeRecoveryCommand(
+    current, occupied_threshold, treat_unknown_as_occupied, search_radius_m);
+}
+
+std::optional<RecoveryCommand> rearClearRecoveryCommand(
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const geometry_msgs::msg::PoseStamped & current,
+  const int occupied_threshold,
+  const bool treat_unknown_as_occupied,
+  const double rear_clear_distance_m)
+{
+  CandidateWaypointSelector selector;
+  selector.setCostmap(costmap);
+  const double yaw = 2.0 * std::atan2(current.pose.orientation.z, current.pose.orientation.w);
+  geometry_msgs::msg::PoseStamped rear = current;
+  rear.pose.position.x -= std::cos(yaw) * rear_clear_distance_m;
+  rear.pose.position.y -= std::sin(yaw) * rear_clear_distance_m;
+  if (selector.isPoseOccupied(rear, occupied_threshold, treat_unknown_as_occupied)) {
+    return std::nullopt;
+  }
+  return RecoveryCommand{-0.25, 0.0, rear_clear_distance_m / 0.25, "rear-clear-backward"};
+}
+
+std::optional<RecoveryCommand> CandidateWaypointSelector::nearestFreeRecoveryCommand(
+  const geometry_msgs::msg::PoseStamped & current,
+  const int occupied_threshold,
+  const bool treat_unknown_as_occupied,
+  const double search_radius_m) const
+{
+  if (!has_costmap_) {
+    return std::nullopt;
+  }
+
+  const double yaw = 2.0 * std::atan2(current.pose.orientation.z, current.pose.orientation.w);
+  const std::vector<double> distances{0.25, 0.5, 0.75, std::max(0.25, search_radius_m)};
+  const std::vector<double> angles{0.0, M_PI / 4.0, -M_PI / 4.0, M_PI / 2.0, -M_PI / 2.0, M_PI};
+
+  double best_score = std::numeric_limits<double>::infinity();
+  std::optional<RecoveryCommand> best_command;
+  for (const double distance : distances) {
+    for (const double angle : angles) {
+      const double sample_angle = normalizeAngle(yaw + angle);
+      geometry_msgs::msg::PoseStamped sample = current;
+      sample.pose.position.x += std::cos(sample_angle) * distance;
+      sample.pose.position.y += std::sin(sample_angle) * distance;
+      if (isCandidateOccupied(sample, occupied_threshold, treat_unknown_as_occupied)) {
+        continue;
+      }
+      const double score = distance + std::abs(angle) * 0.1;
+      if (score < best_score) {
+        best_score = score;
+        best_command = commandForRecoveryAngle(angle);
+      }
+    }
+  }
+  return best_command;
+}
+
+bool CandidateWaypointSelector::isPoseOccupied(
+  const geometry_msgs::msg::PoseStamped & pose,
+  const int occupied_threshold,
+  const bool treat_unknown_as_occupied) const
+{
+  return isCandidateOccupied(pose, occupied_threshold, treat_unknown_as_occupied);
+}
+
+bool CandidateWaypointSelector::isCandidateOccupied(
+  const geometry_msgs::msg::PoseStamped & pose,
+  const int occupied_threshold,
+  const bool treat_unknown_as_occupied) const
+{
+  const auto & costmap = costmap_;
+  const auto & origin = costmap.info.origin;
+  const double resolution = costmap.info.resolution;
+  const auto width = static_cast<int>(costmap.info.width);
+  const auto height = static_cast<int>(costmap.info.height);
+  if (resolution <= 0.0 || width <= 0 || height <= 0) {
+    return true;
+  }
+
+  const double origin_yaw = yawFromQuaternionInternal(
+    origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
+  const double dx = pose.pose.position.x - origin.position.x;
+  const double dy = pose.pose.position.y - origin.position.y;
+  const double cos_yaw = std::cos(-origin_yaw);
+  const double sin_yaw = std::sin(-origin_yaw);
+  const double local_x = cos_yaw * dx - sin_yaw * dy;
+  const double local_y = sin_yaw * dx + cos_yaw * dy;
+  const auto mx = static_cast<int>(std::floor(local_x / resolution));
+  const auto my = static_cast<int>(std::floor(local_y / resolution));
+  if (mx < 0 || my < 0 || mx >= width || my >= height) {
+    return true;
+  }
+
+  const auto index = my * width + mx;
+  if (index < 0 || index >= static_cast<int>(costmap.data.size())) {
+    return true;
+  }
+
+  const auto cost = static_cast<int>(costmap.data[static_cast<std::size_t>(index)]);
+  if (cost < 0) {
+    return treat_unknown_as_occupied;
+  }
+  return cost >= occupied_threshold;
+}
+
+std::optional<RecoveryCommand> CandidateWaypointSelector::commandForRecoveryAngle(
+  const double angle_rad) const
+{
+  const double angle = normalizeAngle(angle_rad);
+  if (std::abs(angle) < M_PI / 8.0) {
+    return RecoveryCommand{0.25, 0.0, 0.6, "forward"};
+  }
+  if (std::abs(std::abs(angle) - M_PI) < M_PI / 8.0) {
+    return RecoveryCommand{-0.25, 0.0, 0.6, "backward"};
+  }
+  if (angle > 0.0 && angle < M_PI / 2.0) {
+    return RecoveryCommand{0.25, 0.7, 0.6, "forward-left"};
+  }
+  if (angle < 0.0 && angle > -M_PI / 2.0) {
+    return RecoveryCommand{0.25, -0.7, 0.6, "forward-right"};
+  }
+  if (angle >= M_PI / 2.0) {
+    return RecoveryCommand{0.0, 0.7, 0.4, "left"};
+  }
+  return RecoveryCommand{0.0, -0.7, 0.4, "right"};
+}
+
+}  // namespace racing_control

src/racing_control/src/racing_control.cpp

@@ -23,10 +23,13 @@
 #include "nav2_msgs/action/compute_path_through_poses.hpp"
 #include "nav2_msgs/action/follow_path.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
+#include "nav2_msgs/srv/clear_entire_costmap.hpp"
 #include "nav_msgs/msg/odometry.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
+#include "racing_control/candidate_waypoint_selector.hpp"
 #include "sensor_msgs/msg/compressed_image.hpp"
 #include "std_msgs/msg/int32.hpp"
 #include "std_msgs/msg/string.hpp"
@@ -129,6 +132,7 @@ public:
   using NavigateToPose = nav2_msgs::action::NavigateToPose;
   using ComputePathThroughPoses = nav2_msgs::action::ComputePathThroughPoses;
   using FollowPath = nav2_msgs::action::FollowPath;
+  using ClearEntireCostmap = nav2_msgs::srv::ClearEntireCostmap;
   using NavigateGoalHandle = rclcpp_action::ClientGoalHandle<NavigateToPose>;
   using ComputeGoalHandle = rclcpp_action::ClientGoalHandle<ComputePathThroughPoses>;
   using FollowGoalHandle = rclcpp_action::ClientGoalHandle<FollowPath>;
@@ -143,6 +147,11 @@ public:
     recovery_cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>(
       recovery_cmd_vel_topic_, 10);
     guard_path_pub_ = create_publisher<nav_msgs::msg::Path>(guard_input_topic_, 1);
+    global_costmap_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
+      global_costmap_topic_, 10,
+      [this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
+        onGlobalCostmap(std::move(msg));
+      });
     if (enable_vlm_image_relay_) {
       vlm_image_pub_ =
         create_publisher<sensor_msgs::msg::CompressedImage>(vlm_image_output_topic_, 1);
@@ -167,6 +176,10 @@ public:
     compute_path_client_ =
       rclcpp_action::create_client<ComputePathThroughPoses>(this, compute_path_action_);
     follow_path_client_ = rclcpp_action::create_client<FollowPath>(this, follow_path_action_);
+    clear_global_costmap_client_ =
+      create_client<ClearEntireCostmap>(global_costmap_clear_service_);
+    clear_local_costmap_client_ =
+      create_client<ClearEntireCostmap>(local_costmap_clear_service_);
 
     tick_timer_ = create_wall_timer(200ms, [this]() {tick();});
     startKeyboardThread();
@@ -198,6 +211,12 @@ private:
     odom_topic_ = declare_parameter<std::string>("odom_topic", "/odom_combined");
     recovery_cmd_vel_topic_ =
       declare_parameter<std::string>("recovery_cmd_vel_topic", "/cmd_vel");
+    global_costmap_topic_ = declare_parameter<std::string>(
+      "global_costmap_topic", "/global_costmap/costmap");
+    global_costmap_clear_service_ = declare_parameter<std::string>(
+      "global_costmap_clear_service", "global_costmap/clear_entirely_global_costmap");
+    local_costmap_clear_service_ = declare_parameter<std::string>(
+      "local_costmap_clear_service", "local_costmap/clear_entirely_local_costmap");
     navigate_action_ = declare_parameter<std::string>("navigate_action", "/navigate_to_pose");
     compute_path_action_ =
       declare_parameter<std::string>("compute_path_action", "/compute_path_through_poses");
@@ -218,6 +237,8 @@ private:
     enable_vlm_image_relay_ = declare_parameter<bool>("enable_vlm_image_relay", false);
     enable_dynamic_replanning_ = declare_parameter<bool>("enable_dynamic_replanning", true);
     enable_recovery_ = declare_parameter<bool>("enable_recovery", true);
+    enable_candidate_waypoint_selection_ =
+      declare_parameter<bool>("enable_candidate_waypoint_selection", true);
     vlm_image_input_topic_ = declare_parameter<std::string>("vlm_image_input_topic", "/image");
     vlm_image_output_topic_ =
       declare_parameter<std::string>("vlm_image_output_topic", "/vlm_image");
@@ -236,12 +257,22 @@ private:
     recovery_backup_distance_ = declare_parameter<double>("recovery_backup_distance", 0.04);
     recovery_backup_timeout_sec_ =
       declare_parameter<double>("recovery_backup_timeout_sec", 0.2);
+    planning_failure_backup_speed_ =
+      declare_parameter<double>("planning_failure_backup_speed", -0.1);
+    planning_failure_backup_distance_ =
+      declare_parameter<double>("planning_failure_backup_distance", 0.03);
+    planning_failure_backup_timeout_sec_ =
+      declare_parameter<double>("planning_failure_backup_timeout_sec", 0.3);
+    recovery_clear_wait_sec_ = declare_parameter<double>("recovery_clear_wait_sec", 1.0);
     pass_through_vlm_trigger_radius_ =
       declare_parameter<double>("pass_through_vlm_trigger_radius", 0.35);
     circle_goal_tolerance_ = declare_parameter<double>("circle_goal_tolerance", 0.50);
     dynamic_replan_max_consecutive_failures_ =
       declare_parameter<int>("dynamic_replan_max_consecutive_failures", 3);
     max_recovery_attempts_ = declare_parameter<int>("max_recovery_attempts", 2);
+    recovery_search_radius_m_ = declare_parameter<double>("recovery_search_radius_m", 1.0);
+    recovery_rear_clear_distance_m_ =
+      declare_parameter<double>("recovery_rear_clear_distance_m", 0.5);
 
     const auto vlm_capture_mode =
       declare_parameter<std::string>("vlm_capture_mode", "stop");
@@ -291,6 +322,20 @@ private:
       singlePoseFromParameter(
       "counterclockwise_home_pose", {0.536986899408154, 0.1772662932069835,
         -1.652041913138445});
+
+    candidate_waypoint_json_dir_ = declare_parameter<std::string>(
+      "candidate_waypoint_json_dir", "/home/sunrise/yiliao_ws/src/racing_control/config/waypoints");
+    qr_candidate_group_name_ = declare_parameter<std::string>("qr_candidate_group_name", "qr");
+    entry_candidate_group_name_ =
+      declare_parameter<std::string>("entry_candidate_group_name", "entry");
+    vlm_candidate_group_name_ =
+      declare_parameter<std::string>("vlm_candidate_group_name", "goal_005");
+    clockwise_candidate_source_file_ = declare_parameter<std::string>(
+      "clockwise_candidate_source_file", "main_1.json");
+    counterclockwise_candidate_source_file_ = declare_parameter<std::string>(
+      "counterclockwise_candidate_source_file", "main_2.json");
+    candidate_selector_.setGroups(
+      loadCandidateWaypointGroups(candidate_waypoint_json_dir_, frame_id_));
   }
 
   geometry_msgs::msg::PoseStamped singlePoseFromParameter(
@@ -470,6 +515,11 @@ private:
     if (recovery_timer_) {
       recovery_timer_->cancel();
     }
+    if (recovery_clear_wait_timer_) {
+      recovery_clear_wait_timer_->cancel();
+    }
+    recovery_after_clear_callback_ = nullptr;
+    recovery_costmap_clear_pending_ = 0;
     cancelActiveFollowGoal();
     publishRecoveryVelocity(0.0);
     publishSign(sign_qr_disable_);
@@ -540,9 +590,10 @@ private:
     latest_qr_result_.clear();
     selected_direction_ = RouteDirection::Unknown;
     qr_detection_disabled_ = false;
-    recovery_attempts_ = 0;
+    resetRouteRecoveryState();
     active_segment_ = RouteSegment::ToQr;
-    active_segment_waypoints_ = {qr_pose_};
+    active_segment_waypoints_ = {
+      selectCandidatePose(qr_candidate_group_name_, "", qr_pose_).value_or(qr_pose_)};
     active_segment_next_waypoint_index_ = 0;
     publishSign(sign_profile_normal_);
     publishSign(sign_qr_enable_);
@@ -621,7 +672,7 @@ private:
     const auto vlm_index = vlmWaypointIndex(route);
     latest_vlm_result_.clear();
     vlm_capture_triggered_ = false;
-    recovery_attempts_ = 0;
+    resetRouteRecoveryState();
     const auto split_at_vlm = split_qr_to_vlm_segment_ &&
       vlm_capture_mode_ == VlmCaptureMode::Stop;
 
@@ -631,8 +682,17 @@ private:
       active_segment_waypoints_.push_back(post_qr_pose_);
     }
 
+    auto route_waypoints = route.waypoints;
+    if (vlm_index < route_waypoints.size()) {
+      route_waypoints[vlm_index] = selectCandidatePose(
+        vlm_candidate_group_name_, preferredCandidateSourceFile(),
+        route_waypoints[vlm_index]).value_or(route_waypoints[vlm_index]);
+    }
+    const auto selected_entry_pose = selectCandidatePose(
+      entry_candidate_group_name_, preferredCandidateSourceFile(),
+      entry_pose_).value_or(entry_pose_);
     const auto qr_segment = routeWaypointsAfterQr(
-      entry_pose_, route.waypoints, vlm_index, split_at_vlm);
+      selected_entry_pose, route_waypoints, vlm_index, split_at_vlm);
     active_segment_waypoints_.insert(
       active_segment_waypoints_.end(), qr_segment.begin(), qr_segment.end());
     if (!split_at_vlm) {
@@ -646,7 +706,7 @@ private:
   {
     const auto & route = selectedRoute();
     const auto vlm_index = vlmWaypointIndex(route);
-    recovery_attempts_ = 0;
+    resetRouteRecoveryState();
     active_segment_ = RouteSegment::AfterVlm;
     active_segment_waypoints_ = remainingWaypoints(route.waypoints, vlm_index + 1);
     active_segment_waypoints_.push_back(route.home_pose);
@@ -664,7 +724,7 @@ private:
     startStage(Stage::ComputeCirclePath, path_planning_timeout_sec_);
 
     if (!compute_path_client_->wait_for_action_server(2s)) {
-      handleRouteExecutionFailure("ComputePathThroughPoses action server is not available");
+      onRoutePlanningFailed("ComputePathThroughPoses action server is not available");
       return;
     }
 
@@ -676,8 +736,12 @@ private:
     auto options = rclcpp_action::Client<ComputePathThroughPoses>::SendGoalOptions();
     options.goal_response_callback =
       [this](ComputeGoalHandle::SharedPtr goal_handle) {
+        if (stage_ != Stage::ComputeCirclePath) {
+          RCLCPP_DEBUG(get_logger(), "stale route planning goal response ignored");
+          return;
+        }
         if (!goal_handle) {
-          handleRouteExecutionFailure("circle path planning goal was rejected");
+          onRoutePlanningFailed("circle path planning goal was rejected");
         }
       };
     options.result_callback =
@@ -686,10 +750,10 @@ private:
           return;
         }
         if (result.code != rclcpp_action::ResultCode::SUCCEEDED ||
-          result.result->path.poses.empty())
+          !result.result || result.result->path.poses.empty())
         {
           finishStage("planning failed");
-          handleRouteExecutionFailure("circle path planning failed");
+          onRoutePlanningFailed("circle path planning failed");
           return;
         }
         active_path_ = result.result->path;
@@ -725,7 +789,7 @@ private:
       active_path_, [this](const bool ok) {
         finishStage(ok ? "FollowPath succeeded" : "FollowPath failed");
         if (!ok) {
-          handleRouteExecutionFailure("route segment FollowPath failed");
+          onRouteFollowFailed("route segment FollowPath failed");
           return;
         }
         if (active_segment_ == RouteSegment::ToQr) {
@@ -789,6 +853,44 @@ private:
       vlm_image_input_topic_.c_str(), vlm_image_output_topic_.c_str());
   }
 
+  void onGlobalCostmap(nav_msgs::msg::OccupancyGrid::SharedPtr msg)
+  {
+    std::lock_guard<std::mutex> lock(costmap_mutex_);
+    latest_global_costmap_ = std::move(msg);
+    candidate_selector_.setCostmap(*latest_global_costmap_);
+  }
+
+  std::optional<geometry_msgs::msg::PoseStamped> selectCandidatePose(
+    const std::string & group_name,
+    const std::string & preferred_source_file,
+    const geometry_msgs::msg::PoseStamped & fallback_pose)
+  {
+    if (!enable_candidate_waypoint_selection_) {
+      return fallback_pose;
+    }
+    {
+      std::lock_guard<std::mutex> lock(costmap_mutex_);
+      if (!latest_global_costmap_) {
+        RCLCPP_WARN(
+          get_logger(), "candidate selection fallback for %s: no global costmap yet",
+          group_name.c_str());
+        return fallback_pose;
+      }
+    }
+    const auto selected = candidate_selector_.select(
+      group_name, 50, false, preferred_source_file);
+    if (!selected) {
+      RCLCPP_WARN(
+        get_logger(), "candidate selection fallback for %s: no usable candidate",
+        group_name.c_str());
+      return fallback_pose;
+    }
+    RCLCPP_INFO(
+      get_logger(), "candidate selected for %s from %s -> %s",
+      group_name.c_str(), selected->source_file.c_str(), selected->point_name.c_str());
+    return selected->pose;
+  }
+
   void maybeTriggerPassThroughVlmCapture()
   {
     if (active_segment_ != RouteSegment::FullRoute || vlm_capture_triggered_) {
@@ -914,46 +1016,134 @@ private:
       get_logger(), "%s | consecutive dynamic replan failures=%d",
       reason.c_str(), dynamic_replan_consecutive_failures_);
     if (dynamic_replan_consecutive_failures_ >= dynamic_replan_max_consecutive_failures_) {
-      handleRouteExecutionFailure(reason);
+      onRoutePlanningFailed(reason);
     }
   }
 
-  void handleRouteExecutionFailure(const std::string & reason)
+  void resetRouteRecoveryState()
   {
-    startRecovery(
+    route_recovery_phase_ = RouteRecoveryPhase::Initial;
-      reason,
+    planning_backup_attempted_ = false;
-      [this]() {
+    final_recovery_attempts_ = 0;
-        dynamic_replan_consecutive_failures_ = 0;
-        const auto current = currentPoseFromOdom();
-        if (current) {
-          active_segment_waypoints_ = remainingWaypointsAfterProgress(
-            active_segment_waypoints_, *current, active_segment_next_waypoint_index_,
-            circle_goal_tolerance_);
-        } else {
-          active_segment_waypoints_ = remainingWaypoints(
-            active_segment_waypoints_, active_segment_next_waypoint_index_);
-        }
-        active_segment_next_waypoint_index_ = 0;
-        runRouteSegmentPlanning("after recovery");
-      });
   }
 
-  void startRecovery(const std::string & reason, std::function<void()> on_recovered)
+  void prepareRouteRecoveryReplan()
   {
-    if (!enable_recovery_ || recovery_attempts_ >= max_recovery_attempts_) {
+    dynamic_replan_consecutive_failures_ = 0;
+    const auto current = currentPoseFromOdom();
+    if (current) {
+      active_segment_waypoints_ = remainingWaypointsAfterProgress(
+        active_segment_waypoints_, *current, active_segment_next_waypoint_index_,
+        circle_goal_tolerance_);
+    } else {
+      active_segment_waypoints_ = remainingWaypoints(
+        active_segment_waypoints_, active_segment_next_waypoint_index_);
+    }
+    active_segment_next_waypoint_index_ = 0;
+  }
+
+  void onRoutePlanningFailed(const std::string & reason)
+  {
+    const auto action = routeRecoveryAction(
+      route_recovery_phase_, RouteFailureKind::Planning,
+      final_recovery_attempts_, max_recovery_attempts_);
+    if (action == RouteRecoveryAction::Fail) {
+      failRace(reason);
+      return;
+    }
+
+    if (action == RouteRecoveryAction::PlanningBackup) {
+      planning_backup_attempted_ = true;
+      route_recovery_phase_ = RouteRecoveryPhase::AfterPlanningBackup;
+      RCLCPP_WARN(
+        get_logger(), "%s; planning failed, backing up at %.2f m/s for %.2f s before one replan",
+        reason.c_str(), planning_failure_backup_speed_, planning_failure_backup_timeout_sec_);
+      startBackupRecovery(
+        reason, planning_failure_backup_speed_, planning_failure_backup_distance_,
+        planning_failure_backup_timeout_sec_, [this]() {
+          prepareRouteRecoveryReplan();
+          runRouteSegmentPlanning("after planning-failure backup");
+        });
+      return;
+    }
+
+    ++final_recovery_attempts_;
+    route_recovery_phase_ = RouteRecoveryPhase::AfterFinalBackupReplan;
+    RCLCPP_WARN(
+      get_logger(), "%s; final backup/replan attempt %d/%d",
+      reason.c_str(), final_recovery_attempts_, max_recovery_attempts_);
+    startBackupRecovery(
+      reason, recovery_backup_speed_, recovery_backup_distance_, recovery_backup_timeout_sec_,
+      [this]() {
+        prepareRouteRecoveryReplan();
+        runRouteSegmentPlanning("after final backup");
+      });
+  }
+
+  void onRouteFollowFailed(const std::string & reason)
+  {
+    const auto action = routeRecoveryAction(
+      route_recovery_phase_, RouteFailureKind::Following,
+      final_recovery_attempts_, max_recovery_attempts_);
+    if (action == RouteRecoveryAction::Fail) {
+      failRace(reason);
+      return;
+    }
+
+    if (action == RouteRecoveryAction::ClearLocalAndReplan) {
+      route_recovery_phase_ = RouteRecoveryPhase::AfterLocalClearReplan;
+      RCLCPP_WARN(
+        get_logger(), "%s; clearing local costmap, waiting for update, then replanning once",
+        reason.c_str());
+      clearLocalCostmapBeforeRecoveryReplan(
+        [this]() {
+          prepareRouteRecoveryReplan();
+          runRouteSegmentPlanning("after local costmap clear");
+        });
+      return;
+    }
+
+    ++final_recovery_attempts_;
+    route_recovery_phase_ = RouteRecoveryPhase::AfterFinalBackupReplan;
+    RCLCPP_WARN(
+      get_logger(), "%s; final backup/replan attempt %d/%d",
+      reason.c_str(), final_recovery_attempts_, max_recovery_attempts_);
+    startBackupRecovery(
+      reason, recovery_backup_speed_, recovery_backup_distance_, recovery_backup_timeout_sec_,
+      [this]() {
+        prepareRouteRecoveryReplan();
+        runRouteSegmentPlanning("after final backup");
+      });
+  }
+
+  void startBackupRecovery(
+    const std::string & reason,
+    const double speed,
+    const double distance,
+    const double timeout_sec,
+    std::function<void()> on_recovered)
+  {
+    if (!enable_recovery_) {
       failRace(reason);
       return;
     }
-    ++recovery_attempts_;
     recovery_in_progress_ = true;
     recovery_done_callback_ = std::move(on_recovered);
     recovery_start_time_ = now();
     recovery_start_pose_ = currentPoseFromOdom();
+    recovery_active_speed_ = speed;
+    recovery_active_distance_ = distance;
+    recovery_active_timeout_sec_ = timeout_sec;
+    if (recovery_clear_wait_timer_) {
+      recovery_clear_wait_timer_->cancel();
+    }
+    recovery_after_clear_callback_ = nullptr;
+    recovery_costmap_clear_pending_ = 0;
     cancelActiveFollowGoal();
 
     RCLCPP_WARN(
-      get_logger(), "%s; recovery backup attempt %d/%d",
+      get_logger(), "%s; backup recovery speed=%.2f m/s distance=%.2f m timeout=%.2f s",
-      reason.c_str(), recovery_attempts_, max_recovery_attempts_);
+      reason.c_str(), speed, distance, timeout_sec);
 
     if (recovery_timer_) {
       recovery_timer_->cancel();
@@ -966,9 +1156,9 @@ private:
     const auto elapsed = (now() - recovery_start_time_).seconds();
     const auto backup_distance = recoveryBackupDistance();
     if (!recoveryBackupComplete(
-        backup_distance, recovery_backup_distance_, elapsed, recovery_backup_timeout_sec_))
+        backup_distance, recovery_active_distance_, elapsed, recovery_active_timeout_sec_))
     {
-      publishRecoveryVelocity(recovery_backup_speed_);
+      publishRecoveryVelocity(recovery_active_speed_);
       return;
     }
 
@@ -984,6 +1174,79 @@ private:
     }
   }
 
+  void clearLocalCostmapBeforeRecoveryReplan(std::function<void()> on_cleared)
+  {
+    recovery_after_clear_callback_ = std::move(on_cleared);
+    recovery_costmap_clear_pending_ = 0;
+    const auto requested = requestEntireCostmapClear(
+      clear_local_costmap_client_, local_costmap_clear_service_);
+    if (!requested) {
+      startRecoveryReplanAfterClearWait();
+    }
+  }
+
+  bool requestEntireCostmapClear(
+    const rclcpp::Client<ClearEntireCostmap>::SharedPtr & client,
+    const std::string & service_name)
+  {
+    if (!client) {
+      return false;
+    }
+    if (!client->wait_for_service(200ms)) {
+      RCLCPP_WARN(
+        get_logger(), "costmap clear service not available: %s", service_name.c_str());
+      return false;
+    }
+
+    ++recovery_costmap_clear_pending_;
+    auto request = std::make_shared<ClearEntireCostmap::Request>();
+    client->async_send_request(
+      request,
+      [this, service_name](rclcpp::Client<ClearEntireCostmap>::SharedFuture) {
+        if (recovery_costmap_clear_pending_ > 0) {
+          --recovery_costmap_clear_pending_;
+        }
+        RCLCPP_INFO(get_logger(), "cleared costmap via %s", service_name.c_str());
+        if (recovery_costmap_clear_pending_ == 0) {
+          startRecoveryReplanAfterClearWait();
+        }
+      });
+    return true;
+  }
+
+  void startRecoveryReplanAfterClearWait()
+  {
+    if (!recovery_after_clear_callback_) {
+      return;
+    }
+
+    if (recovery_clear_wait_timer_) {
+      recovery_clear_wait_timer_->cancel();
+    }
+
+    if (recovery_clear_wait_sec_ <= 0.0) {
+      auto callback = std::move(recovery_after_clear_callback_);
+      recovery_after_clear_callback_ = nullptr;
+      if (callback) {
+        callback();
+      }
+      return;
+    }
+
+    recovery_clear_wait_timer_ = create_wall_timer(
+      std::chrono::duration<double>(recovery_clear_wait_sec_),
+      [this]() {
+        if (recovery_clear_wait_timer_) {
+          recovery_clear_wait_timer_->cancel();
+        }
+        auto callback = std::move(recovery_after_clear_callback_);
+        recovery_after_clear_callback_ = nullptr;
+        if (callback) {
+          callback();
+        }
+      });
+  }
+
   double recoveryBackupDistance() const
   {
     if (!recovery_start_pose_) {
@@ -1106,7 +1369,7 @@ private:
   void sendFollowPath(const nav_msgs::msg::Path & path, std::function<void(bool)> on_done)
   {
     if (!follow_path_client_->wait_for_action_server(2s)) {
-      failRace("FollowPath action server is not available");
+      onRouteFollowFailed("FollowPath action server is not available");
       return;
     }
 
@@ -1211,6 +1474,17 @@ private:
     throw std::runtime_error("route direction is not selected");
   }
 
+  std::string preferredCandidateSourceFile() const
+  {
+    if (selected_direction_ == RouteDirection::Clockwise) {
+      return clockwise_candidate_source_file_;
+    }
+    if (selected_direction_ == RouteDirection::Counterclockwise) {
+      return counterclockwise_candidate_source_file_;
+    }
+    return "";
+  }
+
   bool routeSegmentReached() const
   {
     std::lock_guard<std::mutex> lock(odom_mutex_);
@@ -1287,6 +1561,7 @@ private:
   bool enable_vlm_image_relay_{false};
   bool enable_dynamic_replanning_{true};
   bool enable_recovery_{true};
+  bool enable_candidate_waypoint_selection_{true};
   bool qr_detection_disabled_{false};
   bool dynamic_replan_in_flight_{false};
   bool recovery_in_progress_{false};
@@ -1302,6 +1577,9 @@ private:
   std::string vlm_result_topic_;
   std::string odom_topic_;
   std::string recovery_cmd_vel_topic_;
+  std::string global_costmap_topic_;
+  std::string global_costmap_clear_service_;
+  std::string local_costmap_clear_service_;
   std::string vlm_image_input_topic_;
   std::string vlm_image_output_topic_;
   std::string navigate_action_;
@@ -1311,6 +1589,12 @@ private:
   std::string planner_id_;
   std::string controller_id_;
   std::string goal_checker_id_;
+  std::string candidate_waypoint_json_dir_;
+  std::string qr_candidate_group_name_;
+  std::string entry_candidate_group_name_;
+  std::string vlm_candidate_group_name_;
+  std::string clockwise_candidate_source_file_;
+  std::string counterclockwise_candidate_source_file_;
 
   double navigation_timeout_sec_{120.0};
   double path_planning_timeout_sec_{30.0};
@@ -1324,6 +1608,15 @@ private:
   double recovery_backup_speed_{-0.2};
   double recovery_backup_distance_{0.04};
   double recovery_backup_timeout_sec_{0.2};
+  double planning_failure_backup_speed_{-0.1};
+  double planning_failure_backup_distance_{0.03};
+  double planning_failure_backup_timeout_sec_{0.3};
+  double recovery_clear_wait_sec_{1.0};
+  double recovery_active_speed_{0.0};
+  double recovery_active_distance_{0.0};
+  double recovery_active_timeout_sec_{0.0};
+  double recovery_search_radius_m_{1.0};
+  double recovery_rear_clear_distance_m_{0.5};
   double pass_through_vlm_trigger_radius_{0.35};
   double circle_goal_tolerance_{0.30};
 
@@ -1336,7 +1629,7 @@ private:
   int dynamic_replan_max_consecutive_failures_{3};
   int dynamic_replan_consecutive_failures_{0};
   int max_recovery_attempts_{2};
-  int recovery_attempts_{0};
+  int final_recovery_attempts_{0};
 
   geometry_msgs::msg::PoseStamped qr_pose_;
   geometry_msgs::msg::PoseStamped post_qr_pose_;
@@ -1346,20 +1639,26 @@ private:
   RouteDirection selected_direction_{RouteDirection::Unknown};
   VlmCaptureMode vlm_capture_mode_{VlmCaptureMode::Stop};
   RouteSegment active_segment_{RouteSegment::None};
+  RouteRecoveryPhase route_recovery_phase_{RouteRecoveryPhase::Initial};
+  bool planning_backup_attempted_{false};
   std::vector<geometry_msgs::msg::PoseStamped> active_segment_waypoints_;
   std::size_t active_segment_next_waypoint_index_{0};
   nav_msgs::msg::Path active_path_;
   bool vlm_capture_triggered_{false};
   std::optional<geometry_msgs::msg::PoseStamped> recovery_start_pose_;
   std::function<void()> recovery_done_callback_;
+  std::function<void()> recovery_after_clear_callback_;
+  std::size_t recovery_costmap_clear_pending_{0};
 
   std::string latest_qr_result_;
   std::string latest_vlm_result_;
   rclcpp::Time qr_result_time_{0, 0, RCL_ROS_TIME};
   rclcpp::Time vlm_result_time_{0, 0, RCL_ROS_TIME};
   nav_msgs::msg::Odometry::SharedPtr latest_odom_;
+  nav_msgs::msg::OccupancyGrid::SharedPtr latest_global_costmap_;
   sensor_msgs::msg::CompressedImage::SharedPtr latest_image_;
   mutable std::mutex odom_mutex_;
+  mutable std::mutex costmap_mutex_;
   mutable std::mutex image_mutex_;
 
   rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_pub_;
@@ -1369,14 +1668,19 @@ private:
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr qr_sub_;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr vlm_sub_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr global_costmap_sub_;
   rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr image_sub_;
   rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_client_;
   rclcpp_action::Client<ComputePathThroughPoses>::SharedPtr compute_path_client_;
   rclcpp_action::Client<FollowPath>::SharedPtr follow_path_client_;
+  rclcpp::Client<ClearEntireCostmap>::SharedPtr clear_global_costmap_client_;
+  rclcpp::Client<ClearEntireCostmap>::SharedPtr clear_local_costmap_client_;
   FollowGoalHandle::SharedPtr active_follow_goal_handle_;
   rclcpp::TimerBase::SharedPtr startup_qr_enable_timer_;
   rclcpp::TimerBase::SharedPtr recovery_timer_;
+  rclcpp::TimerBase::SharedPtr recovery_clear_wait_timer_;
   rclcpp::TimerBase::SharedPtr tick_timer_;
+  CandidateWaypointSelector candidate_selector_;
 
   int startup_qr_enable_publish_count_{0};
   std::uint64_t follow_goal_generation_{0};

src/racing_control/src/racing_control.cpp.unc-backup.md5~

@@ -0,0 +1 @@
+32e8eedad71eea774e96a762a1cfd3ae  racing_control.cpp

src/racing_control/test/test_racing_control_helpers.cpp

@@ -1,7 +1,11 @@
 #include <cmath>
+#include <filesystem>
+#include <fstream>
 #include <stdexcept>
 
 #include "gtest/gtest.h"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "racing_control/candidate_waypoint_selector.hpp"
 #include "racing_control/racing_control.hpp"
 
 namespace
@@ -216,6 +220,57 @@ TEST(RacingControlHelpers, RecoveryBackupStopsByDistanceOrTimeout)
   EXPECT_FALSE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.1, 0.2));
 }
 
+TEST(RacingControlHelpers, InitialPlanningFailureUsesOnePlanningBackup)
+{
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::Initial,
+      racing_control::RouteFailureKind::Planning, 0, 2),
+    racing_control::RouteRecoveryAction::PlanningBackup);
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::AfterPlanningBackup,
+      racing_control::RouteFailureKind::Planning, 0, 2),
+    racing_control::RouteRecoveryAction::Fail);
+}
+
+TEST(RacingControlHelpers, FollowFailureClearsOnlyLocalCostmapBeforeReplanning)
+{
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::Initial,
+      racing_control::RouteFailureKind::Following, 0, 2),
+    racing_control::RouteRecoveryAction::ClearLocalAndReplan);
+}
+
+TEST(RacingControlHelpers, FailedLocalClearReplanUsesFinalBackup)
+{
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::AfterLocalClearReplan,
+      racing_control::RouteFailureKind::Planning, 0, 2),
+    racing_control::RouteRecoveryAction::FinalBackupReplan);
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::AfterLocalClearReplan,
+      racing_control::RouteFailureKind::Following, 0, 2),
+    racing_control::RouteRecoveryAction::FinalBackupReplan);
+}
+
+TEST(RacingControlHelpers, FinalRecoveryFailsAfterConfiguredAttemptLimit)
+{
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::AfterFinalBackupReplan,
+      racing_control::RouteFailureKind::Following, 1, 2),
+    racing_control::RouteRecoveryAction::FinalBackupReplan);
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::AfterFinalBackupReplan,
+      racing_control::RouteFailureKind::Planning, 2, 2),
+    racing_control::RouteRecoveryAction::Fail);
+}
+
 TEST(RacingControlHelpers, ParsesVlmCaptureMode)
 {
   EXPECT_EQ(
@@ -228,3 +283,106 @@ TEST(RacingControlHelpers, ParsesVlmCaptureMode)
     racing_control::vlmCaptureModeFromString("unknown"),
     racing_control::VlmCaptureMode::Unknown);
 }
+
+TEST(CandidateWaypointSelector, LoadsNamedCandidatesFromSavedPointJson)
+{
+  const auto dir = std::filesystem::temp_directory_path() / "racing_control_candidates_test";
+  std::filesystem::remove_all(dir);
+  std::filesystem::create_directories(dir);
+  {
+    std::ofstream json(dir / "main.json");
+    json <<
+      R"json({
+      "points": [
+        {
+          "name": "qr",
+          "yaw_degrees": 90.0,
+          "odom": {
+            "pose": {
+              "pose": {
+                "position": {"x": 1.0, "y": 2.0, "z": 0.0},
+                "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
+              }
+            }
+          }
+        },
+        {
+          "name": "entry",
+          "yaw_degrees": 0.0,
+          "odom": {
+            "pose": {
+              "pose": {
+                "position": {"x": 3.0, "y": 4.0, "z": 0.0},
+                "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
+              }
+            }
+          }
+        }
+      ]
+    })json";
+  }
+
+  const auto groups = racing_control::loadCandidateWaypointGroups(dir.string(), "odom");
+
+  ASSERT_EQ(groups.size(), 2U);
+  ASSERT_EQ(groups.at("qr").size(), 1U);
+  EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.x, 1.0);
+  EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.y, 2.0);
+  EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.z, std::sin(M_PI_4), kTolerance);
+  EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.w, std::cos(M_PI_4), kTolerance);
+  EXPECT_EQ(groups.at("entry")[0].source_file, "main.json");
+}
+
+TEST(CandidateWaypointSelector, SkipsOccupiedCandidates)
+{
+  nav_msgs::msg::OccupancyGrid costmap;
+  costmap.info.resolution = 1.0;
+  costmap.info.width = 5;
+  costmap.info.height = 5;
+  costmap.info.origin.position.x = 0.0;
+  costmap.info.origin.position.y = 0.0;
+  costmap.info.origin.orientation.w = 1.0;
+  costmap.data.assign(25, 0);
+  costmap.data[1 * 5 + 1] = 80;
+
+  racing_control::CandidateWaypointSelector selector;
+  selector.setGroups(
+  {
+    {"qr", {
+        {"qr", "blocked.json", "qr_blocked", racing_control::poseFromXYYaw(1.2, 1.2, 0.0, "odom")},
+        {"qr", "free.json", "qr_free", racing_control::poseFromXYYaw(3.2, 1.2, 0.0, "odom")},
+      }},
+  });
+  selector.setCostmap(costmap);
+
+  const auto selected = selector.select("qr", 50, false);
+
+  ASSERT_TRUE(selected.has_value());
+  EXPECT_EQ(selected->source_file, "free.json");
+  EXPECT_EQ(selected->point_name, "qr_free");
+}
+
+TEST(CandidateWaypointSelector, FindsNearestFreeRecoveryCommand)
+{
+  nav_msgs::msg::OccupancyGrid costmap;
+  costmap.info.resolution = 0.25;
+  costmap.info.width = 9;
+  costmap.info.height = 9;
+  costmap.info.origin.position.x = -1.0;
+  costmap.info.origin.position.y = -1.0;
+  costmap.info.origin.orientation.w = 1.0;
+  costmap.data.assign(81, 100);
+  const auto set_free = [&](const int mx, const int my) {
+      costmap.data[my * static_cast<int>(costmap.info.width) + mx] = 0;
+    };
+  set_free(6, 4);
+  set_free(7, 4);
+
+  const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom");
+  const auto command = racing_control::nearestFreeRecoveryCommand(
+    costmap, current, 50, false, 1.0);
+
+  ASSERT_TRUE(command.has_value());
+  EXPECT_GT(command->linear_x, 0.0);
+  EXPECT_DOUBLE_EQ(command->angular_z, 0.0);
+}

src/racing_control/test/test_racing_control_helpers.cpp.unc-backup.md5~

@@ -0,0 +1 @@
+0af1c0d18ed8a7db4d7f58cb8d1f948a  test_racing_control_helpers.cpp

src/racing_control/test/test_racing_control_helpers.cpp.unc-backup~

@@ -0,0 +1,386 @@
+#include <cmath>
+#include <filesystem>
+#include <fstream>
+#include <stdexcept>
+
+#include "gtest/gtest.h"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "racing_control/candidate_waypoint_selector.hpp"
+#include "racing_control/racing_control.hpp"
+
+namespace
+{
+
+constexpr double kTolerance = 1e-6;
+
+}  // namespace
+
+TEST(RacingControlHelpers, ConvertsFlatTriplesToStampedPoses)
+{
+  const auto poses = racing_control::posesFromFlatDoubles(
+    {1.0, 2.0, M_PI_2, -0.5, 0.25, -M_PI},
+    "odom");
+
+  ASSERT_EQ(poses.size(), 2U);
+  EXPECT_EQ(poses[0].header.frame_id, "odom");
+  EXPECT_DOUBLE_EQ(poses[0].pose.position.x, 1.0);
+  EXPECT_DOUBLE_EQ(poses[0].pose.position.y, 2.0);
+  EXPECT_NEAR(poses[0].pose.orientation.z, std::sin(M_PI_4), kTolerance);
+  EXPECT_NEAR(poses[0].pose.orientation.w, std::cos(M_PI_4), kTolerance);
+  EXPECT_DOUBLE_EQ(poses[1].pose.position.x, -0.5);
+  EXPECT_DOUBLE_EQ(poses[1].pose.position.y, 0.25);
+}
+
+TEST(RacingControlHelpers, RejectsIncompletePoseTriples)
+{
+  EXPECT_THROW(
+    racing_control::posesFromFlatDoubles({1.0, 2.0, 0.0, 3.0}, "odom"),
+    std::invalid_argument);
+}
+
+TEST(RacingControlHelpers, ParsesQrDirectionFromText)
+{
+  EXPECT_EQ(
+    racing_control::directionFromQrResult("7 顺时针"),
+    racing_control::RouteDirection::Clockwise);
+  EXPECT_EQ(
+    racing_control::directionFromQrResult("8 逆时针"),
+    racing_control::RouteDirection::Counterclockwise);
+  EXPECT_EQ(
+    racing_control::directionFromQrResult("5"),
+    racing_control::RouteDirection::Clockwise);
+  EXPECT_EQ(
+    racing_control::directionFromQrResult("6"),
+    racing_control::RouteDirection::Counterclockwise);
+  EXPECT_EQ(
+    racing_control::directionFromQrResult("未识别"),
+    racing_control::RouteDirection::Unknown);
+}
+
+TEST(RacingControlHelpers, LatchesFirstKnownQrDirection)
+{
+  EXPECT_TRUE(
+    racing_control::shouldAcceptQrDirection(
+      racing_control::RouteDirection::Unknown,
+      racing_control::RouteDirection::Clockwise));
+  EXPECT_FALSE(
+    racing_control::shouldAcceptQrDirection(
+      racing_control::RouteDirection::Unknown,
+      racing_control::RouteDirection::Unknown));
+  EXPECT_FALSE(
+    racing_control::shouldAcceptQrDirection(
+      racing_control::RouteDirection::Clockwise,
+      racing_control::RouteDirection::Clockwise));
+  EXPECT_FALSE(
+    racing_control::shouldAcceptQrDirection(
+      racing_control::RouteDirection::Clockwise,
+      racing_control::RouteDirection::Counterclockwise));
+}
+
+TEST(RacingControlHelpers, SelectsPostQrTransitTargetWhenEnabled)
+{
+  EXPECT_EQ(
+    racing_control::qrTransitTargetAfterRecognition(true),
+    racing_control::QrTransitTarget::PostQr);
+  EXPECT_EQ(
+    racing_control::qrTransitTargetAfterRecognition(false),
+    racing_control::QrTransitTarget::Entry);
+}
+
+TEST(RacingControlHelpers, PublishesOneVlmImageFrameOnlyWhenEnabledAndAvailable)
+{
+  EXPECT_TRUE(racing_control::shouldPublishVlmImageFrame(true, true));
+  EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, true));
+  EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(true, false));
+  EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, false));
+}
+
+TEST(RacingControlHelpers, DefaultsToPureNav2RouteExecution)
+{
+  EXPECT_FALSE(racing_control::defaultUseTrajectoryGuard());
+}
+
+TEST(RacingControlHelpers, RetriesActionServerWaitFourTimesBeforeFailing)
+{
+  int wait_calls = 0;
+  const bool result = racing_control::retryActionServerWait(
+    [&]() {
+      ++wait_calls;
+      return false;
+    },
+    3);
+
+  EXPECT_FALSE(result);
+  EXPECT_EQ(wait_calls, 4);
+}
+
+TEST(RacingControlHelpers, RetriesNavigateGoalOnlyAfterFailure)
+{
+  EXPECT_TRUE(racing_control::shouldRetryNavigateGoal(false, 3));
+  EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(true, 3));
+  EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(false, 0));
+}
+
+TEST(RacingControlHelpers, DefaultsToStartupQrEnableBurst)
+{
+  EXPECT_EQ(racing_control::defaultStartupQrEnableRepeats(), 5);
+  EXPECT_EQ(racing_control::defaultStartupQrEnableIntervalMs(), 200);
+}
+
+TEST(RacingControlHelpers, DynamicReplanningStopsNearGoal)
+{
+  EXPECT_TRUE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 1.0, 1.0));
+  EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.4, 0.5, 1.0, 1.0));
+  EXPECT_FALSE(racing_control::shouldDynamicReplan(true, true, 0.6, 0.5, 1.0, 1.0));
+  EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 0.5, 1.0));
+  EXPECT_FALSE(racing_control::shouldDynamicReplan(false, false, 0.6, 0.5, 1.0, 1.0));
+}
+
+TEST(RacingControlHelpers, AdvancesNextWaypointOnlyAfterItIsReached)
+{
+  const auto waypoints = racing_control::posesFromFlatDoubles(
+    {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0},
+    "map");
+
+  EXPECT_EQ(
+    racing_control::advanceReachedWaypointIndex(
+      waypoints, racing_control::poseFromXYYaw(0.40, 0.0, 0.0, "map"), 1, 0.50),
+    1U);
+  EXPECT_EQ(
+    racing_control::advanceReachedWaypointIndex(
+      waypoints, racing_control::poseFromXYYaw(1.05, 0.0, 0.0, "map"), 1, 0.50),
+    2U);
+}
+
+TEST(RacingControlHelpers, DynamicReplanningKeepsRemainingUnreachedWaypoints)
+{
+  const auto waypoints = racing_control::posesFromFlatDoubles(
+    {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0},
+    "map");
+
+  const auto remaining = racing_control::remainingWaypoints(waypoints, 1);
+
+  ASSERT_EQ(remaining.size(), 2U);
+  EXPECT_DOUBLE_EQ(remaining[0].pose.position.x, 1.0);
+  EXPECT_DOUBLE_EQ(remaining[1].pose.position.x, 2.0);
+  EXPECT_TRUE(racing_control::remainingWaypoints(waypoints, waypoints.size()).empty());
+  EXPECT_TRUE(racing_control::remainingWaypoints(waypoints, waypoints.size() + 1).empty());
+}
+
+TEST(RacingControlHelpers, TrimsPassedWaypointsBeforeReplanning)
+{
+  const auto waypoints = racing_control::posesFromFlatDoubles(
+    {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0},
+    "map");
+
+  const auto trimmed = racing_control::remainingWaypointsAfterProgress(
+    waypoints, racing_control::poseFromXYYaw(1.02, 0.0, 0.0, "map"), 1, 0.50);
+
+  ASSERT_EQ(trimmed.size(), 1U);
+  EXPECT_DOUBLE_EQ(trimmed[0].pose.position.x, 2.0);
+}
+
+TEST(RacingControlHelpers, BuildsQrToVlmSegmentThroughEntry)
+{
+  const std::string frame({'m', 'a', 'p'});
+  const auto entry = racing_control::poseFromXYYaw(10.0, 0.0, 0.0, frame);
+  const auto route = racing_control::posesFromFlatDoubles(
+    {1.0, 0.0, 0.0, 2.0, 0.0, 0.0, 3.0, 0.0, 0.0},
+    frame);
+
+  const auto segment = racing_control::routeWaypointsAfterQr(entry, route, 1, true);
+
+  ASSERT_EQ(segment.size(), 3U);
+  EXPECT_DOUBLE_EQ(segment[0].pose.position.x, 10.0);
+  EXPECT_DOUBLE_EQ(segment[1].pose.position.x, 1.0);
+  EXPECT_DOUBLE_EQ(segment[2].pose.position.x, 2.0);
+}
+
+TEST(RacingControlHelpers, CanConnectQrThroughAllRemainingRoute)
+{
+  const std::string frame({'m', 'a', 'p'});
+  const auto entry = racing_control::poseFromXYYaw(10.0, 0.0, 0.0, frame);
+  const auto route = racing_control::posesFromFlatDoubles(
+    {1.0, 0.0, 0.0, 2.0, 0.0, 0.0, 3.0, 0.0, 0.0},
+    frame);
+
+  const auto segment = racing_control::routeWaypointsAfterQr(entry, route, 1, false);
+
+  ASSERT_EQ(segment.size(), 4U);
+  EXPECT_DOUBLE_EQ(segment[0].pose.position.x, 10.0);
+  EXPECT_DOUBLE_EQ(segment[1].pose.position.x, 1.0);
+  EXPECT_DOUBLE_EQ(segment[2].pose.position.x, 2.0);
+  EXPECT_DOUBLE_EQ(segment[3].pose.position.x, 3.0);
+}
+
+TEST(RacingControlHelpers, RecoveryBackupStopsByDistanceOrTimeout)
+{
+  EXPECT_TRUE(racing_control::recoveryBackupComplete(0.04, 0.04, 0.1, 0.2));
+  EXPECT_TRUE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.2, 0.2));
+  EXPECT_FALSE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.1, 0.2));
+}
+
+TEST(RacingControlHelpers, InitialPlanningFailureUsesOnePlanningBackup)
+{
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::Initial,
+      racing_control::RouteFailureKind::Planning, 0, 2),
+    racing_control::RouteRecoveryAction::PlanningBackup);
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::AfterPlanningBackup,
+      racing_control::RouteFailureKind::Planning, 0, 2),
+    racing_control::RouteRecoveryAction::Fail);
+}
+
+TEST(RacingControlHelpers, FollowFailureClearsOnlyLocalCostmapBeforeReplanning)
+{
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::Initial,
+      racing_control::RouteFailureKind::Following, 0, 2),
+    racing_control::RouteRecoveryAction::ClearLocalAndReplan);
+}
+
+TEST(RacingControlHelpers, FailedLocalClearReplanUsesFinalBackup)
+{
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::AfterLocalClearReplan,
+      racing_control::RouteFailureKind::Planning, 0, 2),
+    racing_control::RouteRecoveryAction::FinalBackupReplan);
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::AfterLocalClearReplan,
+      racing_control::RouteFailureKind::Following, 0, 2),
+    racing_control::RouteRecoveryAction::FinalBackupReplan);
+}
+
+TEST(RacingControlHelpers, FinalRecoveryFailsAfterConfiguredAttemptLimit)
+{
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::AfterFinalBackupReplan,
+      racing_control::RouteFailureKind::Following, 1, 2),
+    racing_control::RouteRecoveryAction::FinalBackupReplan);
+  EXPECT_EQ(
+    racing_control::routeRecoveryAction(
+      racing_control::RouteRecoveryPhase::AfterFinalBackupReplan,
+      racing_control::RouteFailureKind::Planning, 2, 2),
+    racing_control::RouteRecoveryAction::Fail);
+}
+
+TEST(RacingControlHelpers, ParsesVlmCaptureMode)
+{
+  EXPECT_EQ(
+    racing_control::vlmCaptureModeFromString("stop"),
+    racing_control::VlmCaptureMode::Stop);
+  EXPECT_EQ(
+    racing_control::vlmCaptureModeFromString("pass_through"),
+    racing_control::VlmCaptureMode::PassThrough);
+  EXPECT_EQ(
+    racing_control::vlmCaptureModeFromString("unknown"),
+    racing_control::VlmCaptureMode::Unknown);
+}
+
+TEST(CandidateWaypointSelector, LoadsNamedCandidatesFromSavedPointJson)
+{
+  const auto dir = std::filesystem::temp_directory_path() / "racing_control_candidates_test";
+  std::filesystem::remove_all(dir);
+  std::filesystem::create_directories(dir);
+  {
+    std::ofstream json(dir / "main.json");
+    json << R"json({
+      "points": [
+        {
+          "name": "qr",
+          "yaw_degrees": 90.0,
+          "odom": {
+            "pose": {
+              "pose": {
+                "position": {"x": 1.0, "y": 2.0, "z": 0.0},
+                "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
+              }
+            }
+          }
+        },
+        {
+          "name": "entry",
+          "yaw_degrees": 0.0,
+          "odom": {
+            "pose": {
+              "pose": {
+                "position": {"x": 3.0, "y": 4.0, "z": 0.0},
+                "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
+              }
+            }
+          }
+        }
+      ]
+    })json";
+  }
+
+  const auto groups = racing_control::loadCandidateWaypointGroups(dir.string(), "odom");
+
+  ASSERT_EQ(groups.size(), 2U);
+  ASSERT_EQ(groups.at("qr").size(), 1U);
+  EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.x, 1.0);
+  EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.y, 2.0);
+  EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.z, std::sin(M_PI_4), kTolerance);
+  EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.w, std::cos(M_PI_4), kTolerance);
+  EXPECT_EQ(groups.at("entry")[0].source_file, "main.json");
+}
+
+TEST(CandidateWaypointSelector, SkipsOccupiedCandidates)
+{
+  nav_msgs::msg::OccupancyGrid costmap;
+  costmap.info.resolution = 1.0;
+  costmap.info.width = 5;
+  costmap.info.height = 5;
+  costmap.info.origin.position.x = 0.0;
+  costmap.info.origin.position.y = 0.0;
+  costmap.info.origin.orientation.w = 1.0;
+  costmap.data.assign(25, 0);
+  costmap.data[1 * 5 + 1] = 80;
+
+  racing_control::CandidateWaypointSelector selector;
+  selector.setGroups({
+    {"qr", {
+      {"qr", "blocked.json", "qr_blocked", racing_control::poseFromXYYaw(1.2, 1.2, 0.0, "odom")},
+      {"qr", "free.json", "qr_free", racing_control::poseFromXYYaw(3.2, 1.2, 0.0, "odom")},
+    }},
+  });
+  selector.setCostmap(costmap);
+
+  const auto selected = selector.select("qr", 50, false);
+
+  ASSERT_TRUE(selected.has_value());
+  EXPECT_EQ(selected->source_file, "free.json");
+  EXPECT_EQ(selected->point_name, "qr_free");
+}
+
+TEST(CandidateWaypointSelector, FindsNearestFreeRecoveryCommand)
+{
+  nav_msgs::msg::OccupancyGrid costmap;
+  costmap.info.resolution = 0.25;
+  costmap.info.width = 9;
+  costmap.info.height = 9;
+  costmap.info.origin.position.x = -1.0;
+  costmap.info.origin.position.y = -1.0;
+  costmap.info.origin.orientation.w = 1.0;
+  costmap.data.assign(81, 100);
+  const auto set_free = [&](const int mx, const int my) {
+    costmap.data[my * static_cast<int>(costmap.info.width) + mx] = 0;
+  };
+  set_free(6, 4);
+  set_free(7, 4);
+
+  const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom");
+  const auto command = racing_control::nearestFreeRecoveryCommand(
+    costmap, current, 50, false, 1.0);
+
+  ASSERT_TRUE(command.has_value());
+  EXPECT_GT(command->linear_x, 0.0);
+  EXPECT_DOUBLE_EQ(command->angular_z, 0.0);
+}

src/vlm_detect/launch/vlm_detect.launch.py

@@ -24,6 +24,12 @@ def generate_launch_description():
     prompt_text = LaunchConfiguration('prompt_text')
     max_tokens = LaunchConfiguration('max_tokens')
     image_max_dim = LaunchConfiguration('image_max_dim')
+    use_tcp = LaunchConfiguration('use_tcp')
+    use_serial = LaunchConfiguration('use_serial')
+    tcp_host = LaunchConfiguration('tcp_host')
+    tcp_port = LaunchConfiguration('tcp_port')
+    serial_port = LaunchConfiguration('serial_port')
+    serial_baud = LaunchConfiguration('serial_baud')
 
     audio_sink = LaunchConfiguration('audio_sink')
     tts_speed = LaunchConfiguration('tts_speed')
@@ -45,6 +51,12 @@ def generate_launch_description():
     declare_prompt_text = DeclareLaunchArgument('prompt_text', default_value='请描述这个病人的状态。不要描述边框、背景。15字以内。')
     declare_max_tokens = DeclareLaunchArgument('max_tokens', default_value='30')
     declare_image_max_dim = DeclareLaunchArgument('image_max_dim', default_value='448')
+    declare_use_tcp = DeclareLaunchArgument('use_tcp', default_value='true')
+    declare_use_serial = DeclareLaunchArgument('use_serial', default_value='false')
+    declare_tcp_host = DeclareLaunchArgument('tcp_host', default_value='192.168.127.1')
+    declare_tcp_port = DeclareLaunchArgument('tcp_port', default_value='9216')
+    declare_serial_port = DeclareLaunchArgument('serial_port', default_value='/dev/ttyUSB0')
+    declare_serial_baud = DeclareLaunchArgument('serial_baud', default_value='921600')
     declare_audio_sink = DeclareLaunchArgument('audio_sink',
         default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
     declare_tts_speed = DeclareLaunchArgument('tts_speed', default_value='1.5')
@@ -66,6 +78,12 @@ def generate_launch_description():
                 'prompt_text': prompt_text,
                 'max_tokens': max_tokens,
                 'image_max_dim': image_max_dim,
+                'use_tcp': use_tcp,
+                'use_serial': use_serial,
+                'tcp_host': tcp_host,
+                'tcp_port': tcp_port,
+                'serial_port': serial_port,
+                'serial_baud': serial_baud,
             }],
     )
 
@@ -104,6 +122,12 @@ def generate_launch_description():
         declare_prompt_text,
         declare_max_tokens,
         declare_image_max_dim,
+        declare_use_tcp,
+        declare_use_serial,
+        declare_tcp_host,
+        declare_tcp_port,
+        declare_serial_port,
+        declare_serial_baud,
         declare_audio_sink,
         declare_tts_speed,
         LogInfo(msg=['Config: ', config_file]),

src/vlm_detect/vlm_detect/serial_vlm_client.py

@@ -0,0 +1,157 @@
+#!/usr/bin/env python3
+"""Serial VLM Client — 通过串口调用 VLM 推理。
+用在客户端 (192.168.175.65)，模拟 OpenAI client.chat.completions.create 接口。
+"""
+
+import json, struct, time, serial
+
+MAGIC = b'\xAB\xCD'
+FLAG_CMD  = ord('C')
+FLAG_IMG  = ord('I')
+FLAG_RESP = ord('R')
+
+
+class SerialVLMError(Exception):
+    pass
+
+
+class SerialVLMClient:
+    """串口 VLM 客户端，兼容 OpenAI client.chat.completions.create 调用模式。"""
+
+    def __init__(self, port='/dev/ttyUSB0', baud=921600, timeout=95):
+        self.port = port
+        self.baud = baud
+        self.timeout = timeout
+        self._ser = None
+
+    def _connect(self):
+        if self._ser is None or not self._ser.is_open:
+            self._ser = serial.Serial(self.port, self.baud, timeout=0.1)
+            self._ser.reset_input_buffer()
+            self._ser.reset_output_buffer()
+
+    def _send_packet(self, flag, data):
+        if isinstance(data, str):
+            data = data.encode('utf-8')
+        self._ser.write(MAGIC)
+        self._ser.write(bytes([flag]))
+        self._ser.write(struct.pack('>I', len(data)))
+        self._ser.write(data)
+        self._ser.flush()
+
+    def _recv_exact(self, n, timeout=15):
+        deadline = time.time() + timeout
+        buf = b''
+        while len(buf) < n:
+            remain = n - len(buf)
+            chunk = self._ser.read(remain)
+            if chunk:
+                buf += chunk
+            if time.time() > deadline:
+                raise SerialVLMError(f"recv timeout: got {len(buf)}/{n}")
+            if not chunk:
+                time.sleep(0.005)
+        return buf
+
+    def _recv_packet(self):
+        while True:
+            b = self._ser.read(1)
+            if not b:
+                continue
+            if b == b'\xAB':
+                b2 = self._ser.read(1)
+                if b2 == b'\xCD':
+                    break
+        flag = self._recv_exact(1)[0]
+        length = struct.unpack('>I', self._recv_exact(4))[0]
+        data = self._recv_exact(length)
+        return flag, data
+
+    def infer(self, image_bytes: bytes, prompt: str) -> dict:
+        """发送图片+提示词，返回 {"success": bool, "answer": str, "elapsed_sec": float}"""
+        self._connect()
+        try:
+            # Reset buffers
+            self._ser.reset_input_buffer()
+            self._ser.reset_output_buffer()
+
+            # Send command
+            cmd = json.dumps({"prompt": prompt, "image_len": len(image_bytes)})
+            self._send_packet(FLAG_CMD, cmd)
+
+            # Send image
+            self._send_packet(FLAG_IMG, image_bytes)
+
+            # Receive response
+            t0 = time.time()
+            flag, data = self._recv_packet()
+            elapsed = time.time() - t0
+
+            if flag != FLAG_RESP:
+                return {"success": False, "error": f"unexpected flag: {flag}"}
+
+            result = json.loads(data.decode('utf-8'))
+            return result
+
+        except SerialVLMError:
+            return {"success": False, "error": "serial timeout"}
+        except Exception as e:
+            return {"success": False, "error": str(e)}
+
+    def close(self):
+        if self._ser and self._ser.is_open:
+            self._ser.close()
+            self._ser = None
+
+    # ---- OpenAI-compatible interface ----
+    class _FakeChoices:
+        class _Msg:
+            def __init__(self, content): self.content = content
+        def __init__(self, content):
+            self.message = self._Msg(content)
+
+    class _FakeResponse:
+        def __init__(self, content):
+            self.choices = [SerialVLMClient._FakeChoices(content)]
+
+    class _FakeCompletions:
+        def __init__(self, client): self._client = client
+        def create(self, *, model=None, messages=None, max_tokens=None, temperature=None, timeout=None):
+            # Extract image and text from OpenAI-format messages
+            import base64
+            image_bytes = None
+            prompt = ""
+            for msg in (messages or []):
+                content = msg.get("content", "")
+                if isinstance(content, list):
+                    for item in content:
+                        t = item.get("type", "")
+                        if t == "text":
+                            prompt = item.get("text", "")
+                        elif t == "image_url":
+                            url = item.get("image_url", {}).get("url", "")
+                            if url.startswith("data:") and "," in url:
+                                image_bytes = base64.b64decode(url.split(",", 1)[1])
+            if image_bytes is None:
+                raise SerialVLMError("no image in messages")
+
+            result = self._client.infer(image_bytes, prompt)
+            if not result.get("success"):
+                raise SerialVLMError(result.get("error", "unknown"))
+            return SerialVLMClient._FakeResponse(result["answer"])
+
+    class _FakeChat:
+        def __init__(self, client): self.completions = SerialVLMClient._FakeCompletions(client)
+
+    @property
+    def chat(self):
+        return self._FakeChat(self)
+
+
+# Convenience function
+def serial_infer(image_bytes, prompt, port='/dev/ttyUSB0', baud=921600):
+    client = SerialVLMClient(port=port, baud=baud)
+    try:
+        return client.infer(image_bytes, prompt)
+    finally:
+        client.close()

src/vlm_detect/vlm_detect/tcp_vlm_client.py

@@ -0,0 +1,146 @@
+#!/usr/bin/env python3
+"""TCP VLM Client — 通过 TCP 套接字调用 VLM 推理。
+用在客户端 (192.168.175.65)，模拟 OpenAI client.chat.completions.create 接口。
+"""
+
+import json, struct, time, socket
+
+
+MAGIC = b'\xAB\xCD'
+FLAG_CMD  = ord('C')
+FLAG_IMG  = ord('I')
+FLAG_RESP = ord('R')
+
+
+class TCPVLMError(Exception):
+    pass
+
+
+class TCPVLMClient:
+    """TCP VLM 客户端，兼容 OpenAI client.chat.completions.create 调用模式。"""
+
+    def __init__(self, host='192.168.127.1', port=9216, timeout=95):
+        self.host = host
+        self.port = port
+        self.timeout = timeout
+
+    def _recv_exact(self, sock, n, timeout=15):
+        deadline = time.time() + timeout
+        buf = b''
+        while len(buf) < n:
+            remain = n - len(buf)
+            sock.settimeout(max(0.1, deadline - time.time()))
+            try:
+                chunk = sock.recv(remain)
+            except socket.timeout:
+                if time.time() > deadline:
+                    raise TCPVLMError(f"recv timeout: got {len(buf)}/{n}")
+                continue
+            if not chunk:
+                raise TCPVLMError("connection closed by server")
+            buf += chunk
+        return buf
+
+    def _send_packet(self, sock, flag, data):
+        if isinstance(data, str):
+            data = data.encode('utf-8')
+        sock.sendall(MAGIC)
+        sock.sendall(bytes([flag]))
+        sock.sendall(struct.pack('>I', len(data)))
+        sock.sendall(data)
+
+    def _recv_packet(self, sock):
+        while True:
+            b = self._recv_exact(sock, 1, timeout=60)
+            if b == b'\xAB':
+                b2 = self._recv_exact(sock, 1, timeout=5)
+                if b2 == b'\xCD':
+                    break
+        flag = self._recv_exact(sock, 1)[0]
+        length = struct.unpack('>I', self._recv_exact(sock, 4))[0]
+        if length > 1024 * 1024:
+            raise TCPVLMError(f"packet too large: {length}")
+        data = self._recv_exact(sock, length, timeout=30)
+        return flag, data
+
+    def infer(self, image_bytes: bytes, prompt: str) -> dict:
+        """发送图片+提示词，返回 {"success": bool, "answer": str, "elapsed_sec": float}"""
+        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        sock.settimeout(self.timeout)
+        try:
+            sock.connect((self.host, self.port))
+
+            # Send command
+            cmd = json.dumps({"prompt": prompt, "image_len": len(image_bytes)})
+            self._send_packet(sock, FLAG_CMD, cmd)
+
+            # Send image
+            self._send_packet(sock, FLAG_IMG, image_bytes)
+
+            # Receive response
+            t0 = time.time()
+            flag, data = self._recv_packet(sock)
+            elapsed = time.time() - t0
+
+            if flag != FLAG_RESP:
+                return {"success": False, "error": f"unexpected flag: {flag}"}
+
+            result = json.loads(data.decode('utf-8'))
+            return result
+
+        except TCPVLMError:
+            return {"success": False, "error": "TCP timeout"}
+        except Exception as e:
+            return {"success": False, "error": str(e)}
+        finally:
+            try:
+                sock.close()
+            except Exception:
+                pass
+
+    # ---- OpenAI-compatible interface ----
+    class _FakeChoices:
+        class _Msg:
+            def __init__(self, content): self.content = content
+        def __init__(self, content):
+            self.message = self._Msg(content)
+
+    class _FakeResponse:
+        def __init__(self, content):
+            self.choices = [TCPVLMClient._FakeChoices(content)]
+
+    class _FakeCompletions:
+        def __init__(self, client): self._client = client
+        def create(self, *, model=None, messages=None, max_tokens=None, temperature=None, timeout=None):
+            import base64
+            image_bytes = None
+            prompt = ""
+            for msg in (messages or []):
+                content = msg.get("content", "")
+                if isinstance(content, list):
+                    for item in content:
+                        t = item.get("type", "")
+                        if t == "text":
+                            prompt = item.get("text", "")
+                        elif t == "image_url":
+                            url = item.get("image_url", {}).get("url", "")
+                            if url.startswith("data:") and "," in url:
+                                image_bytes = base64.b64decode(url.split(",", 1)[1])
+            if image_bytes is None:
+                raise TCPVLMError("no image in messages")
+            result = self._client.infer(image_bytes, prompt)
+            if not result.get("success"):
+                raise TCPVLMError(result.get("error", "unknown"))
+            return TCPVLMClient._FakeResponse(result["answer"])
+
+    class _FakeChat:
+        def __init__(self, client): self.completions = TCPVLMClient._FakeCompletions(client)
+
+    @property
+    def chat(self):
+        return self._FakeChat(self)
+
+
+def tcp_infer(image_bytes, prompt, host='192.168.127.1', port=9216):
+    client = TCPVLMClient(host=host, port=port)
+    return client.infer(image_bytes, prompt)

src/vlm_detect/vlm_detect/vlm_node.py

@@ -12,11 +12,25 @@ import time
 import numpy as np
 from origincar_msg.srv import Speak
 
+# TCP VLM client (optional)
+try:
+    from .tcp_vlm_client import TCPVLMClient, TCPVLMError
+    HAS_TCP = True
+except ImportError:
+    HAS_TCP = False
+
+# Serial VLM client (optional)
+try:
+    from .serial_vlm_client import SerialVLMClient, SerialVLMError
+    HAS_SERIAL = True
+except ImportError:
+    HAS_SERIAL = False
+
 
 class VLMProcessor(Node):
     def __init__(self):
         super().__init__('vlm_detect')
-        self.declare_parameter('vlm_host', 'http://192.168.10.189:8000')
+        self.declare_parameter('vlm_host', 'http://192.168.175.64:8000')
         self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf')
         self.declare_parameter('image_topic', '/image_mjpeg')
         self.declare_parameter('trigger_topic', '/sign4return')
@@ -28,6 +42,12 @@ class VLMProcessor(Node):
         self.declare_parameter('temperature', 0.1)
         self.declare_parameter('crop_ratio', 0.45)
         self.declare_parameter('auto_crop', False)
+        self.declare_parameter('use_tcp', False)
+        self.declare_parameter('use_serial', False)
+        self.declare_parameter('tcp_host', '192.168.127.1')
+        self.declare_parameter('tcp_port', 9216)
+        self.declare_parameter('serial_port', '/dev/ttyUSB0')
+        self.declare_parameter('serial_baud', 921600)
 
         vlm_host = self.get_parameter('vlm_host').value
         vlm_model = self.get_parameter('vlm_model').value
@@ -41,12 +61,39 @@ class VLMProcessor(Node):
         self.temperature = self.get_parameter('temperature').value
         self.crop_ratio = self.get_parameter('crop_ratio').value
         self.auto_crop = self.get_parameter('auto_crop').value
+        self.use_tcp = self.get_parameter('use_tcp').value
+        self.use_serial = self.get_parameter('use_serial').value
+        self.tcp_host = self.get_parameter('tcp_host').value
+        self.tcp_port = self.get_parameter('tcp_port').value
+        self.serial_port = self.get_parameter('serial_port').value
+        self.serial_baud = self.get_parameter('serial_baud').value
+
+        # Init client: TCP > Serial > HTTP
+        self.tcp_client = None
+        self.serial_client = None
+        self.client = None
+
+        if self.use_tcp:
+            if not HAS_TCP:
+                self.get_logger().fatal("tcp_vlm_client not found")
+                raise RuntimeError("tcp_vlm_client module required for use_tcp=True")
+            self.tcp_client = TCPVLMClient(host=self.tcp_host, port=self.tcp_port)
+            self.get_logger().info(f"VLM using TCP | {self.tcp_host}:{self.tcp_port}")
+        elif self.use_serial:
+            if not HAS_SERIAL:
+                self.get_logger().fatal("serial_vlm_client not found")
+                raise RuntimeError("serial_vlm_client module required for use_serial=True")
+            self.serial_client = SerialVLMClient(port=self.serial_port, baud=self.serial_baud)
+            self.get_logger().info(f"VLM using SERIAL | port={self.serial_port} baud={self.serial_baud}")
+        else:
+            self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY")
+            self.get_logger().info(f"VLM using HTTP | host={vlm_host}")
 
-        self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY")
         self.vlm_model = vlm_model
         self.latest_image = None
         self.image_lock = threading.Lock()
         self._busy = False
+        self._vlm_done = False  # one-shot: ignore triggers after first
 
         self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10)
         self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10)
@@ -56,7 +103,8 @@ class VLMProcessor(Node):
         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 ready | host={vlm_host} | dim={self.image_max_dim} | crop={self.auto_crop}")
+        mode = "TCP" if self.use_tcp else ("SERIAL" if self.use_serial else "HTTP")
+        self.get_logger().info(f"VLM ready | mode={mode} | dim={self.image_max_dim} | crop={self.auto_crop}")
 
     def image_callback(self, msg):
         with self.image_lock:
@@ -69,10 +117,14 @@ class VLMProcessor(Node):
     def sign_callback(self, msg):
         if msg.data != self.trigger_sign:
             return
+        if self._vlm_done:
+            self.get_logger().warn("VLM already triggered, skip")
+            return
         if self._busy:
             self.get_logger().warn("Busy, skip")
             return
-        self.get_logger().info(f"Trigger {msg.data}")
+        self._vlm_done = True
+        self.get_logger().info(f"Trigger {msg.data} (first and only)")
         with self.image_lock:
             if self.latest_image is None:
                 self.get_logger().warning("No image, using fallback")
@@ -115,17 +167,36 @@ class VLMProcessor(Node):
         else:
             new_w, new_h = w, h
         _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60])
-        b64 = base64.b64encode(jpeg.tobytes()).decode('utf-8')
+        jpeg_bytes = jpeg.tobytes()
         t0 = time.time()
-        resp = self.client.chat.completions.create(
+
-            model=self.vlm_model,
+        if self.use_tcp:
-            messages=[{"role": "user", "content": [
+            result = self.tcp_client.infer(jpeg_bytes, self.prompt_text)
-                {"type": "text", "text": self.prompt_text},
+            if not result.get("success"):
-                {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}}
+                raise RuntimeError(f"TCP inference failed: {result.get('error', 'unknown')}")
-            ]}],
+            elapsed = result.get("elapsed_sec", time.time() - t0)
-            max_tokens=self.max_tokens, temperature=self.temperature, timeout=60)
+            self.get_logger().info(f"TCP {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB")
-        self.get_logger().info(f"API {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg)//1024}KB")
+            return result["answer"]
-        return resp.choices[0].message.content
+
+        elif self.use_serial:
+            result = self.serial_client.infer(jpeg_bytes, self.prompt_text)
+            if not result.get("success"):
+                raise RuntimeError(f"Serial inference failed: {result.get('error', 'unknown')}")
+            elapsed = result.get("elapsed_sec", time.time() - t0)
+            self.get_logger().info(f"Serial {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB")
+            return result["answer"]
+
+        else:
+            b64 = base64.b64encode(jpeg_bytes).decode('utf-8')
+            resp = self.client.chat.completions.create(
+                model=self.vlm_model,
+                messages=[{"role": "user", "content": [
+                    {"type": "text", "text": self.prompt_text},
+                    {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}}
+                ]}],
+                max_tokens=self.max_tokens, temperature=self.temperature, timeout=60)
+            self.get_logger().info(f"HTTP {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB")
+            return resp.choices[0].message.content
 
     def _detect_and_crop(self, img):
         h, w = img.shape[:2]

src/vlm_detect/vlm_detect/vlm_node_http.py.bak

@@ -0,0 +1,165 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Int32, String
+from sensor_msgs.msg import CompressedImage
+import cv2
+import base64
+import threading
+from openai import OpenAI
+import time
+import numpy as np
+from origincar_msg.srv import Speak
+
+
+class VLMProcessor(Node):
+    def __init__(self):
+        super().__init__('vlm_detect')
+        self.declare_parameter('vlm_host', 'http://192.168.10.189:8000')
+        self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf')
+        self.declare_parameter('image_topic', '/image_mjpeg')
+        self.declare_parameter('trigger_topic', '/sign4return')
+        self.declare_parameter('trigger_sign', 9)
+        self.declare_parameter('result_topic', '/vlm_result')
+        self.declare_parameter('prompt_text', '')
+        self.declare_parameter('max_tokens', 30)
+        self.declare_parameter('image_max_dim', 96)
+        self.declare_parameter('temperature', 0.1)
+        self.declare_parameter('crop_ratio', 0.45)
+        self.declare_parameter('auto_crop', False)
+
+        vlm_host = self.get_parameter('vlm_host').value
+        vlm_model = self.get_parameter('vlm_model').value
+        image_topic = self.get_parameter('image_topic').value
+        trigger_topic = self.get_parameter('trigger_topic').value
+        self.trigger_sign = self.get_parameter('trigger_sign').value
+        result_topic = self.get_parameter('result_topic').value
+        self.prompt_text = self.get_parameter('prompt_text').value
+        self.max_tokens = self.get_parameter('max_tokens').value
+        self.image_max_dim = self.get_parameter('image_max_dim').value
+        self.temperature = self.get_parameter('temperature').value
+        self.crop_ratio = self.get_parameter('crop_ratio').value
+        self.auto_crop = self.get_parameter('auto_crop').value
+
+        self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY")
+        self.vlm_model = vlm_model
+        self.latest_image = None
+        self.image_lock = threading.Lock()
+        self._busy = False
+
+        self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10)
+        self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10)
+        self.result_pub = self.create_publisher(String, result_topic, 10)
+
+        self.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 ready | host={vlm_host} | dim={self.image_max_dim} | crop={self.auto_crop}")
+
+    def image_callback(self, msg):
+        with self.image_lock:
+            try:
+                np_arr = np.frombuffer(msg.data, np.uint8)
+                self.latest_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+            except Exception as e:
+                self.get_logger().error(f"Decode error: {e}")
+
+    def sign_callback(self, msg):
+        if msg.data != self.trigger_sign:
+            return
+        if self._busy:
+            self.get_logger().warn("Busy, skip")
+            return
+        self.get_logger().info(f"Trigger {msg.data}")
+        with self.image_lock:
+            if self.latest_image is None:
+                self.get_logger().warning("No image, using fallback")
+                desc = '患者位于病床上，姿态放松，未观察到明显异常行为。'
+                result_msg = String()
+                result_msg.data = desc
+                self.result_pub.publish(result_msg)
+                if self.tts_client.service_is_ready():
+                    req = Speak.Request()
+                    req.text = desc
+                    self.tts_client.call_async(req)
+                return
+            img = self.latest_image.copy()
+        self._busy = True
+        try:
+            t0 = time.time()
+            desc = self.process_image(img)
+            self.get_logger().info(f"Result({time.time()-t0:.1f}s): {desc}")
+            msg = String()
+            msg.data = desc
+            self.result_pub.publish(msg)
+            if self.tts_client.service_is_ready():
+                req = Speak.Request()
+                req.text = desc
+                self.tts_client.call_async(req)
+        except Exception as e:
+            self.get_logger().error(f"Inference: {e}")
+        finally:
+            self._busy = False
+
+    def process_image(self, img):
+        if self.auto_crop:
+            img = self._detect_and_crop(img)
+        h, w = img.shape[:2]
+        max_dim = self.image_max_dim
+        if max(h, w) > max_dim:
+            scale = max_dim / max(h, w)
+            new_w, new_h = int(w * scale), int(h * scale)
+            img = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA)
+        else:
+            new_w, new_h = w, h
+        _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60])
+        b64 = base64.b64encode(jpeg.tobytes()).decode('utf-8')
+        t0 = time.time()
+        resp = self.client.chat.completions.create(
+            model=self.vlm_model,
+            messages=[{"role": "user", "content": [
+                {"type": "text", "text": self.prompt_text},
+                {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}}
+            ]}],
+            max_tokens=self.max_tokens, temperature=self.temperature, timeout=60)
+        self.get_logger().info(f"API {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg)//1024}KB")
+        return resp.choices[0].message.content
+
+    def _detect_and_crop(self, img):
+        h, w = img.shape[:2]
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+        blurred = cv2.GaussianBlur(gray, (5, 5), 0)
+        edges = cv2.Canny(blurred, 50, 150)
+        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
+        edges = cv2.dilate(edges, kernel, iterations=1)
+        contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+        best_rect, best_area = None, 0
+        for cnt in contours:
+            peri = cv2.arcLength(cnt, True)
+            approx = cv2.approxPolyDP(cnt, 0.02 * peri, True)
+            area = cv2.contourArea(approx)
+            if len(approx) == 4 and area > w * h * 0.02 and area > best_area:
+                best_rect, best_area = approx, area
+        if best_rect is not None:
+            rx, ry, rw, rh = cv2.boundingRect(best_rect)
+            self.get_logger().info(f'Crop: {rw}x{rh}')
+            return img[ry:ry + rh, rx:rx + rw]
+        return img
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VLMProcessor()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/vlm_detect/vlm_detect/vlm_node_serial.py

@@ -0,0 +1,202 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Int32, String
+from sensor_msgs.msg import CompressedImage
+import cv2
+import base64
+import threading
+from openai import OpenAI
+import time
+import numpy as np
+from origincar_msg.srv import Speak
+
+# Serial VLM client (optional)
+try:
+    from serial_vlm_client import SerialVLMClient, SerialVLMError
+    HAS_SERIAL = True
+except ImportError:
+    HAS_SERIAL = False
+
+
+class VLMProcessor(Node):
+    def __init__(self):
+        super().__init__('vlm_detect')
+        self.declare_parameter('vlm_host', 'http://192.168.10.189:8000')
+        self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf')
+        self.declare_parameter('image_topic', '/image_mjpeg')
+        self.declare_parameter('trigger_topic', '/sign4return')
+        self.declare_parameter('trigger_sign', 9)
+        self.declare_parameter('result_topic', '/vlm_result')
+        self.declare_parameter('prompt_text', '')
+        self.declare_parameter('max_tokens', 30)
+        self.declare_parameter('image_max_dim', 96)
+        self.declare_parameter('temperature', 0.1)
+        self.declare_parameter('crop_ratio', 0.45)
+        self.declare_parameter('auto_crop', False)
+        self.declare_parameter('use_serial', False)
+        self.declare_parameter('serial_port', '/dev/ttyUSB0')
+        self.declare_parameter('serial_baud', 921600)
+
+        vlm_host = self.get_parameter('vlm_host').value
+        vlm_model = self.get_parameter('vlm_model').value
+        image_topic = self.get_parameter('image_topic').value
+        trigger_topic = self.get_parameter('trigger_topic').value
+        self.trigger_sign = self.get_parameter('trigger_sign').value
+        result_topic = self.get_parameter('result_topic').value
+        self.prompt_text = self.get_parameter('prompt_text').value
+        self.max_tokens = self.get_parameter('max_tokens').value
+        self.image_max_dim = self.get_parameter('image_max_dim').value
+        self.temperature = self.get_parameter('temperature').value
+        self.crop_ratio = self.get_parameter('crop_ratio').value
+        self.auto_crop = self.get_parameter('auto_crop').value
+        self.use_serial = self.get_parameter('use_serial').value
+        self.serial_port = self.get_parameter('serial_port').value
+        self.serial_baud = self.get_parameter('serial_baud').value
+
+        # Init client: serial or HTTP
+        if self.use_serial:
+            if not HAS_SERIAL:
+                self.get_logger().fatal("serial_vlm_client not found, falling back to serial")
+                raise RuntimeError("serial_vlm_client module required for use_serial=True")
+            self.serial_client = SerialVLMClient(port=self.serial_port, baud=self.serial_baud)
+            self.client = None  # not needed for serial mode
+            self.get_logger().info(f"VLM using SERIAL | port={self.serial_port} baud={self.serial_baud}")
+        else:
+            self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY")
+            self.serial_client = None
+            self.get_logger().info(f"VLM using HTTP | host={vlm_host}")
+
+        self.vlm_model = vlm_model
+        self.latest_image = None
+        self.image_lock = threading.Lock()
+        self._busy = False
+
+        self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10)
+        self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10)
+        self.result_pub = self.create_publisher(String, result_topic, 10)
+
+        self.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 ready | dim={self.image_max_dim} | crop={self.auto_crop}")
+
+    def image_callback(self, msg):
+        with self.image_lock:
+            try:
+                np_arr = np.frombuffer(msg.data, np.uint8)
+                self.latest_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+            except Exception as e:
+                self.get_logger().error(f"Decode error: {e}")
+
+    def sign_callback(self, msg):
+        if msg.data != self.trigger_sign:
+            return
+        if self._busy:
+            self.get_logger().warn("Busy, skip")
+            return
+        self.get_logger().info(f"Trigger {msg.data}")
+        with self.image_lock:
+            if self.latest_image is None:
+                self.get_logger().warning("No image, using fallback")
+                desc = '患者位于病床上，姿态放松，未观察到明显异常行为。'
+                result_msg = String()
+                result_msg.data = desc
+                self.result_pub.publish(result_msg)
+                if self.tts_client.service_is_ready():
+                    req = Speak.Request()
+                    req.text = desc
+                    self.tts_client.call_async(req)
+                return
+            img = self.latest_image.copy()
+        self._busy = True
+        try:
+            t0 = time.time()
+            desc = self.process_image(img)
+            self.get_logger().info(f"Result({time.time()-t0:.1f}s): {desc}")
+            msg = String()
+            msg.data = desc
+            self.result_pub.publish(msg)
+            if self.tts_client.service_is_ready():
+                req = Speak.Request()
+                req.text = desc
+                self.tts_client.call_async(req)
+        except Exception as e:
+            self.get_logger().error(f"Inference: {e}")
+        finally:
+            self._busy = False
+
+    def process_image(self, img):
+        if self.auto_crop:
+            img = self._detect_and_crop(img)
+        h, w = img.shape[:2]
+        max_dim = self.image_max_dim
+        if max(h, w) > max_dim:
+            scale = max_dim / max(h, w)
+            new_w, new_h = int(w * scale), int(h * scale)
+            img = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA)
+        else:
+            new_w, new_h = w, h
+        _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60])
+        jpeg_bytes = jpeg.tobytes()
+        t0 = time.time()
+
+        if self.use_serial:
+            # Serial mode
+            result = self.serial_client.infer(jpeg_bytes, self.prompt_text)
+            if not result.get("success"):
+                raise RuntimeError(f"Serial inference failed: {result.get('error', 'unknown')}")
+            elapsed = result.get("elapsed_sec", time.time() - t0)
+            self.get_logger().info(f"Serial {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB")
+            return result["answer"]
+        else:
+            # HTTP mode (unchanged)
+            b64 = base64.b64encode(jpeg_bytes).decode('utf-8')
+            resp = self.client.chat.completions.create(
+                model=self.vlm_model,
+                messages=[{"role": "user", "content": [
+                    {"type": "text", "text": self.prompt_text},
+                    {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}}
+                ]}],
+                max_tokens=self.max_tokens, temperature=self.temperature, timeout=60)
+            self.get_logger().info(f"API {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB")
+            return resp.choices[0].message.content
+
+    def _detect_and_crop(self, img):
+        h, w = img.shape[:2]
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+        blurred = cv2.GaussianBlur(gray, (5, 5), 0)
+        edges = cv2.Canny(blurred, 50, 150)
+        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
+        edges = cv2.dilate(edges, kernel, iterations=1)
+        contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+        best_rect, best_area = None, 0
+        for cnt in contours:
+            peri = cv2.arcLength(cnt, True)
+            approx = cv2.approxPolyDP(cnt, 0.02 * peri, True)
+            area = cv2.contourArea(approx)
+            if len(approx) == 4 and area > w * h * 0.02 and area > best_area:
+                best_rect, best_area = approx, area
+        if best_rect is not None:
+            rx, ry, rw, rh = cv2.boundingRect(best_rect)
+            self.get_logger().info(f'Crop: {rw}x{rh}')
+            return img[ry:ry + rh, rx:rx + rw]
+        return img
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VLMProcessor()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/vlm_detect/vlm_detect/vlm_node_serial.py.bak

@@ -0,0 +1,202 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Int32, String
+from sensor_msgs.msg import CompressedImage
+import cv2
+import base64
+import threading
+from openai import OpenAI
+import time
+import numpy as np
+from origincar_msg.srv import Speak
+
+# Serial VLM client (optional)
+try:
+    from serial_vlm_client import SerialVLMClient, SerialVLMError
+    HAS_SERIAL = True
+except ImportError:
+    HAS_SERIAL = False
+
+
+class VLMProcessor(Node):
+    def __init__(self):
+        super().__init__('vlm_detect')
+        self.declare_parameter('vlm_host', 'http://192.168.10.189:8000')
+        self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf')
+        self.declare_parameter('image_topic', '/image_mjpeg')
+        self.declare_parameter('trigger_topic', '/sign4return')
+        self.declare_parameter('trigger_sign', 9)
+        self.declare_parameter('result_topic', '/vlm_result')
+        self.declare_parameter('prompt_text', '')
+        self.declare_parameter('max_tokens', 30)
+        self.declare_parameter('image_max_dim', 96)
+        self.declare_parameter('temperature', 0.1)
+        self.declare_parameter('crop_ratio', 0.45)
+        self.declare_parameter('auto_crop', False)
+        self.declare_parameter('use_serial', False)
+        self.declare_parameter('serial_port', '/dev/ttyUSB0')
+        self.declare_parameter('serial_baud', 921600)
+
+        vlm_host = self.get_parameter('vlm_host').value
+        vlm_model = self.get_parameter('vlm_model').value
+        image_topic = self.get_parameter('image_topic').value
+        trigger_topic = self.get_parameter('trigger_topic').value
+        self.trigger_sign = self.get_parameter('trigger_sign').value
+        result_topic = self.get_parameter('result_topic').value
+        self.prompt_text = self.get_parameter('prompt_text').value
+        self.max_tokens = self.get_parameter('max_tokens').value
+        self.image_max_dim = self.get_parameter('image_max_dim').value
+        self.temperature = self.get_parameter('temperature').value
+        self.crop_ratio = self.get_parameter('crop_ratio').value
+        self.auto_crop = self.get_parameter('auto_crop').value
+        self.use_serial = self.get_parameter('use_serial').value
+        self.serial_port = self.get_parameter('serial_port').value
+        self.serial_baud = self.get_parameter('serial_baud').value
+
+        # Init client: serial or HTTP
+        if self.use_serial:
+            if not HAS_SERIAL:
+                self.get_logger().fatal("serial_vlm_client not found, falling back to serial")
+                raise RuntimeError("serial_vlm_client module required for use_serial=True")
+            self.serial_client = SerialVLMClient(port=self.serial_port, baud=self.serial_baud)
+            self.client = None  # not needed for serial mode
+            self.get_logger().info(f"VLM using SERIAL | port={self.serial_port} baud={self.serial_baud}")
+        else:
+            self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY")
+            self.serial_client = None
+            self.get_logger().info(f"VLM using HTTP | host={vlm_host}")
+
+        self.vlm_model = vlm_model
+        self.latest_image = None
+        self.image_lock = threading.Lock()
+        self._busy = False
+
+        self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10)
+        self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10)
+        self.result_pub = self.create_publisher(String, result_topic, 10)
+
+        self.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 ready | dim={self.image_max_dim} | crop={self.auto_crop}")
+
+    def image_callback(self, msg):
+        with self.image_lock:
+            try:
+                np_arr = np.frombuffer(msg.data, np.uint8)
+                self.latest_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+            except Exception as e:
+                self.get_logger().error(f"Decode error: {e}")
+
+    def sign_callback(self, msg):
+        if msg.data != self.trigger_sign:
+            return
+        if self._busy:
+            self.get_logger().warn("Busy, skip")
+            return
+        self.get_logger().info(f"Trigger {msg.data}")
+        with self.image_lock:
+            if self.latest_image is None:
+                self.get_logger().warning("No image, using fallback")
+                desc = '患者位于病床上，姿态放松，未观察到明显异常行为。'
+                result_msg = String()
+                result_msg.data = desc
+                self.result_pub.publish(result_msg)
+                if self.tts_client.service_is_ready():
+                    req = Speak.Request()
+                    req.text = desc
+                    self.tts_client.call_async(req)
+                return
+            img = self.latest_image.copy()
+        self._busy = True
+        try:
+            t0 = time.time()
+            desc = self.process_image(img)
+            self.get_logger().info(f"Result({time.time()-t0:.1f}s): {desc}")
+            msg = String()
+            msg.data = desc
+            self.result_pub.publish(msg)
+            if self.tts_client.service_is_ready():
+                req = Speak.Request()
+                req.text = desc
+                self.tts_client.call_async(req)
+        except Exception as e:
+            self.get_logger().error(f"Inference: {e}")
+        finally:
+            self._busy = False
+
+    def process_image(self, img):
+        if self.auto_crop:
+            img = self._detect_and_crop(img)
+        h, w = img.shape[:2]
+        max_dim = self.image_max_dim
+        if max(h, w) > max_dim:
+            scale = max_dim / max(h, w)
+            new_w, new_h = int(w * scale), int(h * scale)
+            img = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA)
+        else:
+            new_w, new_h = w, h
+        _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60])
+        jpeg_bytes = jpeg.tobytes()
+        t0 = time.time()
+
+        if self.use_serial:
+            # Serial mode
+            result = self.serial_client.infer(jpeg_bytes, self.prompt_text)
+            if not result.get("success"):
+                raise RuntimeError(f"Serial inference failed: {result.get('error', 'unknown')}")
+            elapsed = result.get("elapsed_sec", time.time() - t0)
+            self.get_logger().info(f"Serial {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB")
+            return result["answer"]
+        else:
+            # HTTP mode (unchanged)
+            b64 = base64.b64encode(jpeg_bytes).decode('utf-8')
+            resp = self.client.chat.completions.create(
+                model=self.vlm_model,
+                messages=[{"role": "user", "content": [
+                    {"type": "text", "text": self.prompt_text},
+                    {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}}
+                ]}],
+                max_tokens=self.max_tokens, temperature=self.temperature, timeout=60)
+            self.get_logger().info(f"API {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB")
+            return resp.choices[0].message.content
+
+    def _detect_and_crop(self, img):
+        h, w = img.shape[:2]
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+        blurred = cv2.GaussianBlur(gray, (5, 5), 0)
+        edges = cv2.Canny(blurred, 50, 150)
+        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
+        edges = cv2.dilate(edges, kernel, iterations=1)
+        contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+        best_rect, best_area = None, 0
+        for cnt in contours:
+            peri = cv2.arcLength(cnt, True)
+            approx = cv2.approxPolyDP(cnt, 0.02 * peri, True)
+            area = cv2.contourArea(approx)
+            if len(approx) == 4 and area > w * h * 0.02 and area > best_area:
+                best_rect, best_area = approx, area
+        if best_rect is not None:
+            rx, ry, rw, rh = cv2.boundingRect(best_rect)
+            self.get_logger().info(f'Crop: {rw}x{rh}')
+            return img[ry:ry + rh, rx:rx + rw]
+        return img
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VLMProcessor()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/vlm_detect/vlm_detect/vlm_node_tcp.py

@@ -0,0 +1,231 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Int32, String
+from sensor_msgs.msg import CompressedImage
+import cv2
+import base64
+import threading
+from openai import OpenAI
+import time
+import numpy as np
+from origincar_msg.srv import Speak
+
+# TCP VLM client (optional)
+try:
+    from tcp_vlm_client import TCPVLMClient, TCPVLMError
+    HAS_TCP = True
+except ImportError:
+    HAS_TCP = False
+
+# Serial VLM client (optional)
+try:
+    from serial_vlm_client import SerialVLMClient, SerialVLMError
+    HAS_SERIAL = True
+except ImportError:
+    HAS_SERIAL = False
+
+
+class VLMProcessor(Node):
+    def __init__(self):
+        super().__init__('vlm_detect')
+        self.declare_parameter('vlm_host', 'http://192.168.175.64:8000')
+        self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf')
+        self.declare_parameter('image_topic', '/image_mjpeg')
+        self.declare_parameter('trigger_topic', '/sign4return')
+        self.declare_parameter('trigger_sign', 9)
+        self.declare_parameter('result_topic', '/vlm_result')
+        self.declare_parameter('prompt_text', '')
+        self.declare_parameter('max_tokens', 30)
+        self.declare_parameter('image_max_dim', 96)
+        self.declare_parameter('temperature', 0.1)
+        self.declare_parameter('crop_ratio', 0.45)
+        self.declare_parameter('auto_crop', False)
+        self.declare_parameter('use_tcp', False)
+        self.declare_parameter('use_serial', False)
+        self.declare_parameter('tcp_host', '192.168.127.1')
+        self.declare_parameter('tcp_port', 9216)
+        self.declare_parameter('serial_port', '/dev/ttyUSB0')
+        self.declare_parameter('serial_baud', 921600)
+
+        vlm_host = self.get_parameter('vlm_host').value
+        vlm_model = self.get_parameter('vlm_model').value
+        image_topic = self.get_parameter('image_topic').value
+        trigger_topic = self.get_parameter('trigger_topic').value
+        self.trigger_sign = self.get_parameter('trigger_sign').value
+        result_topic = self.get_parameter('result_topic').value
+        self.prompt_text = self.get_parameter('prompt_text').value
+        self.max_tokens = self.get_parameter('max_tokens').value
+        self.image_max_dim = self.get_parameter('image_max_dim').value
+        self.temperature = self.get_parameter('temperature').value
+        self.crop_ratio = self.get_parameter('crop_ratio').value
+        self.auto_crop = self.get_parameter('auto_crop').value
+        self.use_tcp = self.get_parameter('use_tcp').value
+        self.use_serial = self.get_parameter('use_serial').value
+        self.tcp_host = self.get_parameter('tcp_host').value
+        self.tcp_port = self.get_parameter('tcp_port').value
+        self.serial_port = self.get_parameter('serial_port').value
+        self.serial_baud = self.get_parameter('serial_baud').value
+
+        # Init client: TCP > Serial > HTTP
+        self.tcp_client = None
+        self.serial_client = None
+        self.client = None
+
+        if self.use_tcp:
+            if not HAS_TCP:
+                self.get_logger().fatal("tcp_vlm_client not found")
+                raise RuntimeError("tcp_vlm_client module required for use_tcp=True")
+            self.tcp_client = TCPVLMClient(host=self.tcp_host, port=self.tcp_port)
+            self.get_logger().info(f"VLM using TCP | {self.tcp_host}:{self.tcp_port}")
+        elif self.use_serial:
+            if not HAS_SERIAL:
+                self.get_logger().fatal("serial_vlm_client not found")
+                raise RuntimeError("serial_vlm_client module required for use_serial=True")
+            self.serial_client = SerialVLMClient(port=self.serial_port, baud=self.serial_baud)
+            self.get_logger().info(f"VLM using SERIAL | port={self.serial_port} baud={self.serial_baud}")
+        else:
+            self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY")
+            self.get_logger().info(f"VLM using HTTP | host={vlm_host}")
+
+        self.vlm_model = vlm_model
+        self.latest_image = None
+        self.image_lock = threading.Lock()
+        self._busy = False
+
+        self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10)
+        self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10)
+        self.result_pub = self.create_publisher(String, result_topic, 10)
+
+        self.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...')
+
+        mode = "TCP" if self.use_tcp else ("SERIAL" if self.use_serial else "HTTP")
+        self.get_logger().info(f"VLM ready | mode={mode} | dim={self.image_max_dim} | crop={self.auto_crop}")
+
+    def image_callback(self, msg):
+        with self.image_lock:
+            try:
+                np_arr = np.frombuffer(msg.data, np.uint8)
+                self.latest_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+            except Exception as e:
+                self.get_logger().error(f"Decode error: {e}")
+
+    def sign_callback(self, msg):
+        if msg.data != self.trigger_sign:
+            return
+        if self._busy:
+            self.get_logger().warn("Busy, skip")
+            return
+        self.get_logger().info(f"Trigger {msg.data}")
+        with self.image_lock:
+            if self.latest_image is None:
+                self.get_logger().warning("No image, using fallback")
+                desc = '患者位于病床上，姿态放松，未观察到明显异常行为。'
+                result_msg = String()
+                result_msg.data = desc
+                self.result_pub.publish(result_msg)
+                if self.tts_client.service_is_ready():
+                    req = Speak.Request()
+                    req.text = desc
+                    self.tts_client.call_async(req)
+                return
+            img = self.latest_image.copy()
+        self._busy = True
+        try:
+            t0 = time.time()
+            desc = self.process_image(img)
+            self.get_logger().info(f"Result({time.time()-t0:.1f}s): {desc}")
+            msg = String()
+            msg.data = desc
+            self.result_pub.publish(msg)
+            if self.tts_client.service_is_ready():
+                req = Speak.Request()
+                req.text = desc
+                self.tts_client.call_async(req)
+        except Exception as e:
+            self.get_logger().error(f"Inference: {e}")
+        finally:
+            self._busy = False
+
+    def process_image(self, img):
+        if self.auto_crop:
+            img = self._detect_and_crop(img)
+        h, w = img.shape[:2]
+        max_dim = self.image_max_dim
+        if max(h, w) > max_dim:
+            scale = max_dim / max(h, w)
+            new_w, new_h = int(w * scale), int(h * scale)
+            img = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA)
+        else:
+            new_w, new_h = w, h
+        _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60])
+        jpeg_bytes = jpeg.tobytes()
+        t0 = time.time()
+
+        if self.use_tcp:
+            result = self.tcp_client.infer(jpeg_bytes, self.prompt_text)
+            if not result.get("success"):
+                raise RuntimeError(f"TCP inference failed: {result.get('error', 'unknown')}")
+            elapsed = result.get("elapsed_sec", time.time() - t0)
+            self.get_logger().info(f"TCP {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB")
+            return result["answer"]
+
+        elif self.use_serial:
+            result = self.serial_client.infer(jpeg_bytes, self.prompt_text)
+            if not result.get("success"):
+                raise RuntimeError(f"Serial inference failed: {result.get('error', 'unknown')}")
+            elapsed = result.get("elapsed_sec", time.time() - t0)
+            self.get_logger().info(f"Serial {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB")
+            return result["answer"]
+
+        else:
+            b64 = base64.b64encode(jpeg_bytes).decode('utf-8')
+            resp = self.client.chat.completions.create(
+                model=self.vlm_model,
+                messages=[{"role": "user", "content": [
+                    {"type": "text", "text": self.prompt_text},
+                    {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}}
+                ]}],
+                max_tokens=self.max_tokens, temperature=self.temperature, timeout=60)
+            self.get_logger().info(f"HTTP {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB")
+            return resp.choices[0].message.content
+
+    def _detect_and_crop(self, img):
+        h, w = img.shape[:2]
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+        blurred = cv2.GaussianBlur(gray, (5, 5), 0)
+        edges = cv2.Canny(blurred, 50, 150)
+        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
+        edges = cv2.dilate(edges, kernel, iterations=1)
+        contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+        best_rect, best_area = None, 0
+        for cnt in contours:
+            peri = cv2.arcLength(cnt, True)
+            approx = cv2.approxPolyDP(cnt, 0.02 * peri, True)
+            area = cv2.contourArea(approx)
+            if len(approx) == 4 and area > w * h * 0.02 and area > best_area:
+                best_rect, best_area = approx, area
+        if best_rect is not None:
+            rx, ry, rw, rh = cv2.boundingRect(best_rect)
+            self.get_logger().info(f'Crop: {rw}x{rh}')
+            return img[ry:ry + rh, rx:rx + rw]
+        return img
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VLMProcessor()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()