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

UGV Camera and World Gen Boxes

Browse Source
This commit is contained in:
SirBlobby
2026-02-23 16:00:12 -05:00
parent 9eea35ed1d
commit e17f3e67dd
15 changed files with 426 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

107
scripts/generate_world.py
View File

@@ -1,5 +1,7 @@
#!/usr/bin/env python3
import os
import math
import random
import yaml
import sys
import xml.etree.ElementTree as ET
@@ -9,6 +11,8 @@ PROJECT_DIR = os.path.dirname(SCRIPT_DIR)
CONFIG_DIR = os.path.join(PROJECT_DIR, "config")
WORLD_DIR = os.path.join(PROJECT_DIR, "worlds")
FEET_TO_METERS = 0.3048
def load_config(name):
path = os.path.join(CONFIG_DIR, name)
if not os.path.exists(path):
@@ -16,6 +20,69 @@ def load_config(name):
with open(path, 'r') as f:
return yaml.safe_load(f) or {}
def generate_obstacle_positions(obstacle_cfg, geofence_points, exclusion_zones):
if not obstacle_cfg.get("enabled", False):
return []
min_spacing = obstacle_cfg.get("min_spacing_ft", 5.0) * FEET_TO_METERS
obstacle_types = obstacle_cfg.get("types", [])
if not obstacle_types:
return []
if geofence_points and len(geofence_points) >= 3:
xs = [p[0] for p in geofence_points]
ys = [p[1] for p in geofence_points]
margin = 2.0
x_min, x_max = min(xs) + margin, max(xs) - margin
y_min, y_max = min(ys) + margin, max(ys) - margin
else:
x_min, x_max = -12.0, 12.0
y_min, y_max = -12.0, 12.0
placements = []
for obs_type in obstacle_types:
name = obs_type.get("name", "box_small")
height = obs_type.get("height", 0.3)
count_min = obs_type.get("count_min", 1)
count_max = obs_type.get("count_max", 3)
count = random.randint(count_min, count_max)
for _ in range(count):
placed = False
for _attempt in range(200):
x = random.uniform(x_min, x_max)
y = random.uniform(y_min, y_max)
too_close = False
for ex, ey, er in exclusion_zones:
if math.sqrt((x - ex)**2 + (y - ey)**2) < er:
too_close = True
break
if too_close:
continue
for px, py, _, _ in placements:
if math.sqrt((x - px)**2 + (y - py)**2) < min_spacing:
too_close = True
break
if too_close:
continue
yaw = random.uniform(0, 2 * math.pi)
placements.append((x, y, name, height))
placed = True
break
if not placed:
print(f"[WARN] Could not place {name} with minimum spacing")
return placements
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")
@@ -28,6 +95,7 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
uav_z = 0.40
marker = search_cfg.get("marker", {})
target_ids = marker.get("target_ids", [1])
target_pos = marker.get("target_position", [8.0, -6.0])
target_x = target_pos[0]
target_y = target_pos[1]
@@ -81,7 +149,6 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
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)]
@@ -108,6 +175,44 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
ET.SubElement(material, "diffuse").text = "1 0 0 1"
ET.SubElement(material, "emissive").text = "0.8 0 0 0.5"
for old_model in list(world.findall('model')):
name_attr = old_model.get('name', '')
if name_attr.startswith('obstacle_'):
world.remove(old_model)
for old_include in list(world.findall('include')):
name_el = old_include.find('name')
if name_el is not None and name_el.text and name_el.text.startswith('obstacle_'):
world.remove(old_include)
obstacle_cfg = search_cfg.get("obstacles", {})
geofence_points = []
if geofence_cfg.get("enabled", False):
geofence_points = [(float(p[0]), float(p[1]))
for p in geofence_cfg.get("points", [])]
exclusion_zones = [
(ugv_x, ugv_y, 2.0),
(target_x, target_y, 2.0),
]
if obstacle_cfg.get("enabled", False):
placements = generate_obstacle_positions(
obstacle_cfg, geofence_points, exclusion_zones)
for idx, (ox, oy, obs_name, obs_height) in enumerate(placements):
oz = obs_height / 2.0
yaw = random.uniform(0, 2 * math.pi)
include_el = ET.SubElement(world, "include")
ET.SubElement(include_el, "uri").text = f"model://{obs_name}"
ET.SubElement(include_el, "name").text = f"obstacle_{obs_name}_{idx}"
ET.SubElement(include_el, "pose").text = f"{ox:.3f} {oy:.3f} {oz:.3f} 0 0 {yaw:.3f}"
print(f"[INFO] Placed {len(placements)} obstacles "
f"(min spacing: {obstacle_cfg.get('min_spacing_ft', 5.0)} ft)")
for ox, oy, obs_name, obs_height in placements:
print(f"[INFO] {obs_name} at ({ox:.1f}, {oy:.1f})")
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})")

6
scripts/run_autonomous.sh
View File

@@ -169,10 +169,12 @@ MAIN_ARGS="--device sim --connection tcp:127.0.0.1:5760 --search $SEARCH"
python3 src/main.py $MAIN_ARGS &
MAIN_PID=$!
print_success "main.py running (PID: $MAIN_PID)"
print_info "[4/4] Starting camera viewer ..."
print_info "[4/4] Starting camera viewers ..."
python3 -W ignore:RuntimeWarning -m src.vision.camera_processor down &
CAM_PID=$!
print_success "Camera viewer running (PID: $CAM_PID)"
python3 -W ignore:RuntimeWarning -m src.vision.camera_processor ugv &
UGV_CAM_PID=$!
print_success "Camera viewers running (UAV: $CAM_PID, UGV: $UGV_CAM_PID)"
print_info ""
print_info "==================================="
print_info " Simulation Running"