diff --git a/config/search.yaml b/config/search.yaml
index 513b628..4923253 100644
--- a/config/search.yaml
+++ b/config/search.yaml
@@ -10,10 +10,11 @@ marker:
size: 0.5 # Physical marker size in meters
landing_ids: [0] # Marker IDs that trigger landing (on UGV)
target_ids: [1] # Marker IDs to find and report to UGV
+ target_position: [-5.0, -5.0] # Initial X, Y location of the target Aruco map in the map
# ── Search Patterns ──────────────────────────────────────────
spiral:
- max_legs: 60
+ max_legs: 35
lawnmower:
width: 30.0
diff --git a/scripts/generate_world.py b/scripts/generate_world.py
new file mode 100755
index 0000000..362007d
--- /dev/null
+++ b/scripts/generate_world.py
@@ -0,0 +1,121 @@
+#!/usr/bin/env python3
+import os
+import yaml
+import sys
+import xml.etree.ElementTree as ET
+
+SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
+PROJECT_DIR = os.path.dirname(SCRIPT_DIR)
+CONFIG_DIR = os.path.join(PROJECT_DIR, "config")
+WORLD_DIR = os.path.join(PROJECT_DIR, "worlds")
+
+def load_config(name):
+ path = os.path.join(CONFIG_DIR, name)
+ if not os.path.exists(path):
+ return {}
+ with open(path, 'r') as f:
+ return yaml.safe_load(f) or {}
+
+def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav_ugv_search.sdf"):
+ ugv_cfg = load_config("ugv.yaml")
+ search_cfg = load_config("search.yaml")
+
+ ugv_x = ugv_cfg.get("position", {}).get("x", 0.0)
+ ugv_y = ugv_cfg.get("position", {}).get("y", 0.0)
+
+ uav_x = ugv_x
+ uav_y = ugv_y
+ uav_z = 0.40 # Start on top of UGV (0.18 top + 0.195 legs)
+
+ marker = search_cfg.get("marker", {})
+ target_pos = marker.get("target_position", [8.0, -6.0])
+ target_x = target_pos[0]
+ target_y = target_pos[1]
+
+ base_path = os.path.join(WORLD_DIR, base_filename)
+ output_path = os.path.join(WORLD_DIR, output_filename)
+
+ if not os.path.exists(base_path):
+ print(f"[ERROR] Base world file not found: {base_path}")
+ sys.exit(1)
+
+ try:
+ tree = ET.parse(base_path)
+ root = tree.getroot()
+
+ world = root.find('world')
+ if world is None:
+ print("[ERROR] No tag found in SDF.")
+ sys.exit(1)
+
+ for include in world.findall('include'):
+ uri = include.find('uri')
+ if uri is not None and uri.text == 'model://iris_with_gimbal':
+ pose = include.find('pose')
+ if pose is not None:
+ pose.text = f"{uav_x} {uav_y} {uav_z} 0 0 90"
+
+ name = include.find('name')
+ if name is not None and name.text == 'ugv':
+ pose = include.find('pose')
+ if pose is not None:
+ pose.text = f"{ugv_x} {ugv_y} 0 0 0 0"
+
+ for model in world.findall('model'):
+ name_attr = model.get('name')
+ if name_attr == 'target_tag_1':
+ pose = model.find('pose')
+ if pose is not None:
+ pose.text = f"{target_x} {target_y} 0.005 0 0 0"
+
+ geofence_cfg = search_cfg.get("geofence", {})
+ if geofence_cfg.get("enabled", False):
+ points = geofence_cfg.get("points", [])
+ if len(points) >= 3:
+ # Remove old geofence visual if it exists
+ for old_model in world.findall('model'):
+ if old_model.get('name') == 'geofence_visual':
+ world.remove(old_model)
+
+ gf_model = ET.SubElement(world, "model", name="geofence_visual")
+ ET.SubElement(gf_model, "static").text = "true"
+ link = ET.SubElement(gf_model, "link", name="link")
+
+ import math
+ for i in range(len(points)):
+ p1 = points[i]
+ p2 = points[(i + 1) % len(points)]
+ x1, y1 = float(p1[0]), float(p1[1])
+ x2, y2 = float(p2[0]), float(p2[1])
+
+ dx = x2 - x1
+ dy = y2 - y1
+ length = math.sqrt(dx*dx + dy*dy)
+ cx = x1 + dx / 2.0
+ cy = y1 + dy / 2.0
+ yaw = math.atan2(dy, dx)
+
+ visual = ET.SubElement(link, "visual", name=f"edge_{i}")
+ ET.SubElement(visual, "pose").text = f"{cx} {cy} 0.01 0 0 {yaw}"
+
+ geometry = ET.SubElement(visual, "geometry")
+ box = ET.SubElement(geometry, "box")
+ # size is Length(X), Width(Y), Thickness(Z)
+ ET.SubElement(box, "size").text = f"{length} 0.2 0.02"
+
+ material = ET.SubElement(visual, "material")
+ ET.SubElement(material, "ambient").text = "1 0 0 1"
+ ET.SubElement(material, "diffuse").text = "1 0 0 1"
+ ET.SubElement(material, "emissive").text = "0.8 0 0 0.5"
+
+ tree.write(output_path, encoding="utf-8", xml_declaration=True)
+ print(f"[INFO] Generated world file: {output_path}")
+ print(f"[INFO] UGV set to ({ugv_x}, {ugv_y})")
+ print(f"[INFO] Target Marker set to ({target_x}, {target_y})")
+
+ except Exception as e:
+ print(f"[ERROR] Failed to parse or write XML: {e}")
+ sys.exit(1)
+
+if __name__ == "__main__":
+ generate_world()
diff --git a/scripts/run_autonomous.sh b/scripts/run_autonomous.sh
index 58115d9..a8dcb0a 100755
--- a/scripts/run_autonomous.sh
+++ b/scripts/run_autonomous.sh
@@ -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'(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 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 "==================================="
diff --git a/worlds/uav_ugv_search.sdf b/worlds/uav_ugv_search.sdf
index 54e3dd4..01e0cfa 100644
--- a/worlds/uav_ugv_search.sdf
+++ b/worlds/uav_ugv_search.sdf
@@ -1,52 +1,38 @@
-
-
+
-
+
0.002
1.0
-
-
+
+
-
+
ogre2
-
+
-
+
25
-
+
-
+
-
+
1.0 1.0 1.0
0.6 0.75 0.9
-
+
false
-
+
-35.363262
149.165237
@@ -55,7 +41,7 @@
EARTH_WGS84
-
+
false
0 0 10 0 0 0
@@ -70,7 +56,7 @@
-0.5 0.1 -0.9
-
+
true
@@ -83,6 +69,7 @@
+ 0 0 -0.02 0 0 0
0 0 1
@@ -97,11 +84,9 @@
-
1
-
0
5 0 0.05 0 0 0
@@ -112,7 +97,6 @@
1 0 0 0.5
-
0
0 5 0.05 0 0 0
@@ -123,7 +107,6 @@
0 1 0 0.5
-
0
0 0 5.05 0 0 0
@@ -134,7 +117,6 @@
0 0 1 0.5
-
1
1
@@ -142,26 +124,23 @@
-
-
+
model://iris_with_gimbal
0.0 0.0 0.4 0 0 90
-
-
+
model://custom_ugv
ugv
0.0 0.0 0 0 0 0
-
-
+
true
- 8 -6 0.005 0 0 0
+ -5.0 -5.0 0.005 0 0 0
0.5 0.5 0.01
@@ -174,10 +153,8 @@
-
-
-
-
+
+
true
0 0 0.005 0 0 0
@@ -193,7 +170,7 @@
-
+
true
5 0 0.005 0 0 0
@@ -209,7 +186,7 @@
-
+
true
10 0 0.005 0 0 0
@@ -225,7 +202,7 @@
-
+
true
10 10 0.005 0 0 0
@@ -241,5 +218,5 @@
-
-
+ true-15.0 0.0 0.01 0 0 1.570796326794896630.0 0.2 0.021 0 0 11 0 0 10.8 0 0 0.50.0 15.0 0.01 0 0 0.030.0 0.2 0.021 0 0 11 0 0 10.8 0 0 0.515.0 0.0 0.01 0 0 -1.570796326794896630.0 0.2 0.021 0 0 11 0 0 10.8 0 0 0.50.0 -15.0 0.01 0 0 3.14159265358979330.0 0.2 0.021 0 0 11 0 0 10.8 0 0 0.5
+
\ No newline at end of file
diff --git a/worlds/uav_ugv_search_base.sdf b/worlds/uav_ugv_search_base.sdf
new file mode 100644
index 0000000..e1a5ed6
--- /dev/null
+++ b/worlds/uav_ugv_search_base.sdf
@@ -0,0 +1,236 @@
+
+
+
+
+
+
+
+ 0.002
+ 1.0
+
+
+
+
+
+
+ ogre2
+
+
+
+
+ 25
+
+
+
+
+
+
+
+
+ 1.0 1.0 1.0
+ 0.6 0.75 0.9
+
+ false
+
+
+
+
+ -35.363262
+ 149.165237
+ 584
+ 0
+ EARTH_WGS84
+
+
+
+
+ false
+ 0 0 10 0 0 0
+ 0.9 0.9 0.9 1
+ 0.5 0.5 0.5 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+
+
+ true
+
+
+
+
+ 0 0 1
+ 100 100
+
+
+
+
+ 0 0 -0.02 0 0 0
+
+
+ 0 0 1
+ 100 100
+
+
+
+ 0.5 0.55 0.45 1
+ 0.5 0.55 0.45 1
+
+
+
+
+
+
+ 1
+
+
+ 0
+ 5 0 0.05 0 0 0
+ 10 0.02 0.02
+
+ 1 0 0 0.8
+ 1 0 0 0.8
+ 1 0 0 0.5
+
+
+
+ 0
+ 0 5 0.05 0 0 0
+ 0.02 10 0.02
+
+ 0 1 0 0.8
+ 0 1 0 0.8
+ 0 1 0 0.5
+
+
+
+ 0
+ 0 0 5.05 0 0 0
+ 0.02 0.02 10
+
+ 0 0 1 0.8
+ 0 0 1 0.8
+ 0 0 1 0.5
+
+
+
+ 1
+ 1
+
+
+
+
+
+
+ model://iris_with_gimbal
+ 0.0 0.0 0.4 0 0 90
+
+
+
+
+ model://custom_ugv
+ ugv
+ 0.0 0.0 0 0 0 0
+
+
+
+
+ true
+ 8 -6 0.005 0 0 0
+
+
+ 0.5 0.5 0.01
+
+ 1 1 1 1
+ 1 1 1 1
+ tags/aruco_DICT_4X4_50_1.png
+
+
+
+
+
+
+
+
+ true
+ 0 0 0.005 0 0 0
+
+
+ 0.40.01
+
+ 1 1 0 1
+ 1 1 0 1
+ 0.8 0.8 0 0.6
+
+
+
+
+
+
+
+ true
+ 5 0 0.005 0 0 0
+
+
+ 0.5 0.5 0.01
+
+ 1 0.1 0.1 1
+ 1 0.1 0.1 1
+ 0.8 0 0 0.5
+
+
+
+
+
+
+
+ true
+ 10 0 0.005 0 0 0
+
+
+ 0.5 0.5 0.01
+
+ 0.1 1 0.1 1
+ 0.1 1 0.1 1
+ 0 0.8 0 0.5
+
+
+
+
+
+
+
+ true
+ 10 10 0.005 0 0 0
+
+
+ 0.5 0.5 0.01
+
+ 0.1 0.1 1 1
+ 0.1 0.1 1 1
+ 0 0 0.8 0.5
+
+
+
+
+
+
+