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

Code reorganization. Display recording fixes. Search Flight Planner Fixes. Bug Fixes

Browse Source
This commit is contained in:
SirBlobby
2026-02-23 15:27:07 -05:00
parent 9e86b933ca
commit e509e28f97
45 changed files with 440 additions and 2365 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
scripts/generate_world.py
View File

@@ -22,10 +22,10 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
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)
uav_z = 0.40
marker = search_cfg.get("marker", {})
target_pos = marker.get("target_position", [8.0, -6.0])
@@ -42,19 +42,19 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
try:
tree = ET.parse(base_path)
root = tree.getroot()
world = root.find('world')
if world is None:
print("[ERROR] No <world> 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')
@@ -72,7 +72,7 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
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)
@@ -80,29 +80,29 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
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"
@@ -112,7 +112,7 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
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)