diff --git a/config/search.yaml b/config/search.yaml index 12cb481..513b628 100644 --- a/config/search.yaml +++ b/config/search.yaml @@ -2,7 +2,7 @@ # Single source of truth for all mission parameters. # ── Flight ─────────────────────────────────────────────────── -altitude: 4.0 # Search altitude (meters) +altitude: 4.0 # Search altitude (feet) # ── ArUco Marker ───────────────────────────────────────────── marker: @@ -13,18 +13,14 @@ marker: # ── Search Patterns ────────────────────────────────────────── spiral: - max_legs: 12 - initial_leg: 4.0 - leg_increment: 3.2 + max_legs: 60 lawnmower: width: 30.0 height: 30.0 - lane_spacing: 3.2 - lanes: 2 levy: - max_steps: 20 + max_steps: 40 field_size: 50.0 # ── Geofence ───────────────────────────────────────────────── diff --git a/config/uav.yaml b/config/uav.yaml index 850a33e..4d90383 100644 --- a/config/uav.yaml +++ b/config/uav.yaml @@ -5,3 +5,7 @@ connection: sim: "tcp:127.0.0.1:5760" real: "/dev/ttyAMA0" baud: 57600 + +speed: + min_mph: 0.2 + max_mph: 1.0 diff --git a/config/ugv.yaml b/config/ugv.yaml index b12d042..3e774df 100644 --- a/config/ugv.yaml +++ b/config/ugv.yaml @@ -11,3 +11,7 @@ position: topics: cmd_vel: /ugv/cmd_vel odometry: /ugv/odometry + +speed: + min_mph: 0.5 + max_mph: 3.5 diff --git a/src/control/search.py b/src/control/search.py index 2853364..2f98d4c 100644 --- a/src/control/search.py +++ b/src/control/search.py @@ -16,13 +16,10 @@ class SearchMode(Enum): SPIRAL = "spiral" LAWNMOWER = "lawnmower" LEVY = "levy" - - class Search: - POSITION_TOLERANCE = 1.0 + POSITION_TOLERANCE = 0.2 CHECK_INTERVAL = 0.5 - MAX_TRAVEL_TIME = 30.0 - CAM_FOV_METERS = 4.0 + MAX_TRAVEL_TIME = 300.0 def __init__(self, ctrl: Controller, detector: ObjectDetector, camera=None, mode: str = "spiral", altitude: float = 5.0, @@ -44,14 +41,17 @@ class Search: self.on_target_found = None self._dispatched_targets = set() - self.spiral_max_legs = 12 - self.spiral_initial_leg = self.CAM_FOV_METERS - self.spiral_leg_increment = self.CAM_FOV_METERS * 0.8 + hfov = 1.3962634 # 80 deg horizontal FOV + self.cam_fov_meters = 2.0 * self.altitude * math.tan(hfov / 2.0) + + self.spiral_max_legs = 40 + self.spiral_initial_leg = self.cam_fov_meters + self.spiral_leg_increment = self.cam_fov_meters * 0.8 self.lawn_width = 30.0 self.lawn_height = 30.0 - self.lawn_lane_spacing = self.CAM_FOV_METERS * 0.8 - self.lawn_lanes = 2 + self.lawn_lane_spacing = self.cam_fov_meters * 0.8 + self.lawn_lanes = int(self.lawn_width / max(self.lawn_lane_spacing, 0.1)) + 1 self.levy_max_steps = 20 self.levy_field_size = 50.0 @@ -159,12 +159,29 @@ class Search: return waypoints def _clip_to_geofence(self, waypoints, polygon): + warn_dist = getattr(self, 'geofence_warn_dist', 3.0) + safe_polygon = [] + if warn_dist > 0: + cx = sum(p[0] for p in polygon) / len(polygon) + cy = sum(p[1] for p in polygon) / len(polygon) + for x, y in polygon: + dx = x - cx + dy = y - cy + length = math.sqrt(dx*dx + dy*dy) + if length > 0: + shrink = warn_dist / length + safe_polygon.append((x - dx * shrink, y - dy * shrink)) + else: + safe_polygon.append((x, y)) + else: + safe_polygon = polygon + clipped = [] for wx, wy in waypoints: - if self._point_in_polygon(wx, wy, polygon): + if self._point_in_polygon(wx, wy, safe_polygon): clipped.append((wx, wy)) else: - nearest = self._nearest_point_on_polygon(wx, wy, polygon) + nearest = self._nearest_point_on_polygon(wx, wy, safe_polygon) if not clipped or distance_2d(clipped[-1], nearest) > 0.5: clipped.append(nearest) return clipped @@ -324,10 +341,13 @@ class Search: IMG_CX, IMG_CY = IMG_W / 2, IMG_H / 2 HFOV = 1.3962634 # 80° horizontal FOV - DESCENT_STEP = 1.0 - LAND_ALT = 1.5 + self.ctrl.update_state() + current_alt = self.ctrl.altitude + + DESCENT_STEP = max(0.5, current_alt * 0.25) + LAND_ALT = min(1.0, current_alt - 0.2) CENTER_PX = 50 - MAX_CORRECTIONS = 30 + MAX_CORRECTIONS = 200 MAX_LOST = 30 GAIN = 0.35 @@ -365,12 +385,16 @@ class Search: def center_over_target(self, target_id): print(f"\n[SEARCH] ===== CENTERING OVER TARGET {target_id} =====") + if hasattr(self, 'uav_min_cms'): + print(f"[SEARCH] Reducing speed to minimum ({self.uav_min_cms} cm/s) for centering.") + self.ctrl.configure_speed_limits(wpnav_speed=self.uav_min_cms, loit_speed=self.uav_min_cms) + IMG_W, IMG_H = 640, 480 IMG_CX, IMG_CY = IMG_W / 2, IMG_H / 2 HFOV = 1.3962634 # 80° horizontal FOV CENTER_PX = 30 - MAX_CORRECTIONS = 30 + MAX_CORRECTIONS = 200 MAX_LOST = 30 GAIN = 0.35 @@ -442,15 +466,22 @@ class Search: alt = max(self.ctrl.altitude, 0.5) ground_w = 2.0 * alt * math.tan(hfov / 2.0) m_per_px = ground_w / img_w + + # Calculate movement in the UAV's body frame + correction_forward = -err_y * m_per_px * gain + correction_right = err_x * m_per_px * gain - # 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 + self.ctrl.update_state() + yaw = self.ctrl.current_yaw + + # Rotate body correction (Forward, Right) into world NED (North, East) + correction_n = correction_forward * math.cos(yaw) - correction_right * math.sin(yaw) + correction_e = correction_forward * math.sin(yaw) + correction_right * math.cos(yaw) cur = self.ctrl.get_local_position() self.ctrl.move_local_ned( - cur['x'] + correction_x, - cur['y'] + correction_y, + cur['x'] + correction_n, + cur['y'] + correction_e, -target_alt) sleep(0.5) @@ -460,8 +491,14 @@ class Search: return centered def wait_for_position(self, target_x, target_y, timeout=None): + self.ctrl.update_state() + pos = self.ctrl.get_local_position() + dist_total = distance_2d((pos['x'], pos['y']), (target_x, target_y)) + if timeout is None: - timeout = self.MAX_TRAVEL_TIME + # Dynamic timeout: 30s base time + 3 seconds per meter + timeout = 30.0 + (dist_total * 3.0) + t0 = time.time() while time.time() - t0 < timeout and self.running: self.ctrl.update_state() diff --git a/src/control/ugv_controller.py b/src/control/ugv_controller.py index 2892ed5..5281c22 100644 --- a/src/control/ugv_controller.py +++ b/src/control/ugv_controller.py @@ -22,9 +22,12 @@ YAW_TOL = 0.15 class UGVController: - def __init__(self, cmd_topic="/ugv/cmd_vel", odom_topic="/ugv/odometry"): + def __init__(self, cmd_topic="/ugv/cmd_vel", odom_topic="/ugv/odometry", + min_speed=0.5, max_speed=1.0): self.node = Node() self.pub = self.node.advertise(cmd_topic, Twist) + self.min_linear_vel = min_speed + self.max_linear_vel = max_speed self.position = {'x': 0.0, 'y': 0.0, 'z': 0.0} self.yaw = 0.0 self._lock = threading.Lock() @@ -59,7 +62,7 @@ class UGVController: def send_cmd_vel(self, linear_x, angular_z): msg = Twist() - msg.linear.x = max(-MAX_LINEAR_VEL, min(MAX_LINEAR_VEL, linear_x)) + msg.linear.x = max(-self.max_linear_vel, min(self.max_linear_vel, linear_x)) msg.angular.z = max(-MAX_ANGULAR_VEL, min(MAX_ANGULAR_VEL, angular_z)) self.pub.publish(msg) @@ -117,9 +120,9 @@ class UGVController: if abs(yaw_error) > YAW_TOL: angular = max(-MAX_ANGULAR_VEL, min(MAX_ANGULAR_VEL, yaw_error * 2.0)) - linear = 0.0 if abs(yaw_error) > 0.5 else 0.3 + linear = 0.0 if abs(yaw_error) > 0.5 else self.min_linear_vel else: - linear = min(MAX_LINEAR_VEL, dist * 0.5) + linear = max(self.min_linear_vel, min(self.max_linear_vel, dist * 0.5)) angular = yaw_error * 1.0 self.send_cmd_vel(linear, angular) diff --git a/src/main.py b/src/main.py index 13a3f30..2be8ccc 100644 --- a/src/main.py +++ b/src/main.py @@ -88,7 +88,10 @@ def main(): search_cfg = load_config('search.yaml') ugv_cfg = load_config('ugv.yaml') - altitude = args.altitude or search_cfg.get('altitude', 5.0) + from utils.convert import feet_to_meters, mph_to_ms + + raw_altitude = args.altitude or search_cfg.get('altitude', 4.0) + altitude = feet_to_meters(raw_altitude) search_mode = args.search marker_cfg = search_cfg.get('marker', {}) @@ -100,6 +103,14 @@ def main(): ugv_pos_cfg = ugv_cfg.get('position', {}) ugv_position = (ugv_pos_cfg.get('x', 0.0), ugv_pos_cfg.get('y', 0.0)) + ugv_speed_cfg = ugv_cfg.get('speed', {}) + ugv_min_mph = ugv_speed_cfg.get('min_mph', 0.5) + ugv_max_mph = ugv_speed_cfg.get('max_mph', 3.5) + + uav_speed_cfg = uav_cfg.get('speed', {}) + uav_min_mph = uav_speed_cfg.get('min_mph', 1.0) + uav_max_mph = uav_speed_cfg.get('max_mph', 15.0) + ugv_topics = ugv_cfg.get('topics', {}) ugv_cmd_topic = ugv_topics.get('cmd_vel', '/ugv/cmd_vel') ugv_odom_topic = ugv_topics.get('odometry', '/ugv/odometry') @@ -113,7 +124,11 @@ def main(): ctrl = Controller(conn_str) - ugv = UGVController(cmd_topic=ugv_cmd_topic, odom_topic=ugv_odom_topic) + # Apply configuration speed bounds + ugv_min_ms = mph_to_ms(ugv_min_mph) + ugv_max_ms = mph_to_ms(ugv_max_mph) + ugv = UGVController(cmd_topic=ugv_cmd_topic, odom_topic=ugv_odom_topic, + min_speed=ugv_min_ms, max_speed=ugv_max_ms) detector = ObjectDetector( marker_size=marker_size, @@ -128,9 +143,8 @@ def main(): def detection_overlay(camera_name, frame): detections = detector.detect(frame) - if detections: - annotated = detector.annotate_frame(frame, detections) - camera.frames[camera_name] = annotated + annotated = detector.annotate_frame(frame, detections) + camera.frames[camera_name] = annotated camera.register_callback("downward", detection_overlay) camera.register_callback("gimbal", detection_overlay) @@ -146,12 +160,15 @@ def main(): if mode_cfg: search.configure(**{f"{search_mode}_{k}": v for k, v in mode_cfg.items()}) + search.uav_min_cms = int(mph_to_ms(uav_min_mph) * 100) + geofence_cfg = search_cfg.get('geofence', {}) geofence_points = None if geofence_cfg.get('enabled', False): raw_pts = geofence_cfg.get('points', []) if raw_pts: geofence_points = [tuple(p) for p in raw_pts] + search.geofence_warn_dist = geofence_cfg.get('warning_distance', 3.0) waypoints = search.plan(start_pos=(0.0, 0.0), geofence_points=geofence_points) @@ -172,6 +189,12 @@ def main(): tracker.start() ctrl.configure_ekf_fast_converge() + + # Configure AP speeds in cm/s from max_mph + uav_max_ms = mph_to_ms(uav_max_mph) + uav_max_cms = int(uav_max_ms * 100) + ctrl.configure_speed_limits(wpnav_speed=uav_max_cms, loit_speed=uav_max_cms) + setup_ardupilot(ctrl) if recorder: recorder.set_phase("ekf_wait") diff --git a/src/navigation/flight_tracker.py b/src/navigation/flight_tracker.py index 68c98ec..245060c 100644 --- a/src/navigation/flight_tracker.py +++ b/src/navigation/flight_tracker.py @@ -20,6 +20,7 @@ class FlightTracker: UAV_COLOR = (0, 220, 100) UAV_TRAIL_COLOR = (0, 180, 80) UGV_COLOR = (60, 60, 255) + UGV_TRAIL_COLOR = (40, 40, 200) TEXT_COLOR = (200, 200, 200) TEXT_DIM_COLOR = (120, 120, 120) MARKER_COLOR = (180, 0, 220) @@ -42,6 +43,7 @@ class FlightTracker: self.uav_alt = 0.0 self.uav_heading = 0.0 self.trail = deque() + self.ugv_trail = deque() self.markers_found = {} self.plan_waypoints = [] @@ -81,6 +83,7 @@ class FlightTracker: def update_ugv(self, x, y): with self._lock: self.ugv_pos = (x, y) + self.ugv_trail.append((x, y)) def add_marker(self, marker_id, x, y): with self._lock: @@ -136,6 +139,7 @@ class FlightTracker: with self._lock: trail_copy = list(self.trail) + ugv_trail_copy = list(self.ugv_trail) uav = self.uav_pos uav_alt = self.uav_alt uav_hdg = self.uav_heading @@ -147,7 +151,9 @@ class FlightTracker: if plan_copy: self._draw_plan(frame, plan_copy, active_wp) - self._draw_trail(frame, trail_copy) + self._draw_trail(frame, trail_copy, self.UAV_TRAIL_COLOR) + if self.show_ugv: + self._draw_trail(frame, ugv_trail_copy, self.UGV_TRAIL_COLOR) if self.show_ugv: self._draw_ugv(frame, ugv) @@ -265,12 +271,12 @@ class FlightTracker: cv2.putText(frame, str(i), (px + 7, py - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.28, self.PLAN_COLOR, 1) - def _draw_trail(self, frame, trail): + def _draw_trail(self, frame, trail, color): if len(trail) < 2: return pts = [self.world_to_pixel(x, y) for x, y in trail] for i in range(1, len(pts)): - cv2.line(frame, pts[i - 1], pts[i], self.UAV_TRAIL_COLOR, 2) + cv2.line(frame, pts[i - 1], pts[i], color, 2) def _draw_uav(self, frame, pos, heading): px, py = self.world_to_pixel(pos[0], pos[1]) @@ -350,8 +356,6 @@ class FlightTracker: else: cv2.putText(frame, "None", (x0, y), cv2.FONT_HERSHEY_SIMPLEX, 0.35, self.TEXT_DIM_COLOR, 1) y += 16 - y += 20 - cv2.putText(frame, f"Trail: {len(trail)} pts", (x0, y), cv2.FONT_HERSHEY_SIMPLEX, 0.35, self.TEXT_DIM_COLOR, 1) def start(self): self.running = True diff --git a/src/utils/convert.py b/src/utils/convert.py new file mode 100644 index 0000000..1c669cc --- /dev/null +++ b/src/utils/convert.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 +""" +Unit conversions: Customary (US) to Metric. +""" + +def feet_to_meters(feet: float) -> float: + return feet * 0.3048 + +def inches_to_meters(inches: float) -> float: + return inches * 0.0254 + +def miles_to_meters(miles: float) -> float: + return miles * 1609.34 + +def pounds_to_kg(pounds: float) -> float: + return pounds * 0.453592 + +def ounces_to_kg(ounces: float) -> float: + return ounces * 0.0283495 + +def mph_to_ms(mph: float) -> float: + return mph * 0.44704 diff --git a/src/vision/object_detector.py b/src/vision/object_detector.py index 0a706f9..3e78709 100644 --- a/src/vision/object_detector.py +++ b/src/vision/object_detector.py @@ -123,6 +123,13 @@ class ObjectDetector: def annotate_frame(self, frame, detections): annotated = frame.copy() + + # Draw center crosshair + h, w = frame.shape[:2] + cx, cy = w // 2, h // 2 + cv2.line(annotated, (cx - 15, cy), (cx + 15, cy), (0, 255, 0), 2) + cv2.line(annotated, (cx, cy - 15), (cx, cy + 15), (0, 255, 0), 2) + if not detections: return annotated overlay = annotated.copy()