Add Ghostbox hardware and logistics automation

coordinate_patrol.py

@@ -490,7 +490,14 @@ class CoordinatePatrol:
 
         return False
 
-    def navigate_path(self, get_state, path, forward=True, arrival_threshold=None):
+    def navigate_path(
+        self,
+        get_state,
+        path,
+        forward=True,
+        arrival_threshold=None,
+        snap_to_nearest=False,
+    ):
         """
         按 path 依次走完所有点后返回。每次调用都必须传入 path。
         get_state: 可调用对象，每次调用返回当前状态 dict，需包含 'x','y','facing'。
@@ -520,6 +527,26 @@ class CoordinatePatrol:
                 break
             time.sleep(poll_sleep_sec)
 
+        if snap_to_nearest and points:
+            current_pos = None
+            try:
+                current_pos = (float(state.get("x")), float(state.get("y")))
+            except Exception:
+                current_pos = None
+            if current_pos is not None:
+                best_idx = 0
+                best_dist = None
+                for idx, point in enumerate(points):
+                    dist = self.get_distance(current_pos, point)
+                    if best_dist is None or dist < best_dist:
+                        best_idx = idx
+                        best_dist = dist
+                if best_idx > 0:
+                    self.logger.info(
+                        f">>> 路径智能接入最近点 {best_idx + 1}/{len(points)}，距离 {best_dist:.2f}"
+                    )
+                    points = points[best_idx:]
+
         for i, target in enumerate(points):
             self.logger.info(f">>> 路径点 {i + 1}/{len(points)}: {target}")
             while True: