From 22816a727e1552354ce6aa5ba004a741a31a20c5 Mon Sep 17 00:00:00 2001 From: default Date: Fri, 9 Jan 2026 21:12:06 +0000 Subject: [PATCH] Removed Bashrc modification --- README.md | 10 +-- docs/installation.md | 17 ++--- setup/install_arch.sh | 63 +++++++++++------- setup/install_macos.sh | 3 +- setup/install_ubuntu.sh | 48 +++++++++----- setup/install_windows.ps1 | 2 +- src/drone_controller.py | 132 +++++++++++++++++++++++++++++++++++--- 7 files changed, 214 insertions(+), 61 deletions(-) diff --git a/README.md b/README.md index 333e3f1..0f381ee 100644 --- a/README.md +++ b/README.md @@ -19,8 +19,8 @@ cd RDC_Simulation # Full installation (20-30 minutes) ./setup/install_ubuntu.sh -# Reload environment -source ~/.bashrc +# Activate environment +source activate.sh ``` ### Run the Simulation (3 Terminals) @@ -34,7 +34,7 @@ cd ~/RDC_Simulation **Terminal 2 - Start ArduCopter SITL:** ```bash -source ~/.ardupilot_env +source activate.sh sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console # Wait for "JSON received:" messages (shows Gazebo is connected) ``` @@ -96,14 +96,14 @@ Gazebo isn't sending data to SITL. ### "empy not found" or wrong Python Wrong virtual environment. ```bash -source ~/.ardupilot_env +source activate.sh pip install empy==3.3.4 ``` ### "sim_vehicle.py not found" ArduPilot tools not in PATH. ```bash -source ~/.ardupilot_env +source activate.sh ``` ### Drone won't arm diff --git a/docs/installation.md b/docs/installation.md index 04faeed..507fb25 100644 --- a/docs/installation.md +++ b/docs/installation.md @@ -21,8 +21,8 @@ cd ~/RDC_Simulation # Run full installer (20-30 minutes) ./setup/install_ubuntu.sh -# Reload shell -source ~/.bashrc +# Activate environment +source activate.sh ``` This installs: @@ -54,8 +54,8 @@ git clone ~/RDC_Simulation cd ~/RDC_Simulation ./setup/install_ubuntu.sh -# Reload shell -source ~/.bashrc +# Activate environment +source activate.sh ``` ### WSL2 Tips @@ -73,7 +73,7 @@ source ~/.bashrc ```bash ./setup/install_arch.sh -source ~/.bashrc +source activate.sh ``` ## macOS @@ -92,7 +92,8 @@ macOS doesn't support Gazebo Harmonic. Options: ```bash # Check ArduPilot environment -source ~/.ardupilot_env +# Activate environment & check +source activate.sh python -c "import em; print('empy: OK')" which sim_vehicle.py @@ -114,7 +115,7 @@ Wait for the drone to appear! ### Terminal 2 - SITL ```bash -source ~/.ardupilot_env +source activate.sh sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console ``` Wait for "JSON received:" messages. @@ -131,7 +132,7 @@ python scripts/run_ardupilot.py --pattern square ### "empy not found" ```bash -source ~/.ardupilot_env +source activate.sh pip install empy==3.3.4 ``` diff --git a/setup/install_arch.sh b/setup/install_arch.sh index 5056fa1..a9406d9 100755 --- a/setup/install_arch.sh +++ b/setup/install_arch.sh @@ -108,27 +108,38 @@ echo "[STEP 5/5] Creating activation script..." cat > "$PROJECT_ROOT/activate.sh" << 'EOF' #!/bin/bash -# Drone Simulation - Environment Activation (Arch Linux) +# RDC Simulation - Environment Activation +# Usage: source activate.sh SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" -# Try to source ROS 2 if available (from AUR) -if [ -f "/opt/ros/humble/setup.bash" ]; then - source /opt/ros/humble/setup.bash - echo "[OK] ROS 2 humble sourced" -elif [ -f "/opt/ros/jazzy/setup.bash" ]; then +# ============================================================================= +# 1. ROS 2 Setup +# ============================================================================= +if [ -f "/opt/ros/jazzy/setup.bash" ]; then source /opt/ros/jazzy/setup.bash - echo "[OK] ROS 2 jazzy sourced" + echo "[OK] ROS 2 jazzy" +elif [ -f "/opt/ros/humble/setup.bash" ]; then + source /opt/ros/humble/setup.bash + echo "[OK] ROS 2 humble" else - echo "[INFO] ROS 2 not found - standalone mode available" + echo "[WARN] ROS 2 installation not found in /opt/ros/" fi -# Source ArduPilot environment (if installed) -if [ -f "$HOME/.ardupilot_env" ]; then - source "$HOME/.ardupilot_env" - echo "[OK] ArduPilot environment" -fi +# ============================================================================= +# 2. ArduPilot Environment +# ============================================================================= +export ARDUPILOT_HOME="$HOME/ardupilot" +# Add ArduPilot tools to PATH +# We prepend to ensure we use our versions +export PATH="$ARDUPILOT_HOME/Tools/autotest:$PATH" +export PATH="$ARDUPILOT_HOME/Tools/scripts:$PATH" +export PATH="$HOME/.local/bin:$PATH" + +# ============================================================================= +# 3. Python Virtual Environment +# ============================================================================= # Deactivate any existing venv to avoid conflicts if [ -n "$VIRTUAL_ENV" ]; then deactivate 2>/dev/null || true @@ -137,29 +148,35 @@ fi # Activate project venv if [ -f "$SCRIPT_DIR/venv/bin/activate" ]; then source "$SCRIPT_DIR/venv/bin/activate" - echo "[OK] Python venv activated" + echo "[OK] Python venv active" +else + echo "[WARN] Python venv not found at $SCRIPT_DIR/venv" fi -# Set Gazebo model path if available +# ============================================================================= +# 4. Gazebo & Simulation Paths +# ============================================================================= +# Add project models to Gazebo path export GZ_SIM_RESOURCE_PATH="$SCRIPT_DIR/gazebo/models:$GZ_SIM_RESOURCE_PATH" -# Add paths for ArduPilot tools -export PATH="$PATH:$HOME/.local/bin" -export PATH="$PATH:$HOME/ardupilot/Tools/autotest" - -# ArduPilot Gazebo plugin +# ArduPilot Gazebo Plugin paths if [ -d "$HOME/ardupilot_gazebo/build" ]; then export GZ_SIM_SYSTEM_PLUGIN_PATH="$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH" export GZ_SIM_RESOURCE_PATH="$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH" +else + echo "[WARN] ArduPilot Gazebo plugin not found at $HOME/ardupilot_gazebo" fi echo "" -echo "Environment ready! See README.md for usage instructions." -echo "" +echo "Environment Configured!" +echo "-----------------------" +echo "Project Root: $SCRIPT_DIR" +echo "ArduPilot Home: $ARDUPILOT_HOME" +echo "-----------------------" EOF chmod +x "$PROJECT_ROOT/activate.sh" -echo "[INFO] Created: $PROJECT_ROOT/activate.sh" +echo "[OK] Created: $PROJECT_ROOT/activate.sh" # ----------------------------------------------------------------------------- # Verification diff --git a/setup/install_macos.sh b/setup/install_macos.sh index e584109..05e3c1f 100755 --- a/setup/install_macos.sh +++ b/setup/install_macos.sh @@ -133,7 +133,8 @@ echo "" echo "For the full simulation, use a Linux VM or Docker:" echo " - UTM (Apple Silicon): https://mac.getutm.app" echo " - Docker Desktop: docker run -it ubuntu:22.04" -echo " - Then run: ./setup/install_ubuntu.sh --with-ardupilot" +echo " - Then run: ./setup/install_ubuntu.sh" +echo " - Then: source activate.sh" echo "" echo "Once in Linux, run (3 terminals):" echo "" diff --git a/setup/install_ubuntu.sh b/setup/install_ubuntu.sh index a5a0d91..58114a1 100755 --- a/setup/install_ubuntu.sh +++ b/setup/install_ubuntu.sh @@ -177,22 +177,33 @@ cat > "$PROJECT_ROOT/activate.sh" << 'EOF' SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" -# Source ROS 2 +# ============================================================================= +# 1. ROS 2 Setup +# ============================================================================= if [ -f "/opt/ros/jazzy/setup.bash" ]; then source /opt/ros/jazzy/setup.bash echo "[OK] ROS 2 jazzy" elif [ -f "/opt/ros/humble/setup.bash" ]; then source /opt/ros/humble/setup.bash echo "[OK] ROS 2 humble" +else + echo "[WARN] ROS 2 installation not found in /opt/ros/" fi -# Source ArduPilot environment (if installed) -if [ -f "$HOME/.ardupilot_env" ]; then - source "$HOME/.ardupilot_env" - echo "[OK] ArduPilot environment" -fi +# ============================================================================= +# 2. ArduPilot Environment +# ============================================================================= +export ARDUPILOT_HOME="$HOME/ardupilot" -# Deactivate any existing venv to avoid conflicts +# Add ArduPilot tools to PATH +export PATH="$ARDUPILOT_HOME/Tools/autotest:$PATH" +export PATH="$ARDUPILOT_HOME/Tools/scripts:$PATH" +export PATH="$HOME/.local/bin:$PATH" + +# ============================================================================= +# 3. Python Virtual Environment +# ============================================================================= +# Deactivate any existing venv if [ -n "$VIRTUAL_ENV" ]; then deactivate 2>/dev/null || true fi @@ -200,24 +211,31 @@ fi # Activate project venv if [ -f "$SCRIPT_DIR/venv/bin/activate" ]; then source "$SCRIPT_DIR/venv/bin/activate" - echo "[OK] Python venv" + echo "[OK] Python venv active" +else + echo "[WARN] Python venv not found at $SCRIPT_DIR/venv" fi -# Set Gazebo paths +# ============================================================================= +# 4. Gazebo & Simulation Paths +# ============================================================================= +# Add project models to Gazebo path export GZ_SIM_RESOURCE_PATH="$SCRIPT_DIR/gazebo/models:$GZ_SIM_RESOURCE_PATH" -# Add paths for ArduPilot tools -export PATH="$PATH:$HOME/.local/bin" -export PATH="$PATH:$HOME/ardupilot/Tools/autotest" - -# ArduPilot Gazebo plugin +# ArduPilot Gazebo Plugin paths if [ -d "$HOME/ardupilot_gazebo/build" ]; then export GZ_SIM_SYSTEM_PLUGIN_PATH="$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH" export GZ_SIM_RESOURCE_PATH="$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH" +else + echo "[WARN] ArduPilot Gazebo plugin not found at $HOME/ardupilot_gazebo" fi echo "" -echo "Ready! See README.md for usage instructions." +echo "Environment Configured!" +echo "-----------------------" +echo "Project Root: $SCRIPT_DIR" +echo "ArduPilot Home: $ARDUPILOT_HOME" +echo "-----------------------" EOF chmod +x "$PROJECT_ROOT/activate.sh" diff --git a/setup/install_windows.ps1 b/setup/install_windows.ps1 index 19bad3d..0c0c9eb 100644 --- a/setup/install_windows.ps1 +++ b/setup/install_windows.ps1 @@ -231,7 +231,7 @@ Write-Host "" Write-Host "Step 2: Open Ubuntu and install" -ForegroundColor Yellow Write-Host " cd /mnt/c/path/to/RDC_Simulation" -ForegroundColor White Write-Host " ./setup/install_ubuntu.sh" -ForegroundColor White -Write-Host " source ~/.bashrc" -ForegroundColor White +Write-Host " source activate.sh" -ForegroundColor White Write-Host "" Write-Host "Step 3: Run simulation (3 terminals)" -ForegroundColor Yellow Write-Host " Terminal 1: ./scripts/run_ardupilot_sim.sh runway" -ForegroundColor White diff --git a/src/drone_controller.py b/src/drone_controller.py index 07b5cdd..5b64182 100644 --- a/src/drone_controller.py +++ b/src/drone_controller.py @@ -15,6 +15,13 @@ import os import time import math import argparse +import threading + +try: + import cv2 +except ImportError: + cv2 = None + print("[WARN] opencv-python not installed - camera view will be disabled") sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) @@ -28,6 +35,56 @@ except ImportError: from config import MAVLINK +class CameraViewer(threading.Thread): + def __init__(self, port=5600): + super().__init__() + self.port = port + self.running = True + self.daemon = True + + def run(self): + if cv2 is None: + return + + print(f"[INFO] Waiting for video stream on UDP port {self.port}...") + + # GStreamer pipeline for UDP H.264 stream + # This matches the ArduPilot Gazebo plugin output + pipeline = ( + f"udpsrc port={self.port} ! " + "application/x-rtp, payload=96 ! " + "rtph264depay ! h264parse ! avdec_h264 ! " + "videoconvert ! appsink sync=false" + ) + + try: + cap = cv2.VideoCapture(pipeline, cv2.CAP_GSTREAMER) + + if not cap.isOpened(): + print("[WARN] Could not open video stream (GStreamer Error)") + print("[TIP] Ensure Gazebo is running and GST plugins are installed") + return + + print("[OK] Video stream connected") + + while self.running: + ret, frame = cap.read() + if ret: + cv2.imshow("Drone Camera", frame) + if cv2.waitKey(1) & 0xFF == ord('q'): + self.running = False + break + else: + # No frame received + time.sleep(0.1) + + cap.release() + cv2.destroyAllWindows() + + except Exception as e: + print(f"[ERROR] Camera Viewer failed: {e}") + + class DroneController: """Drone controller with takeoff, fly pattern, and land.""" @@ -203,20 +260,70 @@ class DroneController: # Wait for parameter to be acknowledged time.sleep(0.5) + def setup_geofence(self, max_alt=15.0, max_radius=30.0, action="LAND"): + """Configure the geofence (safety cage). + + Args: + max_alt: Maximum altitude in meters + max_radius: Maximum radius from home in meters + action: Action to take on breach ("RTL", "LAND", "BRAKE", "REPORT") + """ + print(f"[INFO] Setting up Geofence: Max Alt={max_alt}m, Max Radius={max_radius}m") + + # Action map + actions = { + "REPORT": 0, + "RTL": 1, + "LAND": 2, + "SMART_RTL": 3, + "BRAKE": 4 + } + action_val = actions.get(action.upper(), 2) # Default to LAND + + # 1. Enable Fence (0=Disable, 1=Enable) + self.set_param("FENCE_ENABLE", 1) + + # 2. Set Type (1=Alt, 2=Circle, 4=Polygon) -> 3=Alt+Circle + self.set_param("FENCE_TYPE", 3) + + # 3. Set Action + self.set_param("FENCE_ACTION", action_val) + + # 4. Set Limits + self.set_param("FENCE_ALT_MAX", max_alt) + self.set_param("FENCE_RADIUS", max_radius) + + # 5. Set floor margin (optional, default 0) + self.set_param("FENCE_MARGIN", 2.0) + + print(f"[OK] Geofence active. Breach action: {action}") + time.sleep(0.5) + def setup_gps_denied(self): - """Configure for GPS-denied operation.""" - print("[INFO] Configuring for GPS-denied operation...") + """Configure for Simulated GPS-Denied operation. + + NOTE: We ENABLE GPS in the EKF so that the Geofence (Safety Cage) works. + However, the controller uses GUIDED_NOGPS mode and sends velocity/attitude + commands, treating the flight as if it were GPS-denied (no position holds). + """ + print("[INFO] Configuring for Simulated GPS-Denied operation...") # Disable all arming checks for testing self.set_param("ARMING_CHECK", 0) - # Configure EKF3 for non-GPS operation - # These parameters tell the EKF to not require GPS - self.set_param("EK3_SRC1_POSXY", 0) # 0 = None (no GPS for horizontal position) - self.set_param("EK3_SRC1_VELXY", 0) # 0 = None (no GPS for horizontal velocity) - self.set_param("EK3_SRC1_POSZ", 1) # 1 = Baro (use barometer for altitude) + # Configure EKF3 - WE ENABLE GPS HERE FOR GEOFENCE SAFETY + # 3 = GPS (Enabled for safety/fence) + self.set_param("EK3_SRC1_POSXY", 3) + self.set_param("EK3_SRC1_VELXY", 3) + self.set_param("EK3_SRC1_POSZ", 1) # 1 = Baro - print("[OK] GPS-denied parameters set") + # Disable GPS Failsafe (so we don't land if GPS is bad, since we 'pretend' to be denied) + self.set_param("FS_GPS_ENABLE", 0) + + # Set a reasonable geofence by default + self.setup_geofence(max_alt=10.0, max_radius=20.0, action="LAND") + + print("[OK] EKF configured (GPS enabled for Fence only)") # Wait longer for parameters to take effect time.sleep(1.0) @@ -563,9 +670,14 @@ def main(): drone = DroneController(args.connection) + # Start Camera Viewer + camera = CameraViewer() + camera.start() + if not drone.connect(): print("\n[ERROR] Could not connect to SITL") print("Make sure sim_vehicle.py is running") + camera.running = False # Stop camera sys.exit(1) try: @@ -625,6 +737,10 @@ def main(): except KeyboardInterrupt: print("\n\n[INFO] Interrupted - Landing...") drone.land() + finally: + camera.running = False # Stop camera thread + if camera.is_alive(): + camera.join(timeout=1.0) if __name__ == "__main__":