XML world parser
This commit is contained in:
@@ -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 "==================================="
|
||||
|
||||
Reference in New Issue
Block a user