SirBlob/simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Aruco Code and UVG/UAV Logic Fixes

Browse Source
This commit is contained in:
SirBlobby
2026-02-20 12:19:44 -05:00
parent 50ef3f0490
commit e2f805f3f3
22 changed files with 667 additions and 1143 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
config/ardupilot_gps_denied.parm
View File

@@ -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

3
config/search.yaml
View File

@@ -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:

11
config/ugv.yaml
View File

@@ -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

228
models/custom_ugv/model.sdf
View File

@@ -1,159 +1,161 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<sdf version="1.9">
<model name="custom_ugv">
<static>false</static>
<link name="base_link">
<pose>0 0 0.1 0 0 0</pose>
<pose>0 0 0.12 0 0 0</pose>
<inertial>
<mass>5.0</mass>
<mass>80.0</mass>
<inertia>
<ixx>0.1</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.15</iyy><iyz>0</iyz><izz>0.1</izz>
<ixx>3.0</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>4.0</iyy><iyz>0</iyz><izz>3.0</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry><box><size>0.4 0.3 0.15</size></box></geometry>
</collision>
<visual name="visual">
<geometry><box><size>0.4 0.3 0.15</size></box></geometry>
<material><ambient>0.3 0.3 0.8 1</ambient></material>
</visual>
</link>
<link name="left_wheel">
<pose>0 0.175 0.05 -1.5708 0 0</pose>
<inertial>
<mass>0.5</mass>
<inertia>
<ixx>0.001</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.001</iyy><iyz>0</iyz><izz>0.001</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry><cylinder><radius>0.05</radius><length>0.04</length></cylinder></geometry>
<geometry><box><size>0.8 0.6 0.12</size></box></geometry>
<surface>
<friction><ode><mu>1.0</mu><mu2>1.0</mu2></ode></friction>
<friction>
<ode><mu>10.0</mu><mu2>10.0</mu2></ode>
</friction>
</surface>
</collision>
<visual name="visual">
<geometry><cylinder><radius>0.05</radius><length>0.04</length></cylinder></geometry>
<material><ambient>0.1 0.1 0.1 1</ambient></material>
<geometry><box><size>0.8 0.6 0.12</size></box></geometry>
<material><ambient>0.2 0.2 0.7 1</ambient><diffuse>0.2 0.2 0.8 1</diffuse></material>
</visual>
</link>
<link name="right_wheel">
<pose>0 -0.175 0.05 -1.5708 0 0</pose>
<link name="front_left_wheel">
<pose>0.25 0.35 0.08 -1.5708 0 0</pose>
<inertial>
<mass>0.5</mass>
<mass>2.0</mass>
<inertia>
<ixx>0.001</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.001</iyy><iyz>0</iyz><izz>0.001</izz>
<ixx>0.005</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.005</iyy><iyz>0</iyz><izz>0.008</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry><cylinder><radius>0.05</radius><length>0.04</length></cylinder></geometry>
<surface>
<friction><ode><mu>1.0</mu><mu2>1.0</mu2></ode></friction>
</surface>
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
<surface><friction><ode><mu>5.0</mu><mu2>5.0</mu2></ode></friction></surface>
</collision>
<visual name="visual">
<geometry><cylinder><radius>0.05</radius><length>0.04</length></cylinder></geometry>
<material><ambient>0.1 0.1 0.1 1</ambient></material>
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
<material><ambient>0.15 0.15 0.15 1</ambient></material>
</visual>
</link>
<link name="caster">
<pose>-0.15 0 0.025 0 0 0</pose>
<link name="front_right_wheel">
<pose>0.25 -0.35 0.08 -1.5708 0 0</pose>
<inertial>
<mass>0.1</mass>
<mass>2.0</mass>
<inertia>
<ixx>0.0001</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.0001</iyy><iyz>0</iyz><izz>0.0001</izz>
<ixx>0.005</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.005</iyy><iyz>0</iyz><izz>0.008</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry><sphere><radius>0.025</radius></sphere></geometry>
<surface>
<friction><ode><mu>0.0</mu><mu2>0.0</mu2></ode></friction>
</surface>
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
<surface><friction><ode><mu>5.0</mu><mu2>5.0</mu2></ode></friction></surface>
</collision>
<visual name="visual">
<geometry><sphere><radius>0.025</radius></sphere></geometry>
<material><ambient>0.1 0.1 0.1 1</ambient></material>
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
<material><ambient>0.15 0.15 0.15 1</ambient></material>
</visual>
</link>
<joint name="left_wheel_joint" type="revolute">
<link name="rear_left_wheel">
<pose>-0.25 0.35 0.08 -1.5708 0 0</pose>
<inertial>
<mass>2.0</mass>
<inertia>
<ixx>0.005</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.005</iyy><iyz>0</iyz><izz>0.008</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
<surface><friction><ode><mu>5.0</mu><mu2>5.0</mu2></ode></friction></surface>
</collision>
<visual name="visual">
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
<material><ambient>0.15 0.15 0.15 1</ambient></material>
</visual>
</link>
<link name="rear_right_wheel">
<pose>-0.25 -0.35 0.08 -1.5708 0 0</pose>
<inertial>
<mass>2.0</mass>
<inertia>
<ixx>0.005</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.005</iyy><iyz>0</iyz><izz>0.008</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
<surface><friction><ode><mu>5.0</mu><mu2>5.0</mu2></ode></friction></surface>
</collision>
<visual name="visual">
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
<material><ambient>0.15 0.15 0.15 1</ambient></material>
</visual>
</link>
<!-- ArUco landing tag (ID 0) on top -->
<link name="aruco_tag">
<pose>0 0 0.185 0 0 0</pose>
<visual name="aruco_visual">
<geometry><box><size>0.5 0.5 0.005</size></box></geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<pbr><metal><albedo_map>tags/land_tag.png</albedo_map></metal></pbr>
</material>
</visual>
</link>
<joint name="aruco_joint" type="fixed">
<parent>base_link</parent>
<child>left_wheel</child>
<child>aruco_tag</child>
</joint>
<joint name="front_left_wheel_joint" type="revolute">
<parent>base_link</parent>
<child>front_left_wheel</child>
<axis><xyz>0 0 1</xyz></axis>
</joint>
<joint name="right_wheel_joint" type="revolute">
<joint name="front_right_wheel_joint" type="revolute">
<parent>base_link</parent>
<child>right_wheel</child>
<child>front_right_wheel</child>
<axis><xyz>0 0 1</xyz></axis>
</joint>
<joint name="caster_joint" type="ball">
<joint name="rear_left_wheel_joint" type="revolute">
<parent>base_link</parent>
<child>caster</child>
<child>rear_left_wheel</child>
<axis><xyz>0 0 1</xyz></axis>
</joint>
<link name="camera_link">
<pose>0.22 0 0.15 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.00001</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.00001</iyy><iyz>0</iyz><izz>0.00001</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry><box><size>0.02 0.04 0.02</size></box></geometry>
<material><ambient>0 0 0 1</ambient></material>
</visual>
<sensor name="camera" type="camera">
<camera>
<horizontal_fov>1.57</horizontal_fov>
<image><width>640</width><height>480</height><format>R8G8B8</format></image>
<clip><near>0.1</near><far>50</far></clip>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<plugin name="camera_plugin" filename="libgazebo_ros_camera.so">
<ros>
<namespace>/ugv</namespace>
<remapping>image_raw:=camera/forward/image_raw</remapping>
<remapping>camera_info:=camera/forward/camera_info</remapping>
</ros>
<camera_name>forward_camera</camera_name>
<frame_name>camera_link</frame_name>
</plugin>
</sensor>
</link>
<joint name="camera_joint" type="fixed">
<joint name="rear_right_wheel_joint" type="revolute">
<parent>base_link</parent>
<child>camera_link</child>
<child>rear_right_wheel</child>
<axis><xyz>0 0 1</xyz></axis>
</joint>
<plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
<ros><namespace>/ugv</namespace></ros>
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>0.35</wheel_separation>
<wheel_diameter>0.1</wheel_diameter>
<max_wheel_torque>5</max_wheel_torque>
<max_wheel_acceleration>2.0</max_wheel_acceleration>
<command_topic>cmd_vel</command_topic>
<odometry_topic>odom</odometry_topic>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_link</robot_base_frame>
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<plugin filename="gz-sim-diff-drive-system"
name="gz::sim::systems::DiffDrive">
<left_joint>front_left_wheel_joint</left_joint>
<left_joint>rear_left_wheel_joint</left_joint>
<right_joint>front_right_wheel_joint</right_joint>
<right_joint>rear_right_wheel_joint</right_joint>
<wheel_separation>0.70</wheel_separation>
<wheel_radius>0.08</wheel_radius>
<max_linear_acceleration>1.0</max_linear_acceleration>
<min_linear_acceleration>-1.0</min_linear_acceleration>
<max_angular_acceleration>2.0</max_angular_acceleration>
<min_angular_acceleration>-2.0</min_angular_acceleration>
<topic>/ugv/cmd_vel</topic>
<odom_topic>/ugv/odometry</odom_topic>
<odom_publisher_frequency>20</odom_publisher_frequency>
</plugin>
</model>
</sdf>

BIN
models/custom_ugv/tags/land_tag.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 6.0 KiB

52
scripts/run_autonomous.sh
View File

@@ -59,6 +59,14 @@ cleanup_sitl() {
}
trap cleanup_all EXIT
if [ -f "$PROJECT_DIR/venv/bin/activate" ]; then
print_info "Using venv: $PROJECT_DIR/venv"
source "$PROJECT_DIR/venv/bin/activate"
elif [ -f "$PROJECT_DIR/.venv/bin/activate" ]; then
print_info "Using .venv: $PROJECT_DIR/.venv"
source "$PROJECT_DIR/.venv/bin/activate"
fi
export PATH=$PATH:$HOME/ardupilot/Tools/autotest:$HOME/.local/bin
export GZ_SIM_RESOURCE_PATH="$PROJECT_DIR/models:$PROJECT_DIR/worlds:$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds"
export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build
@@ -105,17 +113,24 @@ if [ -f "$UGV_CONFIG" ] && [ -f "$WORLD_FILE" ]; then
import yaml, re
cfg = yaml.safe_load(open('$UGV_CONFIG'))
pos = cfg.get('position', {})
x, y = pos.get('x', 5.0), pos.get('y', 5.0)
x, y = pos.get('x', 0.0), pos.get('y', 0.0)
with open('$WORLD_FILE', 'r') as f:
sdf = f.read()
# Sync UGV include position
sdf = re.sub(
r'(<model name=\"ugv_target\">\s*<static>true</static>\s*)<pose>[^<]*</pose>',
r'(<name>ugv</name>\s*)<pose>[^<]*</pose>',
rf'\1<pose>{x} {y} 0 0 0 0</pose>',
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'(<uri>model://iris_with_gimbal</uri>\s*)<pose[^>]*>[^<]*</pose>',
rf'\1<pose degrees=\"true\">{x} {y} {uav_z} 0 0 90</pose>',
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|<start_paused>true</start_paused>|<start_paused>false</start_paused>|' "$GZ_USER_GUI"
sed -i 's|show_again="true"|show_again="false"|' "$GZ_USER_GUI"
cat << 'EOF' >> "$GZ_USER_GUI"
<plugin filename="VideoRecorder" name="VideoRecorder">
<gz-gui>
<property key="resizable" type="bool">false</property>
<property key="x" type="double">300</property>
<property key="y" type="double">50</property>
<property key="width" type="double">50</property>
<property key="height" type="double">50</property>
<property key="state" type="string">floating</property>
<property key="showTitleBar" type="bool">false</property>
<property key="cardBackground" type="string">#777777</property>
</gz-gui>
<record_video>
<use_sim_time>true</use_sim_time>
<lockstep>true</lockstep>
<bitrate>4000000</bitrate>
</record_video>
</plugin>
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 "==================================="

14
src/control/search.py
View File

@@ -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

49
src/control/uav_controller.py
View File

@@ -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

203
src/control/ugv_controller.py
View File

@@ -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

1
src/localization/__init__.py
View File

@@ -1 +0,0 @@
"""Localization module for sensor fusion and position estimation."""

202
src/localization/ekf_fusion.py
View File

@@ -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()

165
src/localization/landmark_tracker.py
View File

@@ -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()

188
src/localization/position_estimator.py
View File

@@ -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()

157
src/main.py
View File

@@ -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.")

1
src/nodes/__init__.py
View File

@@ -1 +0,0 @@
"""ROS 2 Node implementations."""

7
src/nodes/geofence_node.py
View File

@@ -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()

112
src/nodes/multi_vehicle_coord.py
View File

@@ -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()

134
src/nodes/vision_nav_node.py
View File

@@ -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()

7
src/nodes/visual_odom_node.py
View File

@@ -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()

180
src/utils/recorder.py
View File

@@ -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"
)

BIN
worlds/tags/aruco_DICT_4X4_50_1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 5.7 KiB

81
worlds/uav_ugv_search.sdf
View File

@@ -29,6 +29,7 @@
</plugin>
<plugin filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
<state_hertz>25</state_hertz>
</plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
@@ -142,81 +143,35 @@
</model>
<!-- ===================== UAV: ArduPilot Iris ===================== -->
<!-- Uses the ArduPilot iris_with_gimbal model which includes:
- ArduPilotPlugin (connects to SITL)
- IMU sensor
- Gimbal with camera
- Rotor dynamics / LiftDrag
-->
<!-- Starts on top of the UGV. Body top z=0.18 + iris legs 0.195 = 0.375 -->
<include>
<uri>model://iris_with_gimbal</uri>
<pose degrees="true">0 0 0.195 0 0 90</pose>
<pose degrees="true">0.0 0.0 0.4 0 0 90</pose>
</include>
<!-- ===================== UGV: Driveable Rover ===================== -->
<!-- Dynamic 4-wheel skid-steer, drone takes off from it, then it drives -->
<include>
<uri>model://custom_ugv</uri>
<name>ugv</name>
<pose>0.0 0.0 0 0 0 0</pose>
</include>
<!-- ===================== UGV: Target Vehicle ===================== -->
<!-- The drone should search for this vehicle and land on it -->
<model name="ugv_target">
<!-- ===================== TARGET ArUco Tag (ID 1) ===================== -->
<!-- On the ground — the UAV searches for this, then UGV drives here -->
<model name="target_tag_1">
<static>true</static>
<pose>5.0 5.0 0 0 0 0</pose>
<!-- Main body (blue box) -->
<link name="base_link">
<pose>0 0 0.2 0 0 0</pose>
<visual name="body">
<geometry><box><size>1.0 0.7 0.3</size></box></geometry>
<material>
<ambient>0.2 0.2 0.7 1</ambient>
<diffuse>0.2 0.2 0.8 1</diffuse>
</material>
</visual>
<collision name="body_collision">
<geometry><box><size>1.0 0.7 0.3</size></box></geometry>
</collision>
</link>
<!-- ArUco landing tag on top -->
<link name="aruco_tag">
<pose>0 0 0.36 0 0 0</pose>
<visual name="aruco_visual">
<geometry><box><size>0.5 0.5 0.005</size></box></geometry>
<pose>8 -6 0.005 0 0 0</pose>
<link name="link">
<visual name="v">
<geometry><box><size>0.5 0.5 0.01</size></box></geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<pbr>
<metal>
<albedo_map>tags/land_tag.png</albedo_map>
</metal>
</pbr>
<pbr><metal><albedo_map>tags/aruco_DICT_4X4_50_1.png</albedo_map></metal></pbr>
</material>
</visual>
</link>
<!-- Wheels (visual only since static) -->
<link name="wheel_fl">
<pose>0.35 0.38 0.1 1.5708 0 0</pose>
<visual name="v"><geometry><cylinder><radius>0.1</radius><length>0.06</length></cylinder></geometry>
<material><ambient>0.15 0.15 0.15 1</ambient><diffuse>0.15 0.15 0.15 1</diffuse></material>
</visual>
</link>
<link name="wheel_fr">
<pose>0.35 -0.38 0.1 1.5708 0 0</pose>
<visual name="v"><geometry><cylinder><radius>0.1</radius><length>0.06</length></cylinder></geometry>
<material><ambient>0.15 0.15 0.15 1</ambient><diffuse>0.15 0.15 0.15 1</diffuse></material>
</visual>
</link>
<link name="wheel_rl">
<pose>-0.35 0.38 0.1 1.5708 0 0</pose>
<visual name="v"><geometry><cylinder><radius>0.1</radius><length>0.06</length></cylinder></geometry>
<material><ambient>0.15 0.15 0.15 1</ambient><diffuse>0.15 0.15 0.15 1</diffuse></material>
</visual>
</link>
<link name="wheel_rr">
<pose>-0.35 -0.38 0.1 1.5708 0 0</pose>
<visual name="v"><geometry><cylinder><radius>0.1</radius><length>0.06</length></cylinder></geometry>
<material><ambient>0.15 0.15 0.15 1</ambient><diffuse>0.15 0.15 0.15 1</diffuse></material>
</visual>
</link>
</model>
<!-- ===================== VISUAL MARKERS ===================== -->