Aruco Code and UVG/UAV Logic Fixes

src/control/search.py

@@ -36,9 +36,13 @@ class Search:
         self.running = True
         self.landed = False
         self._landing = False
+        self.landing_enabled = False
 
         self.actions = actions or {}
         self.land_ids = set(self.actions.get("land", []))
+        self.target_ids = set(self.actions.get("target", []))
+        self.on_target_found = None
+        self._dispatched_targets = set()
 
         self.spiral_max_legs = 12
         self.spiral_initial_leg = self.CAM_FOV_METERS
@@ -281,7 +285,15 @@ class Search:
                       f"distance:{d['distance']:.2f}m  "
                       f"UAV at ({pos['x']:.1f}, {pos['y']:.1f})")
 
-            if marker_id in self.land_ids and not self._landing:
+            if marker_id in self.target_ids and marker_id not in self._dispatched_targets:
+                self._dispatched_targets.add(marker_id)
+                pos = self.found_markers[marker_id]['uav_position']
+                print(f"\n[SEARCH] Target ID:{marker_id} found! "
+                      f"Dispatching UGV to ({pos['x']:.1f}, {pos['y']:.1f})")
+                if self.on_target_found:
+                    self.on_target_found(marker_id, pos['x'], pos['y'])
+
+            if marker_id in self.land_ids and not self._landing and self.landing_enabled:
                 print(f"\n[SEARCH] Landing target ID:{marker_id} found! "
                       f"Starting approach.")
                 self._landing = True