diff --git a/config/search.yaml b/config/search.yaml index f648ebf..a209595 100644 --- a/config/search.yaml +++ b/config/search.yaml @@ -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 diff --git a/models/box_large/model.config b/models/box_large/model.config new file mode 100644 index 0000000..045f480 --- /dev/null +++ b/models/box_large/model.config @@ -0,0 +1,7 @@ + + + Large Cardboard Box + 1.0 + model.sdf + Large cardboard box obstacle (~3ft cube) + diff --git a/models/box_large/model.sdf b/models/box_large/model.sdf new file mode 100644 index 0000000..19ce248 --- /dev/null +++ b/models/box_large/model.sdf @@ -0,0 +1,19 @@ + + + + true + + + 0.8 0.6 0.7 + + + 0.8 0.6 0.7 + + 0.58 0.40 0.24 1 + 0.58 0.40 0.24 1 + 0.1 0.1 0.1 1 + + + + + diff --git a/models/box_medium/model.config b/models/box_medium/model.config new file mode 100644 index 0000000..1253236 --- /dev/null +++ b/models/box_medium/model.config @@ -0,0 +1,7 @@ + + + Medium Cardboard Box + 1.0 + model.sdf + Medium cardboard box obstacle (~2ft cube) + diff --git a/models/box_medium/model.sdf b/models/box_medium/model.sdf new file mode 100644 index 0000000..5e31bf9 --- /dev/null +++ b/models/box_medium/model.sdf @@ -0,0 +1,19 @@ + + + + true + + + 0.5 0.5 0.5 + + + 0.5 0.5 0.5 + + 0.65 0.45 0.28 1 + 0.65 0.45 0.28 1 + 0.1 0.1 0.1 1 + + + + + diff --git a/models/box_small/model.config b/models/box_small/model.config new file mode 100644 index 0000000..66e1ec1 --- /dev/null +++ b/models/box_small/model.config @@ -0,0 +1,7 @@ + + + Small Cardboard Box + 1.0 + model.sdf + Small cardboard box obstacle (~1ft cube) + diff --git a/models/box_small/model.sdf b/models/box_small/model.sdf new file mode 100644 index 0000000..cd022a4 --- /dev/null +++ b/models/box_small/model.sdf @@ -0,0 +1,19 @@ + + + + true + + + 0.3 0.3 0.3 + + + 0.3 0.3 0.3 + + 0.72 0.53 0.34 1 + 0.72 0.53 0.34 1 + 0.1 0.1 0.1 1 + + + + + diff --git a/models/custom_ugv/model.sdf b/models/custom_ugv/model.sdf index 0ebaa7f..babf88e 100644 --- a/models/custom_ugv/model.sdf +++ b/models/custom_ugv/model.sdf @@ -115,11 +115,53 @@ + + + 0.42 0 0.22 0 0 0 + + 0.05 + + 0.0000100 + 0.0000100.00001 + + + + 0.03 0.03 0.03 + + 0.1 0.1 0.1 1 + 0.1 0.1 0.1 1 + + + + 1 + 10 + 1 + /ugv/camera/forward + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.1 + 50 + + + + + base_link aruco_tag + + base_link + front_camera_link + + base_link front_left_wheel diff --git a/models/traffic_cone/model.config b/models/traffic_cone/model.config new file mode 100644 index 0000000..30735d7 --- /dev/null +++ b/models/traffic_cone/model.config @@ -0,0 +1,7 @@ + + + Traffic Cone + 1.0 + model.sdf + Standard traffic cone obstacle + diff --git a/models/traffic_cone/model.sdf b/models/traffic_cone/model.sdf new file mode 100644 index 0000000..ca36f42 --- /dev/null +++ b/models/traffic_cone/model.sdf @@ -0,0 +1,128 @@ + + + + true + + + + + 0 0 0.015 0 0 0 + 0.38 0.38 0.03 + + + 0 0 0.015 0 0 0 + 0.38 0.38 0.03 + + 0.08 0.08 0.08 1 + 0.10 0.10 0.10 1 + + + + + + + 0 0 0.065 0 0 0 + 0.1350.07 + + 1.0 0.35 0.0 1 + 1.0 0.40 0.05 1 + 0.25 0.08 0.0 0.2 + + + + + + 0 0 0.125 0 0 0 + 0.1200.05 + + 1.0 0.35 0.0 1 + 1.0 0.40 0.05 1 + 0.25 0.08 0.0 0.2 + + + + + + 0 0 0.168 0 0 0 + 0.1080.036 + + 0.95 0.95 0.95 1 + 1.0 1.0 1.0 1 + 0.6 0.6 0.6 0.5 + + + + + + 0 0 0.210 0 0 0 + 0.0950.05 + + 1.0 0.35 0.0 1 + 1.0 0.40 0.05 1 + 0.25 0.08 0.0 0.2 + + + + + + 0 0 0.253 0 0 0 + 0.0800.036 + + 0.95 0.95 0.95 1 + 1.0 1.0 1.0 1 + 0.6 0.6 0.6 0.5 + + + + + + 0 0 0.295 0 0 0 + 0.0650.05 + + 1.0 0.35 0.0 1 + 1.0 0.40 0.05 1 + 0.25 0.08 0.0 0.2 + + + + + + 0 0 0.340 0 0 0 + 0.0480.04 + + 1.0 0.35 0.0 1 + 1.0 0.40 0.05 1 + 0.25 0.08 0.0 0.2 + + + + + + 0 0 0.375 0 0 0 + 0.0320.03 + + 1.0 0.35 0.0 1 + 1.0 0.40 0.05 1 + 0.25 0.08 0.0 0.2 + + + + + + 0 0 0.395 0 0 0 + 0.0200.01 + + 1.0 0.40 0.05 1 + 1.0 0.45 0.10 1 + + + + + + 0 0 0.22 0 0 0 + 0.130.40 + + + + + diff --git a/scripts/generate_world.py b/scripts/generate_world.py index 357a80d..ba9ad7f 100755 --- a/scripts/generate_world.py +++ b/scripts/generate_world.py @@ -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})") diff --git a/scripts/run_autonomous.sh b/scripts/run_autonomous.sh index f3e3945..c478c1a 100755 --- a/scripts/run_autonomous.sh +++ b/scripts/run_autonomous.sh @@ -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" diff --git a/src/main.py b/src/main.py index 5940304..d5a1acf 100644 --- a/src/main.py +++ b/src/main.py @@ -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 diff --git a/src/utils/recorder.py b/src/utils/recorder.py index 5b790c9..468efd5 100644 --- a/src/utils/recorder.py +++ b/src/utils/recorder.py @@ -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" ) diff --git a/src/vision/camera_processor.py b/src/vision/camera_processor.py index 03dee16..ed3fbcf 100644 --- a/src/vision/camera_processor.py +++ b/src/vision/camera_processor.py @@ -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