Removed Bashrc modification
This commit is contained in:
@@ -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__":
|
||||
|
||||
Reference in New Issue
Block a user