XML world parser

This commit is contained in:
2026-02-21 17:18:30 -05:00
parent 5a60d22458
commit 8ec4bc7846
5 changed files with 387 additions and 74 deletions

View File

@@ -107,30 +107,8 @@ else
exit 1
fi
UGV_CONFIG="$PROJECT_DIR/config/ugv.yaml"
if [ -f "$UGV_CONFIG" ] && [ -f "$WORLD_FILE" ]; then
python3 -c "
import yaml, re
cfg = yaml.safe_load(open('$UGV_CONFIG'))
pos = cfg.get('position', {})
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'(<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 at ({x}, {y}) — UAV on top at z={uav_z}')
" 2>/dev/null || print_info "Position sync skipped"
if [ -f "$PROJECT_DIR/scripts/generate_world.py" ]; then
python3 "$PROJECT_DIR/scripts/generate_world.py"
fi
print_info "==================================="