-
Notifications
You must be signed in to change notification settings - Fork 0
/
float_uav.py
54 lines (48 loc) · 1.79 KB
/
float_uav.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
import time
import signal
import cv2
from apriltag import apriltag
import ps_drone
BLUE = 255, 0, 0
LAST_MARKER_ID = 1
IMAGE_SIZE = (640, 360)
# X-axis threshold, altitude, and altitude threshold for navigation
NAV_DATA = (20, 500, 20)
# X- and Y-axis threshold, altitude and altitude threshold for alignment
ALIGN_DATA = (30, 30, 1120, 30)
# Known vlaues for x, y, z-position of each marker and yaw needed for path to
# next marker. The position of each data tuple corresponds with the marker ID.
MARKER_DATA = ((-115, -75, 145, -0.350), ("""x, y, z, yawForNextMarker""")) # TODO: fill in
drone = ps_drone.Drone()
# Allows manual override shutdown
def exit_gracefully(signal, frame):
print("Shutting down")
#drone.shutdown()
exit()
signal.signal(signal.SIGQUIT, exit_gracefully)
def detect(camera, detector):
while cv2.waitKey(1) != 0x1b:
print("Starting detection...")
ret, img = camera.read()
image = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
detection = detector.detect(image)
for det in detection:
if det["margin"] >= 10:
print("Marker detected!")
rect = det["lb-rb-rt-lt"].astype(int).reshape((-1, 1, 2))
cv2.polylines(img, [rect], True, BLUE, 2)
ident = str(det["id"])
pos = det["center"].astype(int) + (-10, 10)
cv2.putText(img, ident, tuple(pos), cv2.FONT_HERSHEY_SIMPLEX, 1, BLUE, 2)
#print(detection, "\n\nEnding detection...")
#return detection[0]
cv2.imshow("IMG", img)
print(detection, "\n\nEnding detection...")
return detection
def main():
camera = cv2.VideoCapture(0)
detector = apriltag("tagStandard41h12")
detect(camera, detector)
cv2.destroyAllWindows()
print("Done")
main()