Snap prepare route to nearest point

auto_bot_move.py

@@ -305,6 +305,8 @@ class AutoBotMove:
         self.prepare_route_index = 0
         self.prepare_route_completed = not bool(self.prepare_route_waypoints)
         self.is_running_prepare_route = bool(self.prepare_route_waypoints)
+        self.prepare_route_snapped = False
+        self.prepare_route_snap_max_dist = 10.0
         layout = load_layout_config()
         self.patrol_controller = CoordinatePatrol(
             waypoints,
@@ -340,12 +342,52 @@ class AutoBotMove:
     def _has_prepare_route(self) -> bool:
         return bool(self.prepare_route_waypoints)
 
+    def _snap_prepare_route_to_nearest(self, state):
+        if self.prepare_route_snapped or self.prepare_route_completed or not self._has_prepare_route():
+            return
+        self.prepare_route_snapped = True
+
+        try:
+            current_pos = (float(state.get('x')), float(state.get('y')))
+        except Exception:
+            return
+
+        best_idx = None
+        best_dist = None
+        for idx, point in enumerate(self.prepare_route_waypoints):
+            try:
+                target = (float(point[0]), float(point[1]))
+            except Exception:
+                continue
+            dist = self.patrol_controller.get_distance(current_pos, target)
+            if best_dist is None or dist < best_dist:
+                best_idx = idx
+                best_dist = dist
+
+        if best_idx is None or best_dist is None:
+            return
+        if best_dist > self.prepare_route_snap_max_dist:
+            print(
+                f">>> [准备路线] 离准备路线最近点仍较远（{best_dist:.2f}），"
+                "从第 1 个点开始"
+            )
+            return
+
+        self.prepare_route_index = best_idx
+        self.patrol_controller.reset_stuck()
+        print(
+            f">>> [准备路线] 智能接入最近点 {best_idx + 1}/"
+            f"{len(self.prepare_route_waypoints)}，距离 {best_dist:.2f}"
+        )
+
     def _run_prepare_route_step(self, state):
         if self.prepare_route_completed or not self._has_prepare_route():
             self.prepare_route_completed = True
             self.is_running_prepare_route = False
             return
 
+        self._snap_prepare_route_to_nearest(state)
+
         if self.prepare_route_index >= len(self.prepare_route_waypoints):
             self.prepare_route_completed = True
             self.is_running_prepare_route = False