diff --git a/config/search.yaml b/config/search.yaml index c764dfd..f648ebf 100644 --- a/config/search.yaml +++ b/config/search.yaml @@ -4,7 +4,6 @@ marker: size: 0.5 landing_ids: [0] target_ids: [1] - target_position: [5.0, 3.0] spiral: max_legs: 35 lawnmower: diff --git a/docs/architecture.md b/docs/architecture.md index ee65b5d..50920af 100644 --- a/docs/architecture.md +++ b/docs/architecture.md @@ -141,22 +141,24 @@ src/ ├── main.py # Entry point ├── vision/ │ ├── object_detector.py # ArUco marker detection -│ ├── visual_odometry.py # Feature tracking VO -│ ├── optical_flow.py # LK optical flow │ └── camera_processor.py # Gazebo camera feeds -├── localization/ -│ ├── position_estimator.py # Weighted fusion -│ └── ekf_fusion.py # EKF fusion ├── navigation/ -│ ├── local_planner.py # Path planning -│ └── waypoint_follower.py # Waypoint tracking +│ ├── search.py # Search algorithm controller +│ ├── flight_tracker.py # Live flight visualization +│ └── patterns/ +│ ├── spiral.py # Expanding square spiral +│ ├── lawnmower.py # Back-and-forth lanes +│ └── levy.py # Lévy walk random search ├── control/ -│ ├── uav_controller.py # UAV flight control -│ ├── ugv_controller.py # UGV drive control -│ └── search.py # Search algorithms -└── safety/ - ├── geofence_monitor.py # GPS boundaries - └── failsafe_handler.py # Emergency handling +│ ├── uav_controller.py # UAV flight control (pymavlink) +│ └── ugv_controller.py # UGV drive control (gz.transport) +├── safety/ +│ └── geofence.py # GPS boundary enforcement +└── utils/ + ├── config.py # YAML config loader + ├── convert.py # Unit/coordinate conversions + ├── helpers.py # Distance utilities + └── recorder.py # Video recording & cloud upload ``` ## Configuration diff --git a/docs/troubleshooting.md b/docs/troubleshooting.md index cfee2e3..2434f89 100644 --- a/docs/troubleshooting.md +++ b/docs/troubleshooting.md @@ -136,10 +136,6 @@ source activate_venv.sh pip install pexpect future ``` -source activate_venv.sh -pip install pexpect future -``` - ### Build Failed (Missing catkin_pkg) **Symptoms:** diff --git a/scripts/run_autonomous.sh b/scripts/run_autonomous.sh index 09e6561..f3e3945 100755 --- a/scripts/run_autonomous.sh +++ b/scripts/run_autonomous.sh @@ -98,7 +98,7 @@ print_info "===================================" print_info " UAV-UGV Simulation" print_info "===================================" print_info "World: $WORLD_FILE" -print_info "Mission: $MISSION" +print_info "Search: $SEARCH" echo "" GZ_DEFAULT_GUI="/usr/share/gz/gz-sim8/gui/gui.config" GZ_USER_GUI="$HOME/.gz/sim/8/gui.config" @@ -147,12 +147,19 @@ if [ -f "$PARAM_FILE" ]; then fi sim_vehicle.py $SITL_ARGS & SITL_PID=$! -print_info "Waiting for SITL (~20s) ..." -sleep 20 -if ! pgrep -f "arducopter" > /dev/null 2>&1; then - print_error "ArduPilot SITL failed to start" - exit 1 -fi +print_info "Waiting for SITL to start ..." +SITL_TIMEOUT=60 +SITL_WAITED=0 +while ! pgrep -f "arducopter" > /dev/null 2>&1; do + sleep 2 + SITL_WAITED=$((SITL_WAITED + 2)) + if [ $SITL_WAITED -ge $SITL_TIMEOUT ]; then + print_error "ArduPilot SITL failed to start after ${SITL_TIMEOUT}s" + exit 1 + fi + print_info " Waiting for arducopter process ... (${SITL_WAITED}s)" +done +sleep 5 print_success "ArduPilot SITL running (TCP 5760)" print_info "[3/4] Starting main.py ..." cd "$PROJECT_DIR" diff --git a/src/navigation/search.py b/src/navigation/search.py index c5475b5..3abb9e2 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -414,8 +414,6 @@ class Search: print() return False - return False - def move_to_local(self, x, y): z = -self.altitude self.ctrl.move_local_ned(x, y, z) diff --git a/src/utils/recorder.py b/src/utils/recorder.py index f7b2fa4..5b790c9 100644 --- a/src/utils/recorder.py +++ b/src/utils/recorder.py @@ -337,7 +337,40 @@ class RunRecorder: threading.Thread(target=self._upload_file, args=(path, filename), daemon=True).start() def save_summary(self, search_mode="", altitude=0, markers=None, landed=False, ugv_dispatched=False, ugv_target=None, extra=None): - pass + duration = time.time() - self.start_time + summary = { + "search_mode": search_mode, + "altitude": round(altitude, 2), + "duration_seconds": round(duration, 1), + "search_duration_seconds": round(self.search_duration, 1), + "landed": landed, + "ugv_dispatched": ugv_dispatched, + "markers_found": 0, + "markers": {}, + "timestamp": datetime.now().isoformat(), + } + if ugv_target: + summary["ugv_target"] = ugv_target + if markers: + summary["markers_found"] = len(markers) + for mid, info in markers.items(): + pos = info.get("uav_position", {}) + summary["markers"][int(mid)] = { + "x": round(pos.get("x", 0), 2), + "y": round(pos.get("y", 0), 2), + "distance": round(info.get("distance", 0), 2), + } + if extra: + summary.update(extra) + + path = self.run_dir / "summary.yaml" + try: + with open(path, "w") as f: + yaml.dump(summary, f, default_flow_style=False, sort_keys=False) + print(f"[REC] Summary saved: {path}") + threading.Thread(target=self._upload_file, args=(path, "summary.yaml"), daemon=True).start() + except Exception as e: + print(f"[REC] Failed to save summary: {e}") def stop(self): self._recording = False