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
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
|
import libardrone.libardrone as libardrone
import pygame
import cv2
import os
import socket
import sys
import threading
import time
def encode_int(i):
i = int(i)
return chr( (i/(2**24))%256) + chr( (i/(2**16))%256 ) +\
chr( (i/(2**8))%256) + chr(i%256)
class ServerThread(threading.Thread):
def run(self):
while True:
# Wait for a connection
print >>sys.stderr, 'waiting for a connection'
connection, client_address = sock.accept()
try:
print >>sys.stderr, 'connection from', client_address
# Receive the data in small chunks and retransmit it
while True:
data = connection.recv(16)
if data:
if data=="get\n":
lock.acquire()
framestr = frame.tostring()
lenframestr=len(framestr)
connection.sendall(encode_int(lenframestr)+framestr);
lock.release()
else:
print >>sys.stderr, 'no more data from', client_address
break
except:
print "Dingens!!11!1!!!"
finally:
# Clean up the connection
connection.close()
server_address = '/home/flo/uds_socket'
try:
os.unlink(server_address)
except OSError:
if os.path.exists(server_address):
raise
# Create a UDS socket
sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
# Bind the socket to the port
print >>sys.stderr, 'starting up on %s' % server_address
sock.bind(server_address)
# Listen for incoming connections
sock.listen(1)
try:
pygame.init()
pygame.joystick.init()
js=pygame.joystick.Joystick(0)
js.init()
js_angle_shift = 0.0
except:
print "meeeeh"
drone = libardrone.ARDrone(True, True)
drone.reset()
serverthread=ServerThread()
lock=threading.Lock()
cap = cv2.VideoCapture("/home/flo/kruschkram/out2.avi")
status, frame = cap.read()
writer = cv2.VideoWriter("flight.avi",cv2.VideoWriter_fourcc(*'MP42'),25,(1280,720),1)
logfile = open("flight.log", "w")
serverthread.start()
while True:
print "hello world :)"
lock.acquire()
rawimg = drone.get_image()
frame = cv2.cvtColor(rawimg, cv2.COLOR_BGR2RGB)
phi = drone.navdata.get(0, dict()).get('phi',1337)
theta = drone.navdata.get(0, dict()).get('theta',1337)
psi = drone.navdata.get(0, dict()).get('psi',1337)
lock.release()
cv2.imshow("frame", frame)
writer.write(frame)
logfile.write(str(phi)+"\t"+str(theta)+"\t"+str(psi)+"\n")
print phi,theta,psi
cv2.waitKey(50)
|
