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

21
config/search.yaml
View File

@@ -4,6 +4,7 @@ marker:
size: 0.5
landing_ids: [0]
target_ids: [1]
target_position: [5.0, 3.0]
spiral:
max_legs: 35
lawnmower:
@@ -22,3 +23,23 @@ geofence:
- [-15, 15]
- [15, 15]
- [15, -15]
obstacles:
enabled: true
min_spacing_ft: 5.0
types:
- name: box_small
height: 0.3
count_min: 10
count_max: 20
- name: box_medium
height: 0.5
count_min: 10
count_max: 20
- name: box_large
height: 0.7
count_min: 10
count_max: 20
- name: traffic_cone
height: 0.40
count_min: 10
count_max: 20

7
models/box_large/model.config Normal file
View File

@@ -0,0 +1,7 @@
<?xml version="1.0"?>
<model>
<name>Large Cardboard Box</name>
<version>1.0</version>
<sdf version="1.9">model.sdf</sdf>
<description>Large cardboard box obstacle (~3ft cube)</description>
</model>

19
models/box_large/model.sdf Normal file
View File

@@ -0,0 +1,19 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<model name="box_large">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry><box><size>0.8 0.6 0.7</size></box></geometry>
</collision>
<visual name="visual">
<geometry><box><size>0.8 0.6 0.7</size></box></geometry>
<material>
<ambient>0.58 0.40 0.24 1</ambient>
<diffuse>0.58 0.40 0.24 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
</material>
</visual>
</link>
</model>
</sdf>

7
models/box_medium/model.config Normal file
View File

@@ -0,0 +1,7 @@
<?xml version="1.0"?>
<model>
<name>Medium Cardboard Box</name>
<version>1.0</version>
<sdf version="1.9">model.sdf</sdf>
<description>Medium cardboard box obstacle (~2ft cube)</description>
</model>

19
models/box_medium/model.sdf Normal file
View File

@@ -0,0 +1,19 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<model name="box_medium">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry><box><size>0.5 0.5 0.5</size></box></geometry>
</collision>
<visual name="visual">
<geometry><box><size>0.5 0.5 0.5</size></box></geometry>
<material>
<ambient>0.65 0.45 0.28 1</ambient>
<diffuse>0.65 0.45 0.28 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
</material>
</visual>
</link>
</model>
</sdf>

7
models/box_small/model.config Normal file
View File

@@ -0,0 +1,7 @@
<?xml version="1.0"?>
<model>
<name>Small Cardboard Box</name>
<version>1.0</version>
<sdf version="1.9">model.sdf</sdf>
<description>Small cardboard box obstacle (~1ft cube)</description>
</model>

19
models/box_small/model.sdf Normal file
View File

@@ -0,0 +1,19 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<model name="box_small">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry><box><size>0.3 0.3 0.3</size></box></geometry>
</collision>
<visual name="visual">
<geometry><box><size>0.3 0.3 0.3</size></box></geometry>
<material>
<ambient>0.72 0.53 0.34 1</ambient>
<diffuse>0.72 0.53 0.34 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
</material>
</visual>
</link>
</model>
</sdf>

42
models/custom_ugv/model.sdf
View File

@@ -115,11 +115,53 @@
</visual>
</link>
<!-- Forward-facing camera for obstacle detection -->
<link name="front_camera_link">
<pose>0.42 0 0.22 0 0 0</pose>
<inertial>
<mass>0.05</mass>
<inertia>
<ixx>0.00001</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.00001</iyy><iyz>0</iyz><izz>0.00001</izz>
</inertia>
</inertial>
<visual name="camera_visual">
<geometry><box><size>0.03 0.03 0.03</size></box></geometry>
<material>
<ambient>0.1 0.1 0.1 1</ambient>
<diffuse>0.1 0.1 0.1 1</diffuse>
</material>
</visual>
<sensor name="front_camera" type="camera">
<always_on>1</always_on>
<update_rate>10</update_rate>
<visualize>1</visualize>
<topic>/ugv/camera/forward</topic>
<camera>
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>50</far>
</clip>
</camera>
</sensor>
</link>
<joint name="aruco_joint" type="fixed">
<parent>base_link</parent>
<child>aruco_tag</child>
</joint>
<joint name="front_camera_joint" type="fixed">
<parent>base_link</parent>
<child>front_camera_link</child>
</joint>
<joint name="front_left_wheel_joint" type="revolute">
<parent>base_link</parent>
<child>front_left_wheel</child>

7
models/traffic_cone/model.config Normal file
View File

@@ -0,0 +1,7 @@
<?xml version="1.0"?>
<model>
<name>Traffic Cone</name>
<version>1.0</version>
<sdf version="1.9">model.sdf</sdf>
<description>Standard traffic cone obstacle</description>
</model>

128
models/traffic_cone/model.sdf Normal file
View File

@@ -0,0 +1,128 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<model name="traffic_cone">
<static>true</static>
<link name="link">
<!-- Rubber base plate -->
<collision name="base_collision">
<pose>0 0 0.015 0 0 0</pose>
<geometry><box><size>0.38 0.38 0.03</size></box></geometry>
</collision>
<visual name="base_visual">
<pose>0 0 0.015 0 0 0</pose>
<geometry><box><size>0.38 0.38 0.03</size></box></geometry>
<material>
<ambient>0.08 0.08 0.08 1</ambient>
<diffuse>0.10 0.10 0.10 1</diffuse>
</material>
</visual>
<!-- Cone body: tapered segments (bottom to top) -->
<!-- Segment 1 (bottom) — widest -->
<visual name="seg_1">
<pose>0 0 0.065 0 0 0</pose>
<geometry><cylinder><radius>0.135</radius><length>0.07</length></cylinder></geometry>
<material>
<ambient>1.0 0.35 0.0 1</ambient>
<diffuse>1.0 0.40 0.05 1</diffuse>
<emissive>0.25 0.08 0.0 0.2</emissive>
</material>
</visual>
<!-- Segment 2 -->
<visual name="seg_2">
<pose>0 0 0.125 0 0 0</pose>
<geometry><cylinder><radius>0.120</radius><length>0.05</length></cylinder></geometry>
<material>
<ambient>1.0 0.35 0.0 1</ambient>
<diffuse>1.0 0.40 0.05 1</diffuse>
<emissive>0.25 0.08 0.0 0.2</emissive>
</material>
</visual>
<!-- Lower reflective stripe -->
<visual name="stripe_lower">
<pose>0 0 0.168 0 0 0</pose>
<geometry><cylinder><radius>0.108</radius><length>0.036</length></cylinder></geometry>
<material>
<ambient>0.95 0.95 0.95 1</ambient>
<diffuse>1.0 1.0 1.0 1</diffuse>
<emissive>0.6 0.6 0.6 0.5</emissive>
</material>
</visual>
<!-- Segment 3 (between stripes) -->
<visual name="seg_3">
<pose>0 0 0.210 0 0 0</pose>
<geometry><cylinder><radius>0.095</radius><length>0.05</length></cylinder></geometry>
<material>
<ambient>1.0 0.35 0.0 1</ambient>
<diffuse>1.0 0.40 0.05 1</diffuse>
<emissive>0.25 0.08 0.0 0.2</emissive>
</material>
</visual>
<!-- Upper reflective stripe -->
<visual name="stripe_upper">
<pose>0 0 0.253 0 0 0</pose>
<geometry><cylinder><radius>0.080</radius><length>0.036</length></cylinder></geometry>
<material>
<ambient>0.95 0.95 0.95 1</ambient>
<diffuse>1.0 1.0 1.0 1</diffuse>
<emissive>0.6 0.6 0.6 0.5</emissive>
</material>
</visual>
<!-- Segment 4 -->
<visual name="seg_4">
<pose>0 0 0.295 0 0 0</pose>
<geometry><cylinder><radius>0.065</radius><length>0.05</length></cylinder></geometry>
<material>
<ambient>1.0 0.35 0.0 1</ambient>
<diffuse>1.0 0.40 0.05 1</diffuse>
<emissive>0.25 0.08 0.0 0.2</emissive>
</material>
</visual>
<!-- Segment 5 -->
<visual name="seg_5">
<pose>0 0 0.340 0 0 0</pose>
<geometry><cylinder><radius>0.048</radius><length>0.04</length></cylinder></geometry>
<material>
<ambient>1.0 0.35 0.0 1</ambient>
<diffuse>1.0 0.40 0.05 1</diffuse>
<emissive>0.25 0.08 0.0 0.2</emissive>
</material>
</visual>
<!-- Segment 6 (top) — narrowest -->
<visual name="seg_6">
<pose>0 0 0.375 0 0 0</pose>
<geometry><cylinder><radius>0.032</radius><length>0.03</length></cylinder></geometry>
<material>
<ambient>1.0 0.35 0.0 1</ambient>
<diffuse>1.0 0.40 0.05 1</diffuse>
<emissive>0.25 0.08 0.0 0.2</emissive>
</material>
</visual>
<!-- Tip cap -->
<visual name="tip">
<pose>0 0 0.395 0 0 0</pose>
<geometry><cylinder><radius>0.020</radius><length>0.01</length></cylinder></geometry>
<material>
<ambient>1.0 0.40 0.05 1</ambient>
<diffuse>1.0 0.45 0.10 1</diffuse>
</material>
</visual>
<!-- Collision: simplified bounding shape for the whole cone -->
<collision name="cone_collision">
<pose>0 0 0.22 0 0 0</pose>
<geometry><cylinder><radius>0.13</radius><length>0.40</length></cylinder></geometry>
</collision>
</link>
</model>
</sdf>

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"

1
src/main.py
View File

@@ -101,6 +101,7 @@ def main():
camera.register_callback("downward", detection_overlay)
camera.register_callback("gimbal", detection_overlay)
camera.register_callback("ugv_forward", detection_overlay)
except Exception as e:
print(f"[MAIN] Camera unavailable: {e}")
camera = None

38
src/utils/recorder.py
View File

@@ -81,18 +81,22 @@ class RunRecorder:
self._tracker_writer = None
self._camera_writer = None
self._ugv_camera_writer = None
self._gazebo_writer = None
self._tracker_size = None
self._camera_size = None
self._ugv_camera_size = None
self._gazebo_size = None
self._tracker_frames = 0
self._camera_frames = 0
self._ugv_camera_frames = 0
self._gazebo_frames = 0
self._last_tracker_frame = None
self._last_camera_frame = None
self._last_ugv_camera_frame = None
self._camera_snapshots = []
self._recording = False
@@ -290,6 +294,14 @@ class RunRecorder:
except Exception:
pass
try:
ugv_frame = self._camera_ref.frames.get("ugv_forward")
if ugv_frame is not None:
self._write_ugv_camera_frame(ugv_frame)
self._last_ugv_camera_frame = ugv_frame.copy()
except Exception:
pass
elapsed = time.time() - t0
sleep_time = max(0, interval - elapsed)
time.sleep(sleep_time)
@@ -314,6 +326,16 @@ class RunRecorder:
self._camera_writer.write(frame)
self._camera_frames += 1
def _write_ugv_camera_frame(self, frame):
h, w = frame.shape[:2]
if self._ugv_camera_writer is None:
self._ugv_camera_size = (w, h)
fourcc = cv2.VideoWriter_fourcc(*'XVID')
path = str(self.run_dir / "ugv_camera.avi")
self._ugv_camera_writer = cv2.VideoWriter(path, fourcc, self.fps, (w, h))
self._ugv_camera_writer.write(frame)
self._ugv_camera_frames += 1
def snapshot_camera(self, label="snapshot"):
if self._camera_ref is None:
return
@@ -398,12 +420,21 @@ class RunRecorder:
cv2.imwrite(str(path), self._last_camera_frame)
self._upload_file(path, filename)
if self._last_ugv_camera_frame is not None:
filename = "ugv_camera_final.png"
path = self.run_dir / filename
cv2.imwrite(str(path), self._last_ugv_camera_frame)
self._upload_file(path, filename)
if self._tracker_writer:
self._tracker_writer.release()
self._upload_file(self.run_dir / "flight_path.avi", "flight_path.avi")
if self._camera_writer:
self._camera_writer.release()
self._upload_file(self.run_dir / "camera.avi", "camera.avi")
if self._ugv_camera_writer:
self._ugv_camera_writer.release()
self._upload_file(self.run_dir / "ugv_camera.avi", "ugv_camera.avi")
self.stop_logging()
self._log_file.close()
@@ -427,9 +458,10 @@ class RunRecorder:
f"\n[REC] ═══════════════════════════════════════\n"
f"[REC] Cloud Upload Complete (ID: {self.sim_id})\n"
f"[REC] Duration: {mins}m {secs}s\n"
f"[REC] Tracker: {self._tracker_frames} frames\n"
f"[REC] Camera: {self._camera_frames} frames\n"
f"[REC] Gazebo: via ffmpeg (.mp4)\n"
f"[REC] Tracker: {self._tracker_frames} frames\n"
f"[REC] UAV Camera: {self._camera_frames} frames\n"
f"[REC] UGV Camera: {self._ugv_camera_frames} frames\n"
f"[REC] Gazebo: via ffmpeg (.mp4)\n"
f"[REC] ═══════════════════════════════════════\n"
)

4
src/vision/camera_processor.py
View File

@@ -25,6 +25,7 @@ class CameraProcessor:
topics = {
"downward": "/uav/camera/downward",
"gimbal": "/world/uav_ugv_search/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image",
"ugv_forward": "/ugv/camera/forward",
}
self.topics = topics
@@ -100,12 +101,15 @@ def main():
all_topics = {
"downward": "/uav/camera/downward",
"gimbal": "/world/uav_ugv_search/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image",
"ugv_forward": "/ugv/camera/forward",
}
if cameras == "down":
topics = {"downward": all_topics["downward"]}
elif cameras == "gimbal":
topics = {"gimbal": all_topics["gimbal"]}
elif cameras == "ugv":
topics = {"ugv_forward": all_topics["ugv_forward"]}
else:
topics = all_topics