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

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 "==================================="