Simulation Exit Fixes
This commit is contained in:
@@ -19,7 +19,6 @@ class SearchMode(Enum):
|
||||
|
||||
|
||||
class Search:
|
||||
|
||||
POSITION_TOLERANCE = 1.0
|
||||
CHECK_INTERVAL = 0.5
|
||||
MAX_TRAVEL_TIME = 30.0
|
||||
@@ -41,7 +40,6 @@ class Search:
|
||||
self.actions = actions or {}
|
||||
self.land_ids = set(self.actions.get("land", []))
|
||||
|
||||
# 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
|
||||
@@ -55,30 +53,16 @@ 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)
|
||||
self._on_waypoint_change = None
|
||||
|
||||
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:
|
||||
@@ -88,7 +72,6 @@ class Search:
|
||||
else:
|
||||
waypoints = [start_pos]
|
||||
|
||||
# Clip to geofence
|
||||
if geofence_points:
|
||||
waypoints = self._clip_to_geofence(waypoints, geofence_points)
|
||||
|
||||
@@ -105,7 +88,6 @@ class Search:
|
||||
return waypoints
|
||||
|
||||
def _plan_spiral(self, start):
|
||||
"""Expanding square spiral from center outward."""
|
||||
waypoints = [start]
|
||||
x, y = start
|
||||
|
||||
@@ -135,7 +117,6 @@ class Search:
|
||||
return waypoints
|
||||
|
||||
def _plan_lawnmower(self, start):
|
||||
"""Lawnmower (boustrophedon) pattern."""
|
||||
waypoints = [start]
|
||||
sx, sy = start
|
||||
lane_spacing = self.lawn_lane_spacing
|
||||
@@ -158,7 +139,6 @@ class Search:
|
||||
return waypoints
|
||||
|
||||
def _plan_levy(self, start):
|
||||
"""Lévy walk — random but pre-computed."""
|
||||
waypoints = [start]
|
||||
x, y = start
|
||||
|
||||
@@ -175,26 +155,18 @@ class Search:
|
||||
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
|
||||
@@ -209,14 +181,12 @@ class Search:
|
||||
|
||||
@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:
|
||||
@@ -230,12 +200,7 @@ class Search:
|
||||
nearest = proj
|
||||
return nearest
|
||||
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
# EXECUTION
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
|
||||
def run(self):
|
||||
"""Execute the pre-planned search by following waypoints."""
|
||||
if not self.waypoints:
|
||||
self.plan()
|
||||
|
||||
@@ -282,10 +247,6 @@ class Search:
|
||||
|
||||
return self.found_markers
|
||||
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
# CAMERA & MARKER DETECTION
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
|
||||
def get_camera_frame(self):
|
||||
if self.camera is None:
|
||||
return None
|
||||
@@ -329,13 +290,7 @@ class Search:
|
||||
|
||||
return new_markers
|
||||
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
# LANDING
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
|
||||
def execute_landing(self, initial_detections):
|
||||
"""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:
|
||||
@@ -350,12 +305,10 @@ class Search:
|
||||
print(f"[SEARCH] Target marker ID:{target_det['id']} "
|
||||
f"distance:{target_det['distance']:.2f}m")
|
||||
|
||||
# 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 # 80° horizontal FOV
|
||||
|
||||
# Landing parameters
|
||||
DESCENT_STEP = 1.0
|
||||
LAND_ALT = 1.5
|
||||
CENTER_PX = 50
|
||||
@@ -363,7 +316,6 @@ class Search:
|
||||
MAX_LOST = 30
|
||||
GAIN = 0.35
|
||||
|
||||
# Start visual servoing from current position
|
||||
self.ctrl.update_state()
|
||||
current_alt = self.ctrl.altitude
|
||||
|
||||
@@ -379,7 +331,6 @@ class Search:
|
||||
self._landing = False
|
||||
return
|
||||
|
||||
# Descend one step
|
||||
current_alt = max(current_alt - DESCENT_STEP, LAND_ALT)
|
||||
print(f"\n[SEARCH] Descending to {current_alt:.1f}m")
|
||||
self.ctrl.update_state()
|
||||
@@ -387,13 +338,11 @@ class Search:
|
||||
self.ctrl.move_local_ned(cur['x'], cur['y'], -current_alt)
|
||||
sleep(2.0)
|
||||
|
||||
# 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
|
||||
@@ -401,10 +350,6 @@ class Search:
|
||||
|
||||
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
|
||||
|
||||
@@ -465,15 +410,10 @@ class Search:
|
||||
sleep(0.5)
|
||||
|
||||
if not centered:
|
||||
print(f"\n[SEARCH] Partially centered at {target_alt:.1f}m, "
|
||||
f"continuing")
|
||||
print(f"\n[SEARCH] Partially centered at {target_alt:.1f}m, continuing")
|
||||
|
||||
return centered
|
||||
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
# MOVEMENT
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
|
||||
def wait_for_position(self, target_x, target_y, timeout=None):
|
||||
if timeout is None:
|
||||
timeout = self.MAX_TRAVEL_TIME
|
||||
@@ -500,7 +440,6 @@ class Search:
|
||||
return False
|
||||
|
||||
def _wait_arrival(self, target_x, target_y, timeout=15.0):
|
||||
"""Wait for position without checking markers (landing)."""
|
||||
t0 = time.time()
|
||||
while time.time() - t0 < timeout and self.running:
|
||||
self.ctrl.update_state()
|
||||
@@ -524,10 +463,6 @@ class Search:
|
||||
self.ctrl.move_local_ned(x, y, z)
|
||||
return self.wait_for_position(x, y)
|
||||
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
# RESULTS
|
||||
# ═══════════════════════════════════════════════════════════
|
||||
|
||||
def stop(self):
|
||||
self.running = False
|
||||
|
||||
|
||||
Reference in New Issue
Block a user