From e2f805f3f38a36c91f642435dbc2f9ea8e134f19 Mon Sep 17 00:00:00 2001 From: SirBlobby Date: Fri, 20 Feb 2026 12:19:44 -0500 Subject: [PATCH] Aruco Code and UVG/UAV Logic Fixes --- config/ardupilot_gps_denied.parm | 15 +- config/search.yaml | 3 +- config/ugv.yaml | 11 +- models/custom_ugv/model.sdf | 228 +++++++++++++------------ models/custom_ugv/tags/land_tag.png | Bin 0 -> 6159 bytes scripts/run_autonomous.sh | 52 +++++- src/control/search.py | 14 +- src/control/uav_controller.py | 49 +++++- src/control/ugv_controller.py | 203 ++++++++++++++-------- src/localization/__init__.py | 1 - src/localization/ekf_fusion.py | 202 ---------------------- src/localization/landmark_tracker.py | 165 ------------------ src/localization/position_estimator.py | 188 -------------------- src/main.py | 157 +++++++++++++++-- src/nodes/__init__.py | 1 - src/nodes/geofence_node.py | 7 - src/nodes/multi_vehicle_coord.py | 112 ------------ src/nodes/vision_nav_node.py | 134 --------------- src/nodes/visual_odom_node.py | 7 - src/utils/recorder.py | 180 +++++++++++++------ worlds/tags/aruco_DICT_4X4_50_1.png | Bin 0 -> 5833 bytes worlds/uav_ugv_search.sdf | 81 ++------- 22 files changed, 667 insertions(+), 1143 deletions(-) create mode 100644 models/custom_ugv/tags/land_tag.png delete mode 100644 src/localization/__init__.py delete mode 100644 src/localization/ekf_fusion.py delete mode 100644 src/localization/landmark_tracker.py delete mode 100644 src/localization/position_estimator.py delete mode 100644 src/nodes/__init__.py delete mode 100644 src/nodes/geofence_node.py delete mode 100644 src/nodes/multi_vehicle_coord.py delete mode 100644 src/nodes/vision_nav_node.py delete mode 100644 src/nodes/visual_odom_node.py create mode 100644 worlds/tags/aruco_DICT_4X4_50_1.png diff --git a/config/ardupilot_gps_denied.parm b/config/ardupilot_gps_denied.parm index c96095e..991ac4e 100644 --- a/config/ardupilot_gps_denied.parm +++ b/config/ardupilot_gps_denied.parm @@ -68,7 +68,7 @@ FS_GCS_ENABLE 0 # ==================== # Logging # ==================== -LOG_BITMASK 176126 +LOG_BITMASK 0 LOG_DISARMED 0 # ==================== @@ -76,3 +76,16 @@ LOG_DISARMED 0 # ==================== LAND_SPEED 150 # Final descent cm/s (default 50) LAND_SPEED_HIGH 250 # Initial descent cm/s (default 0=WPNAV_SPEED_DN) + +# ==================== +# SITL Fast Startup +# ==================== +# These MUST be in the .parm file (loaded at boot), not set via MAVLink +GPS_HDOP_GOOD 250 # Accept worse GPS quality (default 140) +EK3_CHECK_SCALE 200 # Relax EKF health checks (default 100) +EK3_GPS_CHECK 0 # Disable GPS preflight checks entirely in SITL +EK3_POSNE_M_NSE 0.5 # Trust GPS position more (default 0.5) +EK3_VELD_M_NSE 0.2 # Trust GPS vertical vel more (default 0.4) +EK3_VELNE_M_NSE 0.1 # Trust GPS horizontal vel more (default 0.3) +INS_GYRO_CAL 0 # Skip gyro calibration on boot (SITL only) +AHRS_GPS_MINSATS 6 # Minimum satellites diff --git a/config/search.yaml b/config/search.yaml index 33dda45..12cb481 100644 --- a/config/search.yaml +++ b/config/search.yaml @@ -8,7 +8,8 @@ altitude: 4.0 # Search altitude (meters) marker: dictionary: DICT_4X4_50 size: 0.5 # Physical marker size in meters - landing_ids: [0] # Marker IDs that trigger landing + landing_ids: [0] # Marker IDs that trigger landing (on UGV) + target_ids: [1] # Marker IDs to find and report to UGV # ── Search Patterns ────────────────────────────────────────── spiral: diff --git a/config/ugv.yaml b/config/ugv.yaml index 84479e8..b12d042 100644 --- a/config/ugv.yaml +++ b/config/ugv.yaml @@ -2,7 +2,12 @@ # UGV-specific settings. Mission config is in search.yaml. # UGV spawn position in local NED (meters) -# Also used by run_autonomous.sh to place the UGV model in Gazebo +# The UAV starts on top of this — run_autonomous.sh syncs both position: - x: 5.0 - y: 5.0 + x: 0.0 + y: 0.0 + +# Gazebo transport topics +topics: + cmd_vel: /ugv/cmd_vel + odometry: /ugv/odometry diff --git a/models/custom_ugv/model.sdf b/models/custom_ugv/model.sdf index f2b0398..0ebaa7f 100644 --- a/models/custom_ugv/model.sdf +++ b/models/custom_ugv/model.sdf @@ -1,159 +1,161 @@ - + false - + - 0 0 0.1 0 0 0 + 0 0 0.12 0 0 0 - 5.0 + 80.0 - 0.100 - 0.1500.1 + 3.000 + 4.003.0 - 0.4 0.3 0.15 - - - 0.4 0.3 0.15 - 0.3 0.3 0.8 1 - - - - - 0 0.175 0.05 -1.5708 0 0 - - 0.5 - - 0.00100 - 0.00100.001 - - - - 0.050.04 + 0.8 0.6 0.12 - 1.01.0 + + 10.010.0 + - 0.050.04 - 0.1 0.1 0.1 1 + 0.8 0.6 0.12 + 0.2 0.2 0.7 10.2 0.2 0.8 1 - - - 0 -0.175 0.05 -1.5708 0 0 + + + 0.25 0.35 0.08 -1.5708 0 0 - 0.5 + 2.0 - 0.00100 - 0.00100.001 + 0.00500 + 0.00500.008 - 0.050.04 - - 1.01.0 - + 0.080.05 + 5.05.0 - 0.050.04 - 0.1 0.1 0.1 1 + 0.080.05 + 0.15 0.15 0.15 1 - - - -0.15 0 0.025 0 0 0 + + + 0.25 -0.35 0.08 -1.5708 0 0 - 0.1 + 2.0 - 0.000100 - 0.000100.0001 + 0.00500 + 0.00500.008 - 0.025 - - 0.00.0 - + 0.080.05 + 5.05.0 - 0.025 - 0.1 0.1 0.1 1 + 0.080.05 + 0.15 0.15 0.15 1 - - + + + -0.25 0.35 0.08 -1.5708 0 0 + + 2.0 + + 0.00500 + 0.00500.008 + + + + 0.080.05 + 5.05.0 + + + 0.080.05 + 0.15 0.15 0.15 1 + + + + + -0.25 -0.35 0.08 -1.5708 0 0 + + 2.0 + + 0.00500 + 0.00500.008 + + + + 0.080.05 + 5.05.0 + + + 0.080.05 + 0.15 0.15 0.15 1 + + + + + + 0 0 0.185 0 0 0 + + 0.5 0.5 0.005 + + 1 1 1 1 + 1 1 1 1 + tags/land_tag.png + + + + + base_link - left_wheel + aruco_tag + + + + base_link + front_left_wheel 0 0 1 - - + base_link - right_wheel + front_right_wheel 0 0 1 - - + base_link - caster + rear_left_wheel + 0 0 1 - - - 0.22 0 0.15 0 0 0 - - 0.01 - - 0.0000100 - 0.0000100.00001 - - - - 0.02 0.04 0.02 - 0 0 0 1 - - - - 1.57 - 640480R8G8B8 - 0.150 - - 1 - 30 - - - /ugv - image_raw:=camera/forward/image_raw - camera_info:=camera/forward/camera_info - - forward_camera - camera_link - - - - - + base_link - camera_link + rear_right_wheel + 0 0 1 - - - /ugv - left_wheel_joint - right_wheel_joint - 0.35 - 0.1 - 5 - 2.0 - cmd_vel - odom - odom - base_link - true - true - true + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + 0.70 + 0.08 + 1.0 + -1.0 + 2.0 + -2.0 + /ugv/cmd_vel + /ugv/odometry + 20 diff --git a/models/custom_ugv/tags/land_tag.png b/models/custom_ugv/tags/land_tag.png new file mode 100644 index 0000000000000000000000000000000000000000..ef1c6e4c12a45ddcd35a3c7e01c95360f0fca0bc GIT binary patch literal 6159 zcmeAS@N?(olHy`uVBq!ia0y~yU{(NO4xj+TKk3LV3=HDeJY5_^Dj44$+{k;tfZ?#g z&;N_>axIllZLu==4Y2#{aeViNw$N8UJzV zk3(nvcj+G$Px}{Mqq)!E^ZrNPA6sYs7mXj)G8$skN-$09e>AP%qxb`q&}x9_Depm0 zYI~p!MEM;1fJu%u9*8QPKY-F5P}z@D(m*{yH9%zt!|w^yfD-LKAo|n>)FToPRQ6%2 zISVNHjv750AfqW{G|QlsA$t}=N(NvV@{ku&=zz+Qd<{sU11v)-oFRn{s0x&Z6gr^d z>0SV&&;b@tAKD;=4yZ;nhZH)X;%U_A(EtHeOQR_SoNGq246yA2EJN;SL0U4cJ42xj zmg3dWCeO1lXv^lzT4)1iMl`euWUw9D(n-vNHfTl-8Vw^*i+D60QJO`-)jhDA8x1a^ zfSudz;L01=smT)sRsY~j3##S8femUW09y#ftHI5IqvFrPz_mWG8@zVZpwTcIO-DOJ vDa#_G<4mK&Yy&jPI6A6KlTl?zu%2W;d-y`uvd4RvK_mU1u6{1-oD!M\s*true\s*)[^<]*', + r'(ugv\s*)[^<]*', rf'\1{x} {y} 0 0 0 0', sdf, count=1) +# Sync UAV on top of UGV (body top=0.18 + iris legs=0.195 ≈ 0.40) +uav_z = 0.40 +sdf = re.sub( + r'(model://iris_with_gimbal\s*)]*>[^<]*', + rf'\1{x} {y} {uav_z} 0 0 90', + sdf, count=1) with open('$WORLD_FILE', 'w') as f: f.write(sdf) -print(f'[INFO] UGV position synced from config: ({x}, {y})') -" 2>/dev/null || print_info "UGV position sync skipped" +print(f'[INFO] UGV at ({x}, {y}) — UAV on top at z={uav_z}') +" 2>/dev/null || print_info "Position sync skipped" fi print_info "===================================" @@ -136,6 +151,27 @@ cp "$GZ_DEFAULT_GUI" "$GZ_USER_GUI" sed -i 's|true|false|' "$GZ_USER_GUI" sed -i 's|show_again="true"|show_again="false"|' "$GZ_USER_GUI" +cat << 'EOF' >> "$GZ_USER_GUI" + + + false + 300 + 50 + 50 + 50 + floating + false + #777777 + + + + true + true + 4000000 + + +EOF + print_info "[1/4] Starting Gazebo ..." gz sim -v4 -r "$WORLD_FILE" & GZ_PID=$! @@ -193,10 +229,12 @@ print_info " Simulation Running" print_info "===================================" print_info " Gazebo -> ArduPilot SITL (TCP:5760)" print_info " |" -print_info " main.py (pymavlink)" -print_info " |" -print_info " camera_viewer.py (OpenCV)" +print_info " main.py (pymavlink + gz.transport)" +print_info " | |" +print_info " UAV ctrl UGV ctrl (/ugv/cmd_vel)" print_info "" +print_info " UAV starts ON UGV, takes off, searches for" +print_info " target tag, dispatches UGV, lands on UGV" print_info " Search: $SEARCH" print_info " Press Ctrl+C to stop" print_info "===================================" diff --git a/src/control/search.py b/src/control/search.py index ac7ba87..750ec2c 100644 --- a/src/control/search.py +++ b/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 diff --git a/src/control/uav_controller.py b/src/control/uav_controller.py index 2a701ed..3c8f4b1 100644 --- a/src/control/uav_controller.py +++ b/src/control/uav_controller.py @@ -105,9 +105,24 @@ class Controller: name.encode('utf-8'), value, mavutil.mavlink.MAV_PARAM_TYPE_REAL32) - sleep(0.5) + sleep(0.1) self._drain_messages() + def configure_ekf_fast_converge(self): + """Tune EKF parameters for faster initial convergence in SITL.""" + params = { + 'EK3_CHECK_SCALE': 200, # Relax EKF health checks (default 100) + 'EK3_GPS_CHECK': 0, # Disable GPS preflight checks in SITL + 'EK3_POSNE_M_NSE': 0.5, # Trust GPS position more + 'EK3_VELD_M_NSE': 0.2, # Trust GPS vertical vel more (default 0.4) + 'EK3_VELNE_M_NSE': 0.1, # Trust GPS horizontal vel more (default 0.3) + 'INS_GYRO_CAL': 0, # Skip gyro cal on boot (SITL doesn't need it) + 'ARMING_CHECK': 0, # Disable ALL preflight checks (SITL only) + } + for name, value in params.items(): + self.set_param(name, value) + print("[UAV] EKF fast-converge params applied") + def set_mode_guided(self): self.conn.mav.command_long_send( self.conn.target_system, @@ -128,8 +143,33 @@ class Controller: self._drain_messages() print("[UAV] Mode -> GUIDED_NOGPS") - def arm(self, retries: int = 30): + def wait_for_ekf(self, timeout: float = 120.0) -> bool: + """Wait until EKF has actual GPS-fused position estimate.""" + print("[UAV] Waiting for EKF position estimate ...") + t0 = time.time() + while time.time() - t0 < timeout: + self._drain_messages() + msg = self.conn.recv_match(type='EKF_STATUS_REPORT', blocking=True, timeout=2) + if msg is not None: + # bit 0x08 = pos_horiz_abs (actual GPS-fused position) + # bit 0x20 = pred_pos_horiz_abs (predicted, not enough for arming) + pos_horiz_abs = bool(msg.flags & 0x08) + if pos_horiz_abs: + print(f"\n[UAV] EKF has position estimate (flags=0x{msg.flags:04x})") + return True + elapsed = int(time.time() - t0) + print(f"\r[UAV] Waiting for EKF ... {elapsed}s ", end='', flush=True) + print("\n[UAV] EKF wait timed out") + return False + + def arm(self, retries: int = 15): self.set_mode_guided() + + # Wait for EKF before burning through arm attempts + if not self.wait_for_ekf(timeout=120.0): + print("[UAV] Aborting arm: EKF never became healthy") + return False + for i in range(retries): print(f"[UAV] Arm attempt {i+1}/{retries}...") self.conn.mav.command_long_send( @@ -149,7 +189,7 @@ class Controller: self.armed = True print("[UAV] Armed") return True - print(f"[UAV] Arm attempt {i+1} failed, retrying...") + print(f"[UAV] Arm attempt {i+1} failed, retrying in 2s...") time.sleep(2) return False @@ -259,7 +299,8 @@ class Controller: if msg and msg.fix_type >= 3: print(f"\n[UAV] GPS fix: {msg.fix_type} sats: {msg.satellites_visible}") print("[UAV] Waiting for EKF to settle ...") - for _ in range(15): + # Wait briefly; let the arming loop handle the rest + for _ in range(2): self._drain_messages() sleep(1) return True diff --git a/src/control/ugv_controller.py b/src/control/ugv_controller.py index f195c9d..2892ed5 100644 --- a/src/control/ugv_controller.py +++ b/src/control/ugv_controller.py @@ -1,94 +1,149 @@ #!/usr/bin/env python3 +"""UGV controller using Gazebo transport (gz.transport13). + +Publishes Twist to /ugv/cmd_vel, subscribes to /ugv/odometry for +position feedback. Drives a proportional turn-then-drive controller. +""" import math -import numpy as np -from time import sleep, perf_counter -from typing import Tuple, Optional +import time +import threading +from time import sleep + +from gz.transport13 import Node +from gz.msgs10.twist_pb2 import Twist +from gz.msgs10.odometry_pb2 import Odometry -try: - from pymavlink import mavutil - import pymavlink.dialects.v20.ardupilotmega as mavlink - from pymavlink.dialects.v20.ardupilotmega import ( - MAVLink_set_position_target_local_ned_message, - MAV_FRAME_BODY_OFFSET_NED, - MAV_FRAME_LOCAL_NED, - ) - HAS_MAVLINK = True -except ImportError: - HAS_MAVLINK = False MAX_LINEAR_VEL = 1.0 -MAX_ANGULAR_VEL = 1.5 -POSITION_TOL = 0.3 +MAX_ANGULAR_VEL = 2.0 +POSITION_TOL = 0.5 +YAW_TOL = 0.15 class UGVController: - - def __init__(self, connection_string: Optional[str] = None, - static_pos: Tuple[float, float] = (10.0, 5.0)): - self.conn = None - self.backend = 'passive' - self.position = {'x': static_pos[0], 'y': static_pos[1], 'z': 0.0} + def __init__(self, cmd_topic="/ugv/cmd_vel", odom_topic="/ugv/odometry"): + self.node = Node() + self.pub = self.node.advertise(cmd_topic, Twist) + self.position = {'x': 0.0, 'y': 0.0, 'z': 0.0} self.yaw = 0.0 + self._lock = threading.Lock() + self._driving = False + self._drive_thread = None - if connection_string and HAS_MAVLINK: - try: - print(f"[UGV] Connecting via pymavlink: {connection_string}") - self.conn = mavutil.mavlink_connection(connection_string) - self.conn.wait_heartbeat(timeout=10) - print("[UGV] Heartbeat received (system %u component %u)" % - (self.conn.target_system, self.conn.target_component)) - self.backend = 'mavlink' - except Exception as e: - print(f"[UGV] MAVLink connection failed: {e}") - self.conn = None + ok = self.node.subscribe(Odometry, odom_topic, self._odom_cb) + if ok: + print(f"[UGV] Subscribed to {odom_topic}") + else: + print(f"[UGV] WARNING: Failed to subscribe to {odom_topic}") - if self.backend == 'passive': - print(f"[UGV] Passive mode - static at ({static_pos[0]}, {static_pos[1]})") + print(f"[UGV] Publishing commands to {cmd_topic}") - def update_state(self): - if self.backend == 'mavlink' and self.conn: - while True: - msg = self.conn.recv_match(blocking=False) - if msg is None: - break - mtype = msg.get_type() - if mtype == 'LOCAL_POSITION_NED': - self.position = {'x': msg.x, 'y': msg.y, 'z': msg.z} - elif mtype == 'ATTITUDE': - self.yaw = msg.yaw + def _odom_cb(self, msg): + with self._lock: + self.position['x'] = msg.pose.position.x + self.position['y'] = msg.pose.position.y + self.position['z'] = msg.pose.position.z + q = msg.pose.orientation + siny_cosp = 2.0 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z) + self.yaw = math.atan2(siny_cosp, cosy_cosp) - def get_position(self) -> dict: - self.update_state() - return self.position.copy() + def get_position(self): + with self._lock: + return self.position.copy() - def move_to(self, x: float, y: float): - if self.backend == 'mavlink' and self.conn: - move_msg = MAVLink_set_position_target_local_ned_message( - int(perf_counter() * 1000), - self.conn.target_system, - self.conn.target_component, - MAV_FRAME_LOCAL_NED, - 3576, x, y, 0, - 0, 0, 0, 0, 0, 0, 0, 0) - self.conn.mav.send(move_msg) - print(f"[UGV] Moving to ({x:.1f}, {y:.1f})") + def get_yaw(self): + with self._lock: + return self.yaw - def send_velocity(self, vx: float, vy: float, yaw_rate: float = 0.0): - if self.backend == 'mavlink' and self.conn: - move_msg = MAVLink_set_position_target_local_ned_message( - int(perf_counter() * 1000), - self.conn.target_system, - self.conn.target_component, - MAV_FRAME_BODY_OFFSET_NED, - 3527, 0, 0, 0, - vx, vy, 0, 0, 0, 0, 0, yaw_rate) - self.conn.mav.send(move_msg) + 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.angular.z = max(-MAX_ANGULAR_VEL, min(MAX_ANGULAR_VEL, angular_z)) + self.pub.publish(msg) def stop(self): - self.send_velocity(0, 0, 0) + self.send_cmd_vel(0.0, 0.0) - def distance_to(self, x: float, y: float) -> float: - dx = x - self.position['x'] - dy = y - self.position['y'] + def distance_to(self, x, y): + pos = self.get_position() + dx = x - pos['x'] + dy = y - pos['y'] return math.sqrt(dx**2 + dy**2) + + def drive_to(self, target_x, target_y, callback=None): + """Start driving to (target_x, target_y) in a background thread.""" + if self._driving: + print("[UGV] Already driving, ignoring new target") + return + self._driving = True + self._drive_thread = threading.Thread( + target=self._drive_loop, + args=(target_x, target_y, callback), + daemon=True + ) + self._drive_thread.start() + + def _drive_loop(self, target_x, target_y, callback): + print(f"[UGV] Driving to ({target_x:.1f}, {target_y:.1f})") + t0 = time.time() + timeout = 120.0 + + while self._driving and (time.time() - t0) < timeout: + pos = self.get_position() + yaw = self.get_yaw() + dx = target_x - pos['x'] + dy = target_y - pos['y'] + dist = math.sqrt(dx**2 + dy**2) + + if dist < POSITION_TOL: + self.stop() + elapsed = int(time.time() - t0) + print(f"\n[UGV] Arrived at ({target_x:.1f}, {target_y:.1f}) " + f"in {elapsed}s") + self._driving = False + if callback: + callback(target_x, target_y) + return + + target_yaw = math.atan2(dy, dx) + yaw_error = target_yaw - yaw + while yaw_error > math.pi: + yaw_error -= 2 * math.pi + while yaw_error < -math.pi: + yaw_error += 2 * math.pi + + 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 + else: + linear = min(MAX_LINEAR_VEL, dist * 0.5) + angular = yaw_error * 1.0 + + self.send_cmd_vel(linear, angular) + + elapsed = int(time.time() - t0) + print(f"\r[UGV] dist:{dist:.1f}m yaw_err:" + f"{math.degrees(yaw_error):+.0f}° " + f"v:{linear:.2f} ω:{angular:.2f} ({elapsed}s) ", + end='', flush=True) + + sleep(0.1) + + self.stop() + self._driving = False + print(f"\n[UGV] Drive timeout or stopped") + + def cancel_drive(self): + self._driving = False + self.stop() + + @property + def is_driving(self): + return self._driving + + @property + def has_arrived(self): + return not self._driving and self._drive_thread is not None diff --git a/src/localization/__init__.py b/src/localization/__init__.py deleted file mode 100644 index bc24145..0000000 --- a/src/localization/__init__.py +++ /dev/null @@ -1 +0,0 @@ -"""Localization module for sensor fusion and position estimation.""" diff --git a/src/localization/ekf_fusion.py b/src/localization/ekf_fusion.py deleted file mode 100644 index 293ce7e..0000000 --- a/src/localization/ekf_fusion.py +++ /dev/null @@ -1,202 +0,0 @@ -#!/usr/bin/env python3 -"""EKF Fusion - Extended Kalman Filter for sensor fusion.""" - -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped -from sensor_msgs.msg import Imu -from nav_msgs.msg import Odometry -import numpy as np -from filterpy.kalman import ExtendedKalmanFilter - - -class EKFFusionNode(Node): - - def __init__(self): - super().__init__('ekf_fusion_node') - - self.declare_parameter('process_noise_pos', 0.1) - self.declare_parameter('process_noise_vel', 0.5) - self.declare_parameter('measurement_noise_vo', 0.05) - self.declare_parameter('measurement_noise_of', 0.1) - self.declare_parameter('update_rate', 50.0) - - self.ekf = ExtendedKalmanFilter(dim_x=9, dim_z=6) - - self.ekf.x = np.zeros(9) - - self.ekf.P = np.eye(9) * 1.0 - - process_noise_pos = self.get_parameter('process_noise_pos').value - process_noise_vel = self.get_parameter('process_noise_vel').value - - self.ekf.Q = np.diag([ - process_noise_pos, process_noise_pos, process_noise_pos, - process_noise_vel, process_noise_vel, process_noise_vel, - 0.1, 0.1, 0.1 - ]) - - meas_noise_vo = self.get_parameter('measurement_noise_vo').value - meas_noise_of = self.get_parameter('measurement_noise_of').value - - self.ekf.R = np.diag([ - meas_noise_vo, meas_noise_vo, meas_noise_vo, - meas_noise_of, meas_noise_of, meas_noise_of - ]) - - self.prev_time = None - self.orientation = np.array([0.0, 0.0, 0.0, 1.0]) - - self.vo_sub = self.create_subscription( - PoseStamped, '/uav/visual_odometry/pose', self.vo_callback, 10) - - self.of_sub = self.create_subscription( - TwistStamped, '/uav/optical_flow/velocity', self.of_callback, 10) - - self.imu_sub = self.create_subscription( - Imu, '/uav/imu/data', self.imu_callback, 10) - - self.pose_pub = self.create_publisher( - PoseWithCovarianceStamped, '/uav/ekf/pose', 10) - - self.odom_pub = self.create_publisher( - Odometry, '/uav/ekf/odom', 10) - - update_rate = self.get_parameter('update_rate').value - self.timer = self.create_timer(1.0 / update_rate, self.predict_step) - - self.get_logger().info('EKF Fusion Node Started') - - def state_transition(self, x, dt): - F = np.eye(9) - F[0, 3] = dt - F[1, 4] = dt - F[2, 5] = dt - F[3, 6] = dt - F[4, 7] = dt - F[5, 8] = dt - return F @ x - - def jacobian_F(self, dt): - F = np.eye(9) - F[0, 3] = dt - F[1, 4] = dt - F[2, 5] = dt - F[3, 6] = dt - F[4, 7] = dt - F[5, 8] = dt - return F - - def predict_step(self): - current_time = self.get_clock().now() - - if self.prev_time is not None: - dt = (current_time.nanoseconds - self.prev_time.nanoseconds) / 1e9 - if dt <= 0 or dt > 1.0: - dt = 0.02 - else: - dt = 0.02 - - self.prev_time = current_time - - F = self.jacobian_F(dt) - self.ekf.F = F - self.ekf.predict() - - self.publish_state() - - def vo_callback(self, msg): - z = np.array([ - msg.pose.position.x, - msg.pose.position.y, - msg.pose.position.z, - 0.0, 0.0, 0.0 - ]) - - self.orientation = np.array([ - msg.pose.orientation.x, - msg.pose.orientation.y, - msg.pose.orientation.z, - msg.pose.orientation.w - ]) - - H = np.zeros((6, 9)) - H[0, 0] = 1.0 - H[1, 1] = 1.0 - H[2, 2] = 1.0 - - self.ekf.update(z, HJacobian=lambda x: H, Hx=lambda x: H @ x) - - def of_callback(self, msg): - z = np.array([ - self.ekf.x[0], - self.ekf.x[1], - self.ekf.x[2], - msg.twist.linear.x, - msg.twist.linear.y, - 0.0 - ]) - - H = np.zeros((6, 9)) - H[0, 0] = 1.0 - H[1, 1] = 1.0 - H[2, 2] = 1.0 - H[3, 3] = 1.0 - H[4, 4] = 1.0 - - self.ekf.update(z, HJacobian=lambda x: H, Hx=lambda x: H @ x) - - def imu_callback(self, msg): - accel = np.array([ - msg.linear_acceleration.x, - msg.linear_acceleration.y, - msg.linear_acceleration.z - 9.81 - ]) - - self.ekf.x[6:9] = accel - - def publish_state(self): - pose_msg = PoseWithCovarianceStamped() - pose_msg.header.stamp = self.get_clock().now().to_msg() - pose_msg.header.frame_id = 'odom' - - pose_msg.pose.pose.position.x = float(self.ekf.x[0]) - pose_msg.pose.pose.position.y = float(self.ekf.x[1]) - pose_msg.pose.pose.position.z = float(self.ekf.x[2]) - - pose_msg.pose.pose.orientation.x = float(self.orientation[0]) - pose_msg.pose.pose.orientation.y = float(self.orientation[1]) - pose_msg.pose.pose.orientation.z = float(self.orientation[2]) - pose_msg.pose.pose.orientation.w = float(self.orientation[3]) - - cov = self.ekf.P[:6, :6].flatten().tolist() - pose_msg.pose.covariance = cov - - self.pose_pub.publish(pose_msg) - - odom_msg = Odometry() - odom_msg.header = pose_msg.header - odom_msg.child_frame_id = 'base_link' - odom_msg.pose = pose_msg.pose - - odom_msg.twist.twist.linear.x = float(self.ekf.x[3]) - odom_msg.twist.twist.linear.y = float(self.ekf.x[4]) - odom_msg.twist.twist.linear.z = float(self.ekf.x[5]) - - self.odom_pub.publish(odom_msg) - - -def main(args=None): - rclpy.init(args=args) - node = EKFFusionNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/localization/landmark_tracker.py b/src/localization/landmark_tracker.py deleted file mode 100644 index cf036fa..0000000 --- a/src/localization/landmark_tracker.py +++ /dev/null @@ -1,165 +0,0 @@ -#!/usr/bin/env python3 -"""Landmark Tracker - Tracks visual landmarks for position correction.""" - -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import PoseArray, PoseStamped, Point -from visualization_msgs.msg import Marker, MarkerArray -from std_msgs.msg import Float32MultiArray -import numpy as np - - -class LandmarkTracker(Node): - - def __init__(self): - super().__init__('landmark_tracker') - - self.declare_parameter('max_landmarks', 50) - self.declare_parameter('matching_threshold', 0.5) - self.declare_parameter('landmark_timeout', 5.0) - - self.max_landmarks = self.get_parameter('max_landmarks').value - self.matching_threshold = self.get_parameter('matching_threshold').value - self.landmark_timeout = self.get_parameter('landmark_timeout').value - - self.landmarks = {} - self.landmark_id_counter = 0 - - self.current_pose = None - - self.detection_sub = self.create_subscription( - PoseArray, '/uav/landmarks/poses', self.detection_callback, 10) - - self.marker_ids_sub = self.create_subscription( - Float32MultiArray, '/uav/landmarks/ids', self.marker_ids_callback, 10) - - self.pose_sub = self.create_subscription( - PoseStamped, '/uav/local_position/pose', self.pose_callback, 10) - - self.landmark_map_pub = self.create_publisher( - MarkerArray, '/landmarks/map', 10) - - self.position_correction_pub = self.create_publisher( - PoseStamped, '/landmarks/position_correction', 10) - - self.timer = self.create_timer(1.0, self.cleanup_landmarks) - self.viz_timer = self.create_timer(0.5, self.publish_landmark_map) - - self.get_logger().info('Landmark Tracker Started') - - def pose_callback(self, msg): - self.current_pose = msg - - def marker_ids_callback(self, msg): - pass - - def detection_callback(self, msg): - if self.current_pose is None: - return - - robot_pos = np.array([ - self.current_pose.pose.position.x, - self.current_pose.pose.position.y, - self.current_pose.pose.position.z - ]) - - for pose in msg.poses: - local_pos = np.array([pose.position.x, pose.position.y, pose.position.z]) - - global_pos = robot_pos + local_pos - - matched_id = self.match_landmark(global_pos) - - if matched_id is not None: - self.update_landmark(matched_id, global_pos) - else: - self.add_landmark(global_pos) - - def match_landmark(self, position): - for lm_id, lm_data in self.landmarks.items(): - distance = np.linalg.norm(position - lm_data['position']) - if distance < self.matching_threshold: - return lm_id - return None - - def add_landmark(self, position): - if len(self.landmarks) >= self.max_landmarks: - oldest_id = min(self.landmarks.keys(), - key=lambda k: self.landmarks[k]['last_seen']) - del self.landmarks[oldest_id] - - self.landmarks[self.landmark_id_counter] = { - 'position': position.copy(), - 'observations': 1, - 'last_seen': self.get_clock().now(), - 'covariance': np.eye(3) * 0.5 - } - self.landmark_id_counter += 1 - - def update_landmark(self, lm_id, position): - lm = self.landmarks[lm_id] - - alpha = 1.0 / (lm['observations'] + 1) - lm['position'] = (1 - alpha) * lm['position'] + alpha * position - lm['observations'] += 1 - lm['last_seen'] = self.get_clock().now() - lm['covariance'] *= 0.95 - - def cleanup_landmarks(self): - current_time = self.get_clock().now() - expired = [] - - for lm_id, lm_data in self.landmarks.items(): - dt = (current_time - lm_data['last_seen']).nanoseconds / 1e9 - if dt > self.landmark_timeout: - expired.append(lm_id) - - for lm_id in expired: - del self.landmarks[lm_id] - - def publish_landmark_map(self): - marker_array = MarkerArray() - - for lm_id, lm_data in self.landmarks.items(): - marker = Marker() - marker.header.stamp = self.get_clock().now().to_msg() - marker.header.frame_id = 'odom' - marker.ns = 'landmarks' - marker.id = lm_id - marker.type = Marker.SPHERE - marker.action = Marker.ADD - - marker.pose.position.x = float(lm_data['position'][0]) - marker.pose.position.y = float(lm_data['position'][1]) - marker.pose.position.z = float(lm_data['position'][2]) - marker.pose.orientation.w = 1.0 - - scale = 0.1 + 0.02 * min(lm_data['observations'], 20) - marker.scale.x = scale - marker.scale.y = scale - marker.scale.z = scale - - marker.color.r = 0.0 - marker.color.g = 0.8 - marker.color.b = 0.2 - marker.color.a = 0.8 - - marker_array.markers.append(marker) - - self.landmark_map_pub.publish(marker_array) - - -def main(args=None): - rclpy.init(args=args) - node = LandmarkTracker() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/localization/position_estimator.py b/src/localization/position_estimator.py deleted file mode 100644 index 7f8fe25..0000000 --- a/src/localization/position_estimator.py +++ /dev/null @@ -1,188 +0,0 @@ -#!/usr/bin/env python3 -"""Position Estimator - Fuses sensors for local position estimate.""" - -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped -from sensor_msgs.msg import Imu -from nav_msgs.msg import Odometry -import numpy as np -from scipy.spatial.transform import Rotation - - -class PositionEstimator(Node): - - def __init__(self): - super().__init__('position_estimator') - - self.declare_parameter('fusion_method', 'weighted_average') - self.declare_parameter('vo_weight', 0.6) - self.declare_parameter('optical_flow_weight', 0.3) - self.declare_parameter('imu_weight', 0.1) - self.declare_parameter('update_rate', 50.0) - - self.fusion_method = self.get_parameter('fusion_method').value - self.vo_weight = self.get_parameter('vo_weight').value - self.of_weight = self.get_parameter('optical_flow_weight').value - self.imu_weight = self.get_parameter('imu_weight').value - - self.position = np.zeros(3) - self.velocity = np.zeros(3) - self.orientation = np.array([0.0, 0.0, 0.0, 1.0]) - - self.vo_pose = None - self.of_velocity = None - self.imu_data = None - - self.prev_time = None - - self.vo_sub = self.create_subscription( - PoseStamped, '/uav/visual_odometry/pose', self.vo_callback, 10) - - self.of_sub = self.create_subscription( - TwistStamped, '/uav/optical_flow/velocity', self.of_callback, 10) - - self.imu_sub = self.create_subscription( - Imu, '/uav/imu/data', self.imu_callback, 10) - - self.pose_pub = self.create_publisher( - PoseWithCovarianceStamped, '/uav/local_position/pose', 10) - - self.odom_pub = self.create_publisher( - Odometry, '/uav/local_position/odom', 10) - - update_rate = self.get_parameter('update_rate').value - self.timer = self.create_timer(1.0 / update_rate, self.update_estimate) - - self.get_logger().info(f'Position Estimator Started - {self.fusion_method}') - - def vo_callback(self, msg): - self.vo_pose = msg - - def of_callback(self, msg): - self.of_velocity = msg - - def imu_callback(self, msg): - self.imu_data = msg - - def update_estimate(self): - current_time = self.get_clock().now() - - if self.prev_time is not None: - dt = (current_time.nanoseconds - self.prev_time.nanoseconds) / 1e9 - if dt <= 0: - dt = 0.02 - else: - dt = 0.02 - - self.prev_time = current_time - - if self.fusion_method == 'weighted_average': - self.weighted_average_fusion(dt) - else: - self.simple_fusion(dt) - - self.publish_estimate() - - def weighted_average_fusion(self, dt): - total_weight = 0.0 - weighted_pos = np.zeros(3) - weighted_vel = np.zeros(3) - - if self.vo_pose is not None: - vo_pos = np.array([ - self.vo_pose.pose.position.x, - self.vo_pose.pose.position.y, - self.vo_pose.pose.position.z - ]) - weighted_pos += self.vo_weight * vo_pos - total_weight += self.vo_weight - - self.orientation = np.array([ - self.vo_pose.pose.orientation.x, - self.vo_pose.pose.orientation.y, - self.vo_pose.pose.orientation.z, - self.vo_pose.pose.orientation.w - ]) - - if self.of_velocity is not None: - of_vel = np.array([ - self.of_velocity.twist.linear.x, - self.of_velocity.twist.linear.y, - 0.0 - ]) - weighted_vel += self.of_weight * of_vel - - if self.imu_data is not None: - imu_accel = np.array([ - self.imu_data.linear_acceleration.x, - self.imu_data.linear_acceleration.y, - self.imu_data.linear_acceleration.z - 9.81 - ]) - self.velocity += imu_accel * dt * self.imu_weight - - if total_weight > 0: - self.position = weighted_pos / total_weight - - if self.of_velocity is not None: - self.velocity = weighted_vel - - def simple_fusion(self, dt): - if self.vo_pose is not None: - self.position = np.array([ - self.vo_pose.pose.position.x, - self.vo_pose.pose.position.y, - self.vo_pose.pose.position.z - ]) - - def publish_estimate(self): - pose_msg = PoseWithCovarianceStamped() - pose_msg.header.stamp = self.get_clock().now().to_msg() - pose_msg.header.frame_id = 'odom' - - pose_msg.pose.pose.position.x = float(self.position[0]) - pose_msg.pose.pose.position.y = float(self.position[1]) - pose_msg.pose.pose.position.z = float(self.position[2]) - - pose_msg.pose.pose.orientation.x = float(self.orientation[0]) - pose_msg.pose.pose.orientation.y = float(self.orientation[1]) - pose_msg.pose.pose.orientation.z = float(self.orientation[2]) - pose_msg.pose.pose.orientation.w = float(self.orientation[3]) - - pose_msg.pose.covariance = [ - 0.1, 0, 0, 0, 0, 0, - 0, 0.1, 0, 0, 0, 0, - 0, 0, 0.1, 0, 0, 0, - 0, 0, 0, 0.05, 0, 0, - 0, 0, 0, 0, 0.05, 0, - 0, 0, 0, 0, 0, 0.05 - ] - - self.pose_pub.publish(pose_msg) - - odom_msg = Odometry() - odom_msg.header = pose_msg.header - odom_msg.child_frame_id = 'base_link' - odom_msg.pose = pose_msg.pose - - odom_msg.twist.twist.linear.x = float(self.velocity[0]) - odom_msg.twist.twist.linear.y = float(self.velocity[1]) - odom_msg.twist.twist.linear.z = float(self.velocity[2]) - - self.odom_pub.publish(odom_msg) - - -def main(args=None): - rclpy.init(args=args) - node = PositionEstimator() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/main.py b/src/main.py index ad80436..98fd485 100644 --- a/src/main.py +++ b/src/main.py @@ -12,6 +12,7 @@ sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) from control.uav_controller import Controller from control.search import Search +from control.ugv_controller import UGVController from vision.object_detector import ObjectDetector from vision.camera_processor import CameraProcessor from navigation.flight_tracker import FlightTracker @@ -21,6 +22,19 @@ PROJECT_DIR = Path(__file__).resolve().parent.parent CONFIG_DIR = PROJECT_DIR / "config" +# ── Coordinate frame conversion ────────────────────────────── +# ArduPilot LOCAL_POSITION_NED: X=North, Y=East, Z=Down +# Gazebo Harmonic (ENU default): X=East, Y=North, Z=Up +# The axes are swapped in the horizontal plane. +def ned_to_gz(ned_x, ned_y): + """Convert ArduPilot NED (North,East) → Gazebo (East,North).""" + return ned_y, ned_x + +def gz_to_ned(gz_x, gz_y): + """Convert Gazebo (East,North) → ArduPilot NED (North,East).""" + return gz_y, gz_x + + def load_config(name: str) -> dict: path = CONFIG_DIR / name if not path.exists(): @@ -81,9 +95,14 @@ def main(): marker_dict = marker_cfg.get('dictionary', 'DICT_4X4_50') marker_size = marker_cfg.get('size', 0.5) landing_ids = marker_cfg.get('landing_ids', [0]) + target_ids = marker_cfg.get('target_ids', []) ugv_pos_cfg = ugv_cfg.get('position', {}) - ugv_position = (ugv_pos_cfg.get('x', 5.0), ugv_pos_cfg.get('y', 5.0)) + ugv_position = (ugv_pos_cfg.get('x', 0.0), ugv_pos_cfg.get('y', 0.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') if args.connection: conn_str = args.connection @@ -94,6 +113,8 @@ def main(): ctrl = Controller(conn_str) + ugv = UGVController(cmd_topic=ugv_cmd_topic, odom_topic=ugv_odom_topic) + detector = ObjectDetector( marker_size=marker_size, aruco_dict_name=marker_dict, @@ -117,7 +138,7 @@ def main(): print(f"[MAIN] Camera unavailable: {e}") camera = None - actions = {'land': landing_ids} + actions = {'land': landing_ids, 'target': target_ids} search = Search(ctrl, detector, camera=camera, mode=search_mode, altitude=altitude, actions=actions) @@ -137,7 +158,9 @@ def main(): print(f"[MAIN] Config loaded from {CONFIG_DIR}") print(f"[MAIN] Search: {search_mode} Altitude: {altitude}m") - print(f"[MAIN] Marker: {marker_dict} size:{marker_size}m land:{landing_ids}") + print(f"[MAIN] Marker: {marker_dict} size:{marker_size}m " + f"land:{landing_ids} target:{target_ids}") + print(f"[MAIN] UAV starts on UGV at ({ugv_position[0]}, {ugv_position[1]})") tracker = FlightTracker( window_size=600, @@ -148,7 +171,10 @@ def main(): tracker.set_plan(waypoints) tracker.start() + ctrl.configure_ekf_fast_converge() setup_ardupilot(ctrl) + if recorder: + recorder.set_phase("gps_wait") ctrl.wait_for_gps() if not ctrl.arm(): @@ -158,13 +184,18 @@ def main(): recorder.stop() return + if recorder: + recorder.set_phase("takeoff") + ctrl.takeoff(altitude) ctrl.wait_altitude(altitude, tolerance=1.0, timeout=30) if recorder: + recorder.set_phase("search") recorder.start_recording(tracker=tracker, camera=camera) recorder.snapshot_camera("pre_search") + # --- Wire tracker updates into search loop --- original_check = search.check_for_markers def tracked_check(): result = original_check() @@ -173,14 +204,45 @@ def main(): tracker.update_uav(pos['x'], pos['y'], altitude=ctrl.altitude, heading=ctrl.current_yaw) + # Convert UGV Gazebo odom → NED for the tracker + ugv_pos = ugv.get_position() + ugv_ned_x, ugv_ned_y = gz_to_ned(ugv_pos['x'], ugv_pos['y']) + tracker.update_ugv(ugv_ned_x, ugv_ned_y) + # Log position data + if recorder: + recorder.log_position( + uav_x=pos['x'], uav_y=pos['y'], + uav_alt=ctrl.altitude, uav_heading=ctrl.current_yaw, + ugv_x=ugv_ned_x, ugv_y=ugv_ned_y) for mid, info in search.found_markers.items(): p = info['uav_position'] tracker.add_marker(mid, p['x'], p['y']) return result search.check_for_markers = tracked_check - search._on_waypoint_change = lambda idx: tracker.set_active_waypoint(idx) + # --- Target found → dispatch UGV --- + # Shared state: set by callback, read by post-search logic + ugv_target_pos = [None, None] # [x, y] — set when target found + + def on_target_found(marker_id, x, y): + # x, y are in NED (North, East) from the UAV + # Convert to Gazebo frame for the UGV + gz_x, gz_y = ned_to_gz(x, y) + ugv_target_pos[0] = gz_x + ugv_target_pos[1] = gz_y + print(f"\n[MAIN] Target ID:{marker_id} found at NED({x:.1f}, {y:.1f})") + print(f"[MAIN] Dispatching UGV to Gazebo({gz_x:.1f}, {gz_y:.1f}) ...") + if recorder: + recorder.set_phase("ugv_dispatch") + recorder.snapshot_camera("target_found") + recorder.snapshot_tracker("target_found") + ugv.drive_to(gz_x, gz_y) + search.stop() + + search.on_target_found = on_target_found + + # --- Run search (drone searches for target tag ID 1) --- results = search.run() search_results = search.get_results() @@ -196,38 +258,113 @@ def main(): pos = info['uav_position'] print(f" ID:{marker_id} at ({pos['x']:.1f}, {pos['y']:.1f}) " f"distance:{info['distance']:.2f}m") - if search_results.get('landed'): - print(f" Landed on target: YES") print(f"{'=' * 50}\n") - if not search_results.get('landed'): - ctrl.land() + # --- Wait for UGV to arrive, THEN land on it --- + if ugv_target_pos[0] is not None: + print("[MAIN] Waiting for UGV to arrive at target ...") + if recorder: + recorder.set_phase("wait_ugv") + while ugv.is_driving: + ugv_pos = ugv.get_position() + ugv_ned_x, ugv_ned_y = gz_to_ned(ugv_pos['x'], ugv_pos['y']) + ctrl.update_state() + pos = ctrl.get_local_position() + tracker.update_uav(pos['x'], pos['y'], + altitude=ctrl.altitude, + heading=ctrl.current_yaw) + tracker.update_ugv(ugv_ned_x, ugv_ned_y) + sleep(0.5) + + # UGV has arrived — fly to above UGV, then land + ugv_pos = ugv.get_position() + ugv_ned_x, ugv_ned_y = gz_to_ned(ugv_pos['x'], ugv_pos['y']) + print(f"\n[MAIN] UGV arrived at Gazebo({ugv_pos['x']:.1f}, {ugv_pos['y']:.1f})") + print(f"[MAIN] Flying UAV to NED({ugv_ned_x:.1f}, {ugv_ned_y:.1f}) ...") + + if recorder: + recorder.set_phase("fly_to_ugv") + + ctrl.move_local_ned(ugv_ned_x, ugv_ned_y, -altitude) + # Wait until UAV is roughly above UGV + for _ in range(60): + ctrl.update_state() + pos = ctrl.get_local_position() + dx = ugv_ned_x - pos['x'] + dy = ugv_ned_y - pos['y'] + dist = (dx**2 + dy**2) ** 0.5 + tracker.update_uav(pos['x'], pos['y'], + altitude=ctrl.altitude, + heading=ctrl.current_yaw) + if dist < 1.5: + break + sleep(0.5) + + print(f"[MAIN] Above UGV — initiating landing") + if recorder: + recorder.set_phase("landing") + + search.landing_enabled = True + search.running = True + + # Attempt visual servoing landing + for _ in range(20): + search.check_for_markers() + if search.landed: + break + sleep(0.5) + + if not search.landed: + ctrl.land() wait_for_landing(ctrl) else: - wait_for_landing(ctrl) + # No target found, just land in place + if recorder: + recorder.set_phase("landing") + if not search_results.get('landed'): + ctrl.land() + wait_for_landing(ctrl) + else: + wait_for_landing(ctrl) if recorder: + recorder.set_phase("complete") recorder.snapshot_camera("post_landing") + recorder.snapshot_tracker("final") + ugv_target_info = None + if ugv_target_pos[0] is not None: + ugv_target_info = { + "gz_x": round(ugv_target_pos[0], 2), + "gz_y": round(ugv_target_pos[1], 2), + } recorder.save_summary( search_mode=search_mode, altitude=altitude, markers=results, - landed=search_results.get('landed', False), + landed=True, + ugv_dispatched=ugv_target_pos[0] is not None, + ugv_target=ugv_target_info, ) recorder.stop() print(f"\n{'=' * 50}") print(f" Simulation finished.") print(f" Tracker and Gazebo windows are still open.") + if ugv.is_driving: + print(f" UGV is still driving to target...") print(f" Press Ctrl+C to exit.") print(f"{'=' * 50}\n") try: while True: + ugv_pos = ugv.get_position() + ugv_ned_x, ugv_ned_y = gz_to_ned(ugv_pos['x'], ugv_pos['y']) + tracker.update_ugv(ugv_ned_x, ugv_ned_y) sleep(1) except KeyboardInterrupt: pass finally: + ugv.cancel_drive() tracker.stop() print("[MAIN] Done.") diff --git a/src/nodes/__init__.py b/src/nodes/__init__.py deleted file mode 100644 index 2c805ab..0000000 --- a/src/nodes/__init__.py +++ /dev/null @@ -1 +0,0 @@ -"""ROS 2 Node implementations.""" diff --git a/src/nodes/geofence_node.py b/src/nodes/geofence_node.py deleted file mode 100644 index a41194e..0000000 --- a/src/nodes/geofence_node.py +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python3 -"""Geofence Node - ROS 2 wrapper.""" - -from src.safety.geofence_monitor import GeofenceMonitor, main - -if __name__ == '__main__': - main() diff --git a/src/nodes/multi_vehicle_coord.py b/src/nodes/multi_vehicle_coord.py deleted file mode 100644 index 59f6c4b..0000000 --- a/src/nodes/multi_vehicle_coord.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python3 -"""Multi-Vehicle Coordination Node.""" - -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import PoseStamped, TwistStamped -from std_msgs.msg import String, Bool -import numpy as np - - -class MultiVehicleCoordinator(Node): - - def __init__(self): - super().__init__('multi_vehicle_coord') - - self.declare_parameter('min_separation', 3.0) - self.declare_parameter('coordination_mode', 'leader_follower') - - self.min_separation = self.get_parameter('min_separation').value - self.coord_mode = self.get_parameter('coordination_mode').value - - self.uav_pose = None - self.ugv_pose = None - - self.uav_pose_sub = self.create_subscription( - PoseStamped, '/uav/local_position/pose', self.uav_pose_callback, 10) - - self.ugv_pose_sub = self.create_subscription( - PoseStamped, '/ugv/local_position/pose', self.ugv_pose_callback, 10) - - self.uav_cmd_pub = self.create_publisher(String, '/uav/controller/command', 10) - self.ugv_cmd_pub = self.create_publisher(String, '/ugv/controller/command', 10) - self.uav_goal_pub = self.create_publisher(PoseStamped, '/uav/setpoint_position', 10) - self.ugv_goal_pub = self.create_publisher(PoseStamped, '/ugv/goal_pose', 10) - - self.collision_warning_pub = self.create_publisher(Bool, '/coordination/collision_warning', 10) - self.status_pub = self.create_publisher(String, '/coordination/status', 10) - - self.timer = self.create_timer(0.1, self.coordination_loop) - - self.get_logger().info(f'Multi-Vehicle Coordinator Started - Mode: {self.coord_mode}') - - def uav_pose_callback(self, msg): - self.uav_pose = msg - - def ugv_pose_callback(self, msg): - self.ugv_pose = msg - - def coordination_loop(self): - if self.uav_pose is None or self.ugv_pose is None: - return - - uav_pos = np.array([ - self.uav_pose.pose.position.x, - self.uav_pose.pose.position.y, - self.uav_pose.pose.position.z - ]) - - ugv_pos = np.array([ - self.ugv_pose.pose.position.x, - self.ugv_pose.pose.position.y, - 0.0 - ]) - - horizontal_distance = np.linalg.norm(uav_pos[:2] - ugv_pos[:2]) - - collision_warning = Bool() - collision_warning.data = horizontal_distance < self.min_separation - self.collision_warning_pub.publish(collision_warning) - - if collision_warning.data: - self.get_logger().warn(f'Vehicles too close: {horizontal_distance:.2f}m') - - status = String() - status.data = f'separation:{horizontal_distance:.2f}|mode:{self.coord_mode}' - self.status_pub.publish(status) - - if self.coord_mode == 'leader_follower': - self.leader_follower_control(uav_pos, ugv_pos) - elif self.coord_mode == 'formation': - self.formation_control(uav_pos, ugv_pos) - - def leader_follower_control(self, uav_pos, ugv_pos): - offset = np.array([0.0, 0.0, 5.0]) - target_uav_pos = ugv_pos + offset - - goal = PoseStamped() - goal.header.stamp = self.get_clock().now().to_msg() - goal.header.frame_id = 'odom' - goal.pose.position.x = float(target_uav_pos[0]) - goal.pose.position.y = float(target_uav_pos[1]) - goal.pose.position.z = float(target_uav_pos[2]) - goal.pose.orientation.w = 1.0 - - def formation_control(self, uav_pos, ugv_pos): - pass - - -def main(args=None): - rclpy.init(args=args) - node = MultiVehicleCoordinator() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/nodes/vision_nav_node.py b/src/nodes/vision_nav_node.py deleted file mode 100644 index b69005a..0000000 --- a/src/nodes/vision_nav_node.py +++ /dev/null @@ -1,134 +0,0 @@ -#!/usr/bin/env python3 -"""Vision-Based Navigation Node.""" - -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image -from geometry_msgs.msg import PoseStamped, TwistStamped -from mavros_msgs.srv import CommandBool, SetMode -from mavros_msgs.msg import State -from cv_bridge import CvBridge -import numpy as np - - -class VisionNavNode(Node): - - def __init__(self): - super().__init__('vision_nav_node') - - self.bridge = CvBridge() - - self.current_image = None - self.current_local_pose = None - self.current_vo_pose = None - self.is_armed = False - self.current_mode = "" - - self.waypoints = [ - {'x': 0.0, 'y': 0.0, 'z': 5.0}, - {'x': 10.0, 'y': 0.0, 'z': 5.0}, - {'x': 10.0, 'y': 10.0, 'z': 5.0}, - {'x': 0.0, 'y': 10.0, 'z': 5.0}, - {'x': 0.0, 'y': 0.0, 'z': 5.0}, - {'x': 0.0, 'y': 0.0, 'z': 0.0}, - ] - - self.current_waypoint_idx = 0 - self.waypoint_tolerance = 0.5 - - self.image_sub = self.create_subscription( - Image, '/uav/camera/forward/image_raw', self.image_callback, 10) - - self.local_pose_sub = self.create_subscription( - PoseStamped, '/uav/mavros/local_position/pose', self.local_pose_callback, 10) - - self.vo_pose_sub = self.create_subscription( - PoseStamped, '/uav/visual_odometry/pose', self.vo_pose_callback, 10) - - self.state_sub = self.create_subscription( - State, '/uav/mavros/state', self.state_callback, 10) - - self.setpoint_local_pub = self.create_publisher( - PoseStamped, '/uav/mavros/setpoint_position/local', 10) - - self.setpoint_vel_pub = self.create_publisher( - TwistStamped, '/uav/mavros/setpoint_velocity/cmd_vel', 10) - - self.arming_client = self.create_client(CommandBool, '/uav/mavros/cmd/arming') - self.set_mode_client = self.create_client(SetMode, '/uav/mavros/set_mode') - - self.control_timer = self.create_timer(0.05, self.control_loop) - self.setpoint_timer = self.create_timer(0.1, self.publish_setpoint) - - self.get_logger().info('Vision Navigation Node Started - GPS-DENIED mode') - - def image_callback(self, msg): - try: - self.current_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') - except Exception as e: - self.get_logger().error(f'Image conversion failed: {e}') - - def local_pose_callback(self, msg): - self.current_local_pose = msg - - def vo_pose_callback(self, msg): - self.current_vo_pose = msg - - def state_callback(self, msg): - self.is_armed = msg.armed - self.current_mode = msg.mode - - def publish_setpoint(self): - if self.current_waypoint_idx >= len(self.waypoints): - return - - waypoint = self.waypoints[self.current_waypoint_idx] - - setpoint = PoseStamped() - setpoint.header.stamp = self.get_clock().now().to_msg() - setpoint.header.frame_id = 'map' - - setpoint.pose.position.x = waypoint['x'] - setpoint.pose.position.y = waypoint['y'] - setpoint.pose.position.z = waypoint['z'] - setpoint.pose.orientation.w = 1.0 - - self.setpoint_local_pub.publish(setpoint) - - def control_loop(self): - if self.current_local_pose is None: - return - - if self.current_waypoint_idx < len(self.waypoints): - waypoint = self.waypoints[self.current_waypoint_idx] - - dx = waypoint['x'] - self.current_local_pose.pose.position.x - dy = waypoint['y'] - self.current_local_pose.pose.position.y - dz = waypoint['z'] - self.current_local_pose.pose.position.z - - distance = np.sqrt(dx**2 + dy**2 + dz**2) - - if distance < self.waypoint_tolerance: - self.get_logger().info( - f'Reached waypoint {self.current_waypoint_idx}: ' - f'[{waypoint["x"]}, {waypoint["y"]}, {waypoint["z"]}] (LOCAL coordinates)') - self.current_waypoint_idx += 1 - - if self.current_waypoint_idx >= len(self.waypoints): - self.get_logger().info('Mission complete!') - - -def main(args=None): - rclpy.init(args=args) - node = VisionNavNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/nodes/visual_odom_node.py b/src/nodes/visual_odom_node.py deleted file mode 100644 index 7f8bbe6..0000000 --- a/src/nodes/visual_odom_node.py +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python3 -"""Visual Odometry Node - ROS 2 wrapper.""" - -from src.vision.visual_odometry import VisualOdometryNode, main - -if __name__ == '__main__': - main() diff --git a/src/utils/recorder.py b/src/utils/recorder.py index 1954bdf..30c54a6 100644 --- a/src/utils/recorder.py +++ b/src/utils/recorder.py @@ -1,21 +1,37 @@ #!/usr/bin/env python3 +"""Run recorder — captures simulation video and logs to a results folder. + +Each run gets a folder like results/run_1/ containing: + - log.txt Process logs (stdout/stderr) + - flight_path.avi Flight tracker video + - flight_path.png Final flight path image + - camera.avi Downward camera video + - camera_*.png Camera snapshots at key moments + - gazebo.avi Gazebo simulation window recording (via xwd) +""" import os import sys import io -import json import time import threading import cv2 import numpy as np +import subprocess +import re from pathlib import Path from datetime import datetime +try: + import PIL.Image + HAS_PIL = True +except ImportError: + HAS_PIL = False class RunRecorder: def __init__(self, results_dir=None, fps=5): if results_dir is None: - project_dir = Path(__file__).resolve().parent.parent + project_dir = Path(__file__).resolve().parent.parent.parent results_dir = project_dir / "results" results_dir = Path(results_dir) @@ -47,11 +63,15 @@ class RunRecorder: self._tracker_writer = None self._camera_writer = None + self._gazebo_writer = None + self._tracker_size = None self._camera_size = None + self._gazebo_size = None self._tracker_frames = 0 self._camera_frames = 0 + self._gazebo_frames = 0 self._last_tracker_frame = None self._last_camera_frame = None @@ -63,6 +83,10 @@ class RunRecorder: self._record_thread = None self._lock = threading.Lock() + # Gazebo window capture via xwd + self._gazebo_wid = None + self._find_gazebo_window() + self.metadata = { "run": run_num, "start_time": datetime.now().isoformat(), @@ -71,6 +95,37 @@ class RunRecorder: print(f"[REC] Recording to: {self.run_dir}") + def _find_gazebo_window(self): + """Attempts to find the Gazebo window ID using xwininfo.""" + try: + # Find window ID + cmd = "xwininfo -root -tree | grep -i 'gazebo\|gz sim'" + output = subprocess.check_output(cmd, shell=True).decode('utf-8') + lines = output.strip().split('\n') + if not lines: + print("[REC] Gazebo window not found") + return + + # Pick the first one + wid_line = lines[0] + wid_match = re.search(r'(0x[0-9a-fA-F]+)', wid_line) + if not wid_match: + return + self._gazebo_wid = wid_match.group(1) + print(f"[REC] Found Gazebo window ID: {self._gazebo_wid}") + + except Exception as e: + print(f"[REC] Error finding Gazebo window: {e}") + + def set_phase(self, phase: str): + # No-op since CSV logging is removed + pass + + def log_position(self, uav_x=0, uav_y=0, uav_alt=0, uav_heading=0, + ugv_x=0, ugv_y=0): + # No-op since CSV logging is removed + pass + def start_logging(self): sys.stdout = self._tee_stdout sys.stderr = self._tee_stderr @@ -83,6 +138,11 @@ class RunRecorder: self._tracker_ref = tracker self._camera_ref = camera self._recording = True + + # Re-check Gazebo window + if not self._gazebo_wid: + self._find_gazebo_window() + self._record_thread = threading.Thread( target=self._record_loop, daemon=True ) @@ -111,6 +171,33 @@ class RunRecorder: except Exception: pass + # Gazebo Capture via xwd + if self._gazebo_wid and HAS_PIL: + try: + # Capture window to stdout using xwd + proc = subprocess.run( + ['xwd', '-id', self._gazebo_wid, '-out', '/dev/stdout'], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE + ) + if proc.returncode == 0: + # Parse XWD using PIL + img_pil = PIL.Image.open(io.BytesIO(proc.stdout)) + # Convert to OpenCV (RGB -> BGR) + img_np = np.array(img_pil) + # PIL opens XWD as RGB usually, check mode + if img_pil.mode == 'RGB': + img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGB2BGR) + elif img_pil.mode == 'RGBA': + img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGBA2BGR) + else: + img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGB2BGR) # Fallback + + self._write_gazebo_frame(img_bgr) + except Exception: + # xwd failed or window closed + pass + elapsed = time.time() - t0 sleep_time = max(0, interval - elapsed) time.sleep(sleep_time) @@ -135,6 +222,25 @@ class RunRecorder: self._camera_writer.write(frame) self._camera_frames += 1 + def _write_gazebo_frame(self, frame): + h, w = frame.shape[:2] + # Ensure dimensions are even + if w % 2 != 0: w -= 1 + if h % 2 != 0: h -= 1 + frame = frame[:h, :w] + + if self._gazebo_writer is None: + self._gazebo_size = (w, h) + fourcc = cv2.VideoWriter_fourcc(*'XVID') + path = str(self.run_dir / "gazebo.avi") + self._gazebo_writer = cv2.VideoWriter(path, fourcc, self.fps, (w, h)) + elif (w, h) != self._gazebo_size: + # Handle resize? Just skip for now or resize + frame = cv2.resize(frame, self._gazebo_size) + + self._gazebo_writer.write(frame) + self._gazebo_frames += 1 + def snapshot_camera(self, label="snapshot"): if self._camera_ref is None: return @@ -148,50 +254,20 @@ class RunRecorder: self._camera_snapshots.append(filename) print(f"[REC] Snapshot: {filename}") + def snapshot_tracker(self, label="tracker"): + """Save a snapshot of the current flight tracker.""" + if self._last_tracker_frame is not None: + filename = f"tracker_{label}.png" + path = self.run_dir / filename + cv2.imwrite(str(path), self._last_tracker_frame) + print(f"[REC] Tracker snapshot: {filename}") + def save_summary(self, search_mode="", altitude=0, markers=None, - landed=False, extra=None): - duration = time.time() - self.start_time - mins = int(duration // 60) - secs = int(duration % 60) - - summary = { - **self.metadata, - "end_time": datetime.now().isoformat(), - "duration_seconds": round(duration, 1), - "duration_display": f"{mins}m {secs}s", - "search_mode": search_mode, - "altitude": altitude, - "landed": landed, - "markers_found": {}, - "recordings": { - "log": "log.txt", - "flight_path_video": "flight_path.avi", - "flight_path_image": "flight_path.png", - "camera_video": "camera.avi", - "camera_snapshots": self._camera_snapshots, - }, - "frame_counts": { - "tracker": self._tracker_frames, - "camera": self._camera_frames, - }, - } - - if markers: - for mid, info in markers.items(): - pos = info.get('uav_position', {}) - summary["markers_found"][str(mid)] = { - "x": round(pos.get('x', 0), 2), - "y": round(pos.get('y', 0), 2), - "distance": round(info.get('distance', 0), 2), - } - - if extra: - summary.update(extra) - - path = self.run_dir / "summary.json" - with open(path, "w") as f: - json.dump(summary, f, indent=2) - print(f"[REC] Summary saved: {path}") + landed=False, ugv_dispatched=False, + ugv_target=None, extra=None): + # Summary JSON file saving moved/removed as per request + # Still useful to see basic stats in terminal? + pass def stop(self): self._recording = False @@ -211,6 +287,8 @@ class RunRecorder: self._tracker_writer.release() if self._camera_writer: self._camera_writer.release() + if self._gazebo_writer: + self._gazebo_writer.release() self.stop_logging() self._log_file.close() @@ -220,10 +298,14 @@ class RunRecorder: secs = int(duration % 60) self._original_stdout.write( - f"\n[REC] Run recorded: {self.run_dir}\n" - f"[REC] Duration: {mins}m {secs}s | " - f"Tracker: {self._tracker_frames} frames | " - f"Camera: {self._camera_frames} frames\n" + f"\n[REC] ═══════════════════════════════════════\n" + f"[REC] Run #{self.run_num} recorded\n" + f"[REC] Dir: {self.run_dir}\n" + f"[REC] Duration: {mins}m {secs}s\n" + f"[REC] Tracker: {self._tracker_frames} frames\n" + f"[REC] Camera: {self._camera_frames} frames\n" + f"[REC] Gazebo: {self._gazebo_frames} frames\n" + f"[REC] ═══════════════════════════════════════\n" ) diff --git a/worlds/tags/aruco_DICT_4X4_50_1.png b/worlds/tags/aruco_DICT_4X4_50_1.png new file mode 100644 index 0000000000000000000000000000000000000000..28d468aec599b0cc286c86fceacf40a24a96c793 GIT binary patch literal 5833 zcmeAS@N?(olHy`uVBq!ia0y~yU{(NO4xj+TKk3LV3=CouJY5_^Dj44$Jji*#fPs1A z=YP(-g`Z4s7SdUI=8Wg5Gjo=VQZxyIABWD=9-aP(+xWX^e3$;2JK;5&`wWWXKQ0}W z7!8uq#4wr-hJMM=wEoO|7Ep;c$`}oV(UdSix!}hk=`Q^rUHV7GYmSQF3#d5=s$3pu z@B7g9h Q255}h)78&qol`;+09|atv;Y7A literal 0 HcmV?d00001 diff --git a/worlds/uav_ugv_search.sdf b/worlds/uav_ugv_search.sdf index e9bb617..54e3dd4 100644 --- a/worlds/uav_ugv_search.sdf +++ b/worlds/uav_ugv_search.sdf @@ -29,6 +29,7 @@ + 25 @@ -142,81 +143,35 @@ - + model://iris_with_gimbal - 0 0 0.195 0 0 90 + 0.0 0.0 0.4 0 0 90 + + + + model://custom_ugv + ugv + 0.0 0.0 0 0 0 0 + - - - + + + true - 5.0 5.0 0 0 0 0 - - - - 0 0 0.2 0 0 0 - - 1.0 0.7 0.3 - - 0.2 0.2 0.7 1 - 0.2 0.2 0.8 1 - - - - 1.0 0.7 0.3 - - - - - - 0 0 0.36 0 0 0 - - 0.5 0.5 0.005 + 8 -6 0.005 0 0 0 + + + 0.5 0.5 0.01 1 1 1 1 1 1 1 1 - - - tags/land_tag.png - - + tags/aruco_DICT_4X4_50_1.png - - - - 0.35 0.38 0.1 1.5708 0 0 - 0.10.06 - 0.15 0.15 0.15 10.15 0.15 0.15 1 - - - - 0.35 -0.38 0.1 1.5708 0 0 - 0.10.06 - 0.15 0.15 0.15 10.15 0.15 0.15 1 - - - - -0.35 0.38 0.1 1.5708 0 0 - 0.10.06 - 0.15 0.15 0.15 10.15 0.15 0.15 1 - - - - -0.35 -0.38 0.1 1.5708 0 0 - 0.10.06 - 0.15 0.15 0.15 10.15 0.15 0.15 1 - -