Preflight Planner Update
This commit is contained in:
@@ -9,7 +9,6 @@ from enum import Enum
|
||||
|
||||
from control.uav_controller import Controller
|
||||
from vision.object_detector import ObjectDetector
|
||||
from vision.visual_servoing import VisualServoing
|
||||
from utils.helpers import distance_2d
|
||||
|
||||
|
||||
@@ -42,11 +41,7 @@ class Search:
|
||||
self.actions = actions or {}
|
||||
self.land_ids = set(self.actions.get("land", []))
|
||||
|
||||
self.servoing = None
|
||||
if self.land_ids:
|
||||
target_id = list(self.land_ids)[0]
|
||||
self.servoing = VisualServoing(ctrl, target_marker_id=target_id)
|
||||
|
||||
# Search pattern parameters
|
||||
self.spiral_max_legs = 12
|
||||
self.spiral_initial_leg = self.CAM_FOV_METERS
|
||||
self.spiral_leg_increment = self.CAM_FOV_METERS * 0.8
|
||||
@@ -60,24 +55,236 @@ class Search:
|
||||
self.levy_field_size = 50.0
|
||||
self.levy_angle_dist = uniform(loc=-180, scale=360)
|
||||
|
||||
# Pre-flight plan
|
||||
self.waypoints = []
|
||||
self.current_wp = 0
|
||||
self._on_waypoint_change = None # callback: fn(index)
|
||||
|
||||
def configure(self, **kwargs):
|
||||
for key, value in kwargs.items():
|
||||
if hasattr(self, key):
|
||||
setattr(self, key, value)
|
||||
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
# PRE-FLIGHT PLANNING
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
|
||||
def plan(self, start_pos=(0.0, 0.0), geofence_points=None):
|
||||
"""Pre-compute all waypoints for the search pattern.
|
||||
|
||||
Args:
|
||||
start_pos: (x, y) starting position in local NED
|
||||
geofence_points: list of (x, y) polygon vertices, or None
|
||||
|
||||
Returns:
|
||||
List of (x, y) absolute waypoints
|
||||
"""
|
||||
if self.mode == SearchMode.SPIRAL:
|
||||
waypoints = self._plan_spiral(start_pos)
|
||||
elif self.mode == SearchMode.LAWNMOWER:
|
||||
waypoints = self._plan_lawnmower(start_pos)
|
||||
elif self.mode == SearchMode.LEVY:
|
||||
waypoints = self._plan_levy(start_pos)
|
||||
else:
|
||||
waypoints = [start_pos]
|
||||
|
||||
# Clip to geofence
|
||||
if geofence_points:
|
||||
waypoints = self._clip_to_geofence(waypoints, geofence_points)
|
||||
|
||||
self.waypoints = waypoints
|
||||
self.current_wp = 0
|
||||
|
||||
print(f"\n[PLAN] {self.mode.value.upper()} flight plan: "
|
||||
f"{len(waypoints)} waypoints")
|
||||
for i, (wx, wy) in enumerate(waypoints):
|
||||
print(f" WP{i:02d}: ({wx:+.1f}, {wy:+.1f})")
|
||||
if geofence_points:
|
||||
print(f"[PLAN] Clipped to geofence ({len(geofence_points)} vertices)")
|
||||
|
||||
return waypoints
|
||||
|
||||
def _plan_spiral(self, start):
|
||||
"""Expanding square spiral from center outward."""
|
||||
waypoints = [start]
|
||||
x, y = start
|
||||
|
||||
directions = [
|
||||
(0, 1), # East (+Y)
|
||||
(1, 0), # North (+X)
|
||||
(0, -1), # West (-Y)
|
||||
(-1, 0), # South (-X)
|
||||
]
|
||||
|
||||
distance = self.spiral_initial_leg
|
||||
dir_idx = 0
|
||||
legs_at_this_dist = 0
|
||||
|
||||
for _ in range(self.spiral_max_legs):
|
||||
dx_dir, dy_dir = directions[dir_idx % 4]
|
||||
x += dx_dir * distance
|
||||
y += dy_dir * distance
|
||||
waypoints.append((x, y))
|
||||
|
||||
dir_idx += 1
|
||||
legs_at_this_dist += 1
|
||||
if legs_at_this_dist >= 2:
|
||||
legs_at_this_dist = 0
|
||||
distance += self.spiral_leg_increment
|
||||
|
||||
return waypoints
|
||||
|
||||
def _plan_lawnmower(self, start):
|
||||
"""Lawnmower (boustrophedon) pattern."""
|
||||
waypoints = [start]
|
||||
sx, sy = start
|
||||
lane_spacing = self.lawn_lane_spacing
|
||||
height = self.lawn_height
|
||||
num_lanes = max(1, int(self.lawn_width / lane_spacing))
|
||||
|
||||
for lane in range(num_lanes):
|
||||
lane_x = sx + lane * lane_spacing
|
||||
if lane % 2 == 0:
|
||||
target_y = sy + height
|
||||
else:
|
||||
target_y = sy
|
||||
|
||||
waypoints.append((lane_x, target_y))
|
||||
|
||||
if lane < num_lanes - 1:
|
||||
next_x = sx + (lane + 1) * lane_spacing
|
||||
waypoints.append((next_x, target_y))
|
||||
|
||||
return waypoints
|
||||
|
||||
def _plan_levy(self, start):
|
||||
"""Lévy walk — random but pre-computed."""
|
||||
waypoints = [start]
|
||||
x, y = start
|
||||
|
||||
for _ in range(self.levy_max_steps):
|
||||
angle_deg = self.levy_angle_dist.rvs()
|
||||
angle_rad = math.radians(angle_deg)
|
||||
raw_distance = levy.rvs(loc=1, scale=1)
|
||||
distance = min(max(raw_distance, 1.0), self.levy_field_size)
|
||||
|
||||
x += distance * math.cos(angle_rad)
|
||||
y += distance * math.sin(angle_rad)
|
||||
waypoints.append((x, y))
|
||||
|
||||
return waypoints
|
||||
|
||||
def _clip_to_geofence(self, waypoints, polygon):
|
||||
"""Clip waypoints to stay within the geofence polygon.
|
||||
|
||||
If a waypoint is outside, it is projected to the nearest
|
||||
point on the polygon boundary. Consecutive out-of-bounds
|
||||
waypoints are collapsed.
|
||||
"""
|
||||
clipped = []
|
||||
for wx, wy in waypoints:
|
||||
if self._point_in_polygon(wx, wy, polygon):
|
||||
clipped.append((wx, wy))
|
||||
else:
|
||||
nearest = self._nearest_point_on_polygon(wx, wy, polygon)
|
||||
# Avoid duplicate consecutive points
|
||||
if not clipped or distance_2d(clipped[-1], nearest) > 0.5:
|
||||
clipped.append(nearest)
|
||||
return clipped
|
||||
|
||||
@staticmethod
|
||||
def _point_in_polygon(px, py, polygon):
|
||||
"""Ray-casting point-in-polygon test."""
|
||||
n = len(polygon)
|
||||
inside = False
|
||||
j = n - 1
|
||||
for i in range(n):
|
||||
xi, yi = polygon[i]
|
||||
xj, yj = polygon[j]
|
||||
if ((yi > py) != (yj > py)) and \
|
||||
(px < (xj - xi) * (py - yi) / (yj - yi) + xi):
|
||||
inside = not inside
|
||||
j = i
|
||||
return inside
|
||||
|
||||
@staticmethod
|
||||
def _nearest_point_on_polygon(px, py, polygon):
|
||||
"""Find the nearest point on the polygon edge to (px, py)."""
|
||||
min_dist = float('inf')
|
||||
nearest = (px, py)
|
||||
n = len(polygon)
|
||||
for i in range(n):
|
||||
x1, y1 = polygon[i]
|
||||
x2, y2 = polygon[(i + 1) % n]
|
||||
# Project point onto segment
|
||||
dx, dy = x2 - x1, y2 - y1
|
||||
length_sq = dx * dx + dy * dy
|
||||
if length_sq == 0:
|
||||
proj = (x1, y1)
|
||||
else:
|
||||
t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
|
||||
proj = (x1 + t * dx, y1 + t * dy)
|
||||
dist = math.sqrt((px - proj[0])**2 + (py - proj[1])**2)
|
||||
if dist < min_dist:
|
||||
min_dist = dist
|
||||
nearest = proj
|
||||
return nearest
|
||||
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
# EXECUTION
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
|
||||
def run(self):
|
||||
"""Execute the pre-planned search by following waypoints."""
|
||||
if not self.waypoints:
|
||||
self.plan()
|
||||
|
||||
n = len(self.waypoints)
|
||||
print(f"\n{'=' * 50}")
|
||||
print(f" SEARCH: {self.mode.value.upper()} at {self.altitude}m")
|
||||
print(f" Waypoints: {n}")
|
||||
if self.land_ids:
|
||||
print(f" Landing targets: {self.land_ids}")
|
||||
print(f"{'=' * 50}\n")
|
||||
|
||||
if self.mode == SearchMode.SPIRAL:
|
||||
return self.spiral()
|
||||
elif self.mode == SearchMode.LAWNMOWER:
|
||||
return self.lawnmower()
|
||||
elif self.mode == SearchMode.LEVY:
|
||||
return self.levy_walk()
|
||||
for i, (wx, wy) in enumerate(self.waypoints):
|
||||
if not self.running:
|
||||
break
|
||||
|
||||
self.current_wp = i
|
||||
if self._on_waypoint_change:
|
||||
self._on_waypoint_change(i)
|
||||
|
||||
self.ctrl.update_state()
|
||||
pos = self.ctrl.get_local_position()
|
||||
dist = distance_2d((pos['x'], pos['y']), (wx, wy))
|
||||
|
||||
dir_label = ""
|
||||
if i > 0:
|
||||
prev = self.waypoints[i - 1]
|
||||
dx = wx - prev[0]
|
||||
dy = wy - prev[1]
|
||||
if abs(dx) > abs(dy):
|
||||
dir_label = "N" if dx > 0 else "S"
|
||||
else:
|
||||
dir_label = "E" if dy > 0 else "W"
|
||||
|
||||
print(f"[SEARCH] WP {i + 1}/{n} "
|
||||
f"→ ({wx:+.1f}, {wy:+.1f}) "
|
||||
f"dist:{dist:.1f}m {dir_label} "
|
||||
f"markers:{len(self.found_markers)}")
|
||||
|
||||
self.move_to_local(wx, wy)
|
||||
|
||||
if not self._landing:
|
||||
print(f"\n[SEARCH] Search complete. "
|
||||
f"Found {len(self.found_markers)} markers.")
|
||||
|
||||
return self.found_markers
|
||||
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
# CAMERA & MARKER DETECTION
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
|
||||
def get_camera_frame(self):
|
||||
if self.camera is None:
|
||||
@@ -114,15 +321,21 @@ class Search:
|
||||
f"UAV at ({pos['x']:.1f}, {pos['y']:.1f})")
|
||||
|
||||
if marker_id in self.land_ids and not self._landing:
|
||||
print(f"\n[SEARCH] Landing target ID:{marker_id} found! Starting approach.")
|
||||
print(f"\n[SEARCH] Landing target ID:{marker_id} found! "
|
||||
f"Starting approach.")
|
||||
self._landing = True
|
||||
self.execute_landing(detections)
|
||||
return new_markers
|
||||
|
||||
return new_markers
|
||||
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
# LANDING
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
|
||||
def execute_landing(self, initial_detections):
|
||||
"""Visual-servoing descent: keep the ArUco tag centered while descending."""
|
||||
"""Visual-servoing descent: keep the ArUco tag centered
|
||||
while descending."""
|
||||
target_det = None
|
||||
for d in initial_detections:
|
||||
if d.get("id") in self.land_ids:
|
||||
@@ -137,102 +350,34 @@ class Search:
|
||||
print(f"[SEARCH] Target marker ID:{target_det['id']} "
|
||||
f"distance:{target_det['distance']:.2f}m")
|
||||
|
||||
# Camera parameters
|
||||
# Camera parameters (must match model.sdf)
|
||||
IMG_W, IMG_H = 640, 480
|
||||
IMG_CX, IMG_CY = IMG_W / 2, IMG_H / 2
|
||||
HFOV = 1.3962634 # radians (~80 degrees)
|
||||
HFOV = 1.3962634 # 80° horizontal FOV
|
||||
|
||||
# Landing parameters
|
||||
DESCENT_STEP = 1.0 # descend 1m per step
|
||||
LAND_ALT = 1.5 # switch to blind land at this altitude
|
||||
CENTER_PX = 40 # centered if within this many pixels
|
||||
MAX_CORRECTIONS = 15 # max correction iterations per altitude step
|
||||
GAIN = 0.4 # damping factor for corrections
|
||||
DESCENT_STEP = 1.0
|
||||
LAND_ALT = 1.5
|
||||
CENTER_PX = 50
|
||||
MAX_CORRECTIONS = 30
|
||||
MAX_LOST = 30
|
||||
GAIN = 0.35
|
||||
|
||||
# Phase 1: Initial approach — fly toward marker using tvec as rough guide
|
||||
tvec = target_det.get("tvec", [0, 0, 0])
|
||||
# Start visual servoing from current position
|
||||
self.ctrl.update_state()
|
||||
pos = self.ctrl.get_local_position()
|
||||
current_alt = self.ctrl.altitude
|
||||
|
||||
# For a downward camera (90° pitch): tvec[0]=right=East, tvec[1]=down=North
|
||||
# But this is unreliable, so just use it as a rough initial move
|
||||
rough_x = pos['x'] + tvec[1]
|
||||
rough_y = pos['y'] + tvec[0]
|
||||
|
||||
print(f"[SEARCH] Phase 1: Rough approach to ({rough_x:.1f}, {rough_y:.1f})")
|
||||
self.ctrl.move_local_ned(rough_x, rough_y, -self.altitude)
|
||||
self._wait_arrival(rough_x, rough_y, timeout=10.0)
|
||||
sleep(1.0) # settle
|
||||
|
||||
# Phase 2: Visual servoing descent
|
||||
current_alt = self.altitude
|
||||
lost_count = 0
|
||||
MAX_LOST = 10 # abort if marker lost this many consecutive times
|
||||
|
||||
print(f"[SEARCH] Phase 2: Visual servoing descent from {current_alt:.1f}m")
|
||||
print(f"[SEARCH] Visual servoing descent from {current_alt:.1f}m")
|
||||
|
||||
while current_alt > LAND_ALT and self.running:
|
||||
# Center over marker at current altitude
|
||||
centered = False
|
||||
for attempt in range(MAX_CORRECTIONS):
|
||||
frame = self.get_camera_frame()
|
||||
if frame is None:
|
||||
sleep(0.2)
|
||||
continue
|
||||
centered = self._center_over_marker(
|
||||
current_alt, IMG_W, IMG_H, IMG_CX, IMG_CY, HFOV,
|
||||
CENTER_PX, MAX_CORRECTIONS, MAX_LOST, GAIN)
|
||||
|
||||
detections = self.detector.detect(frame)
|
||||
target = None
|
||||
for d in detections:
|
||||
if d.get("id") in self.land_ids:
|
||||
target = d
|
||||
break
|
||||
|
||||
if target is None:
|
||||
lost_count += 1
|
||||
print(f"\r[SEARCH] Alt:{current_alt:.1f}m MARKER LOST ({lost_count}/{MAX_LOST}) ",
|
||||
end='', flush=True)
|
||||
if lost_count >= MAX_LOST:
|
||||
print(f"\n[SEARCH] Marker lost too many times, aborting landing")
|
||||
self._landing = False
|
||||
return
|
||||
sleep(0.3)
|
||||
continue
|
||||
|
||||
lost_count = 0
|
||||
|
||||
# Pixel error from image center
|
||||
cx, cy = target["center_px"]
|
||||
err_x = cx - IMG_CX # positive = marker is right of center
|
||||
err_y = cy - IMG_CY # positive = marker is below center
|
||||
|
||||
print(f"\r[SEARCH] Alt:{current_alt:.1f}m err=({err_x:+.0f},{err_y:+.0f})px "
|
||||
f"dist:{target['distance']:.2f}m ({attempt+1}/{MAX_CORRECTIONS}) ",
|
||||
end='', flush=True)
|
||||
|
||||
if abs(err_x) < CENTER_PX and abs(err_y) < CENTER_PX:
|
||||
centered = True
|
||||
break
|
||||
|
||||
# Convert pixel error to meters using FOV and altitude
|
||||
# At current altitude, the ground plane width visible is:
|
||||
# ground_width = 2 * alt * tan(HFOV/2)
|
||||
self.ctrl.update_state()
|
||||
alt = max(self.ctrl.altitude, 0.5)
|
||||
ground_w = 2.0 * alt * math.tan(HFOV / 2.0)
|
||||
m_per_px = ground_w / IMG_W
|
||||
|
||||
# Apply correction (pixel right = East = +y, pixel down = North = +x)
|
||||
correction_y = err_x * m_per_px * GAIN # East
|
||||
correction_x = err_y * m_per_px * GAIN # North
|
||||
|
||||
cur = self.ctrl.get_local_position()
|
||||
new_x = cur['x'] + correction_x
|
||||
new_y = cur['y'] + correction_y
|
||||
self.ctrl.move_local_ned(new_x, new_y, -current_alt)
|
||||
sleep(0.4)
|
||||
|
||||
if not centered:
|
||||
print(f"\n[SEARCH] Could not fully center at {current_alt:.1f}m, continuing descent")
|
||||
if centered is None:
|
||||
print(f"\n[SEARCH] Marker lost, aborting landing")
|
||||
self._landing = False
|
||||
return
|
||||
|
||||
# Descend one step
|
||||
current_alt = max(current_alt - DESCENT_STEP, LAND_ALT)
|
||||
@@ -240,52 +385,94 @@ class Search:
|
||||
self.ctrl.update_state()
|
||||
cur = self.ctrl.get_local_position()
|
||||
self.ctrl.move_local_ned(cur['x'], cur['y'], -current_alt)
|
||||
sleep(1.5) # wait for descent
|
||||
sleep(2.0)
|
||||
|
||||
# Phase 3: Final centering at low altitude
|
||||
print(f"[SEARCH] Phase 3: Final centering at {LAND_ALT:.1f}m")
|
||||
for attempt in range(MAX_CORRECTIONS):
|
||||
# Final precision centering
|
||||
print(f"[SEARCH] Final centering at {LAND_ALT:.1f}m")
|
||||
self._center_over_marker(
|
||||
LAND_ALT, IMG_W, IMG_H, IMG_CX, IMG_CY, HFOV,
|
||||
35, MAX_CORRECTIONS, MAX_LOST, 0.25)
|
||||
|
||||
# Land
|
||||
print(f"\n[SEARCH] ===== LANDING =====")
|
||||
self.ctrl.land()
|
||||
self.landed = True
|
||||
self.running = False
|
||||
|
||||
def _center_over_marker(self, target_alt, img_w, img_h, img_cx, img_cy,
|
||||
hfov, center_px, max_corrections, max_lost, gain):
|
||||
"""Center the UAV over the ArUco marker using pixel error.
|
||||
|
||||
Returns True if centered, False if partial, None if lost.
|
||||
"""
|
||||
lost_count = 0
|
||||
centered = False
|
||||
|
||||
for attempt in range(max_corrections):
|
||||
frame = self.get_camera_frame()
|
||||
if frame is None:
|
||||
sleep(0.2)
|
||||
sleep(0.3)
|
||||
continue
|
||||
|
||||
detections = self.detector.detect(frame)
|
||||
target = None
|
||||
for d in detections:
|
||||
if d.get("id") in self.land_ids:
|
||||
target = d
|
||||
break
|
||||
|
||||
if target is None:
|
||||
lost_count += 1
|
||||
print(f"\r[SEARCH] Alt:{target_alt:.1f}m "
|
||||
f"MARKER LOST ({lost_count}/{max_lost}) ",
|
||||
end='', flush=True)
|
||||
if lost_count >= max_lost:
|
||||
return None
|
||||
sleep(0.3)
|
||||
continue
|
||||
|
||||
cx, cy = target["center_px"]
|
||||
err_x = cx - IMG_CX
|
||||
err_y = cy - IMG_CY
|
||||
lost_count = 0
|
||||
|
||||
print(f"\r[SEARCH] Final center: err=({err_x:+.0f},{err_y:+.0f})px ",
|
||||
cx, cy = target["center_px"]
|
||||
err_x = cx - img_cx
|
||||
err_y = cy - img_cy
|
||||
|
||||
print(f"\r[SEARCH] Alt:{target_alt:.1f}m "
|
||||
f"err=({err_x:+.0f},{err_y:+.0f})px "
|
||||
f"dist:{target['distance']:.2f}m "
|
||||
f"({attempt+1}/{max_corrections}) ",
|
||||
end='', flush=True)
|
||||
|
||||
if abs(err_x) < 25 and abs(err_y) < 25:
|
||||
print(f"\n[SEARCH] Centered! Landing now.")
|
||||
if abs(err_x) < center_px and abs(err_y) < center_px:
|
||||
centered = True
|
||||
print(f"\n[SEARCH] Centered at {target_alt:.1f}m!")
|
||||
break
|
||||
|
||||
self.ctrl.update_state()
|
||||
alt = max(self.ctrl.altitude, 0.5)
|
||||
ground_w = 2.0 * alt * math.tan(HFOV / 2.0)
|
||||
m_per_px = ground_w / IMG_W
|
||||
correction_y = err_x * m_per_px * 0.3
|
||||
correction_x = err_y * m_per_px * 0.3
|
||||
ground_w = 2.0 * alt * math.tan(hfov / 2.0)
|
||||
m_per_px = ground_w / img_w
|
||||
|
||||
# Camera pitch-90° inverts image axes relative to NED
|
||||
correction_y = -err_x * m_per_px * gain
|
||||
correction_x = -err_y * m_per_px * gain
|
||||
|
||||
cur = self.ctrl.get_local_position()
|
||||
self.ctrl.move_local_ned(cur['x'] + correction_x,
|
||||
cur['y'] + correction_y, -LAND_ALT)
|
||||
self.ctrl.move_local_ned(
|
||||
cur['x'] + correction_x,
|
||||
cur['y'] + correction_y,
|
||||
-target_alt)
|
||||
sleep(0.5)
|
||||
|
||||
# Phase 4: Land
|
||||
print(f"[SEARCH] ===== LANDING =====")
|
||||
self.ctrl.land()
|
||||
self.landed = True
|
||||
self.running = False
|
||||
if not centered:
|
||||
print(f"\n[SEARCH] Partially centered at {target_alt:.1f}m, "
|
||||
f"continuing")
|
||||
|
||||
return centered
|
||||
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
# MOVEMENT
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
|
||||
def wait_for_position(self, target_x, target_y, timeout=None):
|
||||
if timeout is None:
|
||||
@@ -313,7 +500,7 @@ class Search:
|
||||
return False
|
||||
|
||||
def _wait_arrival(self, target_x, target_y, timeout=15.0):
|
||||
"""Wait for position without checking markers (used during landing)."""
|
||||
"""Wait for position without checking markers (landing)."""
|
||||
t0 = time.time()
|
||||
while time.time() - t0 < timeout and self.running:
|
||||
self.ctrl.update_state()
|
||||
@@ -337,110 +524,9 @@ class Search:
|
||||
self.ctrl.move_local_ned(x, y, z)
|
||||
return self.wait_for_position(x, y)
|
||||
|
||||
def move_relative(self, dx, dy):
|
||||
self.ctrl.update_state()
|
||||
pos = self.ctrl.get_local_position()
|
||||
target_x = pos['x'] + dx
|
||||
target_y = pos['y'] + dy
|
||||
self.ctrl.move_pos_rel(dx, dy, 0)
|
||||
return self.wait_for_position(target_x, target_y)
|
||||
|
||||
def spiral(self):
|
||||
distance = self.spiral_initial_leg
|
||||
increment = self.spiral_leg_increment
|
||||
travel_x = True
|
||||
direction = 1
|
||||
|
||||
for leg in range(self.spiral_max_legs):
|
||||
if not self.running:
|
||||
break
|
||||
|
||||
self.ctrl.update_state()
|
||||
pos = self.ctrl.get_local_position()
|
||||
print(f"[SEARCH] Spiral leg {leg + 1}/{self.spiral_max_legs} "
|
||||
f"pos:({pos['x']:.1f}, {pos['y']:.1f}) "
|
||||
f"step:{distance:.1f}m markers:{len(self.found_markers)}")
|
||||
|
||||
if travel_x:
|
||||
dx = distance * direction
|
||||
self.move_relative(dx, 0)
|
||||
else:
|
||||
dy = distance * direction
|
||||
self.move_relative(0, dy)
|
||||
direction *= -1
|
||||
distance += increment
|
||||
|
||||
travel_x = not travel_x
|
||||
|
||||
print(f"[SEARCH] Spiral complete. Found {len(self.found_markers)} markers.")
|
||||
return self.found_markers
|
||||
|
||||
def lawnmower(self):
|
||||
lane_spacing = self.lawn_lane_spacing
|
||||
height = self.lawn_height
|
||||
num_lanes = max(1, int(self.lawn_width / lane_spacing))
|
||||
|
||||
self.ctrl.update_state()
|
||||
start_pos = self.ctrl.get_local_position()
|
||||
start_x = start_pos['x']
|
||||
start_y = start_pos['y']
|
||||
|
||||
print(f"[SEARCH] Lawnmower: {num_lanes} lanes, "
|
||||
f"{lane_spacing:.1f}m spacing, {height:.1f}m height")
|
||||
|
||||
for lane in range(num_lanes):
|
||||
if not self.running:
|
||||
break
|
||||
|
||||
lane_x = start_x + lane * lane_spacing
|
||||
|
||||
if lane % 2 == 0:
|
||||
target_y = start_y + height
|
||||
else:
|
||||
target_y = start_y
|
||||
|
||||
print(f"[SEARCH] Lane {lane + 1}/{num_lanes} "
|
||||
f"x:{lane_x:.1f} markers:{len(self.found_markers)}")
|
||||
|
||||
self.move_to_local(lane_x, target_y)
|
||||
|
||||
if not self.running:
|
||||
break
|
||||
|
||||
if lane < num_lanes - 1:
|
||||
next_x = start_x + (lane + 1) * lane_spacing
|
||||
self.move_to_local(next_x, target_y)
|
||||
|
||||
print(f"[SEARCH] Lawnmower complete. Found {len(self.found_markers)} markers.")
|
||||
return self.found_markers
|
||||
|
||||
def levy_walk(self):
|
||||
field_size = self.levy_field_size
|
||||
|
||||
for step in range(self.levy_max_steps):
|
||||
if not self.running:
|
||||
break
|
||||
|
||||
angle_deg = self.levy_angle_dist.rvs()
|
||||
angle_rad = math.radians(angle_deg)
|
||||
|
||||
raw_distance = levy.rvs(loc=1, scale=1)
|
||||
distance = min(max(raw_distance, 1.0), field_size)
|
||||
|
||||
dx = distance * math.cos(angle_rad)
|
||||
dy = distance * math.sin(angle_rad)
|
||||
|
||||
self.ctrl.update_state()
|
||||
pos = self.ctrl.get_local_position()
|
||||
print(f"[SEARCH] Lévy step {step + 1}/{self.levy_max_steps} "
|
||||
f"angle:{angle_deg:.0f}° dist:{distance:.1f}m "
|
||||
f"pos:({pos['x']:.1f}, {pos['y']:.1f}) "
|
||||
f"markers:{len(self.found_markers)}")
|
||||
|
||||
self.move_relative(dx, dy)
|
||||
|
||||
print(f"[SEARCH] Lévy walk complete. Found {len(self.found_markers)} markers.")
|
||||
return self.found_markers
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
# RESULTS
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
|
||||
def stop(self):
|
||||
self.running = False
|
||||
|
||||
Reference in New Issue
Block a user