summaryrefslogtreecommitdiff
path: root/server.py
diff options
context:
space:
mode:
Diffstat (limited to 'server.py')
-rw-r--r--server.py149
1 files changed, 130 insertions, 19 deletions
diff --git a/server.py b/server.py
index 60e03df..4233553 100644
--- a/server.py
+++ b/server.py
@@ -6,6 +6,10 @@ import socket
import sys
import threading
import time
+import struct
+
+def putStatusText(img, text, pos, activated):
+ cv2.putText(img, text, pos, cv2.FONT_HERSHEY_PLAIN, 1, (0,0,255) if activated else (127,127,127), 2 if activated else 1)
def encode_int(i):
i = int(i)
@@ -22,15 +26,22 @@ class ServerThread(threading.Thread):
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);
+ framestr = global_frame.tostring()
+ lenframestr=len(global_framestr)
+ connection.sendall(struct.pack(">i",lenframestr)+framestr+struct.pack("@dddd", global_phi, global_theta, global_psi, global_batt));
+ lock.release()
+ elif data[0:3] == "fly" and data[-1]=="\n":
+ values = data[3:-1].split()
+ lock.acquire()
+ global_cmd_x = float(values[0])
+ global_cmd_y = float(values[1])
+ global_cmd_z = float(values[2])
+ global_cmd_rot = float(values[3])
lock.release()
else:
print >>sys.stderr, 'no more data from', client_address
@@ -62,6 +73,7 @@ sock.bind(server_address)
# Listen for incoming connections
sock.listen(1)
+no_flight = False
try:
pygame.init()
@@ -70,7 +82,12 @@ try:
js.init()
js_angle_shift = 0.0
except:
- print "meeeeh"
+ print "no joystick! disabling flight controls"
+ no_flight = True
+
+manual_override_xy = True
+manual_override_z = True
+manual_override_rot = True
drone = libardrone.ARDrone(True, True)
@@ -80,31 +97,125 @@ 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")
-
+global_phi = 0.
+global_theta = 0.
+global_psi = 0.
+global_batt = 0.
+global_frame = None
serverthread.start()
while True:
- print "hello world :)"
+ if no_flight == False:
+ btn_leftshoulder = js.get_button(4) or js.get_button(6)
+ btn_rightshoulder = js.get_button(5) or js.get_button(7)
+ btn_thumb = js.get_button(0) or js.get_button(1) or js.get_button(2) or js.get_button(3)
+ btn_all = js.get_button(0) and js.get_button(1) and js.get_button(2) and js.get_button(3)
+ btn_readjust = js.get_button(10)
+
+ if btn_thumb:
+ drone.land()
+ manual_override_xy = True
+ manual_override_z = True
+ manual_override_rot = True
+ if btn_leftshoulder and btn_rightshoulder and js.get_button(10):
+ drone.takeoff()
+ manual_override_xy = True
+ manual_override_z = True
+ manual_override_rot = True
+ if btn_all:
+ drone.reset()
+ manual_override_xy = True
+ manual_override_z = True
+ manual_override_rot = True
+ if btn_readjust:
+ js_angle_shift = drone.navdata.get(0, dict()).get('psi',0)
+
+ rel_angle = (drone.navdata.get(0, dict()).get('psi',0) - js_angle_shift)/180.*math.pi
+
+ js_x = js.get_axis(0)
+ js_y = js.get_axis(1)
+ js_z = -js.get_axis(4)
+ js_rot = js.get_axis(3)
+ js_radius = math.sqrt(js_x**2 + js_y**2)
+ if btn_leftshoulder==0:
+ js_x, js_y = ( js_x * cos(rel_angle) + js_y * sin(rel_angle) ) , ( -js_x * sin(rel_angle) + js_y * cos(rel_angle) )
+
+ js_hover = (btn_rightshoulder==0 and (js_radius <= 0.01))
+
+ if (js_radius > OVERRIDE_THRESHOLD): manual_override_xy = True
+ if (abs(js_z) > OVERRIDE_THRESHOLD): manual_override_z = True
+ if (abs(js_rot) > OVERRIDE_THRESHOLD): manual_override_rot = True
+
+ if manual_override_xy:
+ actual_hover, actual_x, actual_y = js_hover, js_x, js_y
+ else:
+ actual_hover, actual_x, actual_y = global_cmd_hover, global_cmd_x, global_cmd_y
+
+ if manual_override_z:
+ actual_z = js_z
+ else:
+ actual_z = global_cmd_z
+
+ if manual_override_rot:
+ actual_rot = js_rot
+ else:
+ actual_rot = global_cmd_rot
+
+ drone.move_freely( not actual_hover , actual_x, actual_y, actual_z, actual_rot)
+
+
+
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)
+ global_frame = cv2.cvtColor(rawimg, cv2.COLOR_BGR2RGB)
+ global_phi = drone.navdata.get(0, dict()).get('phi',1337)
+ global_theta = drone.navdata.get(0, dict()).get('theta',1337)
+ global_psi = drone.navdata.get(0, dict()).get('psi',1337)
+ global_batt = drone.navdata.get(0, dict()).get('batt',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
+ smallframe = cv2.resize(global_frame, (640,480))
+ cv2.rectangle(smallframe, (0,0), (640,30), (255,255,255), -1)
+ cv2.putText(smallframe, "override", (0,20), cv2.FONT_HERSHEY_PLAIN, 1, (255,0,0))
+ putStatusText(smallframe, "XY", (100,20), manual_override_xy)
+ putStatusText(smallframe, "height", (200,20), manual_override_z)
+ putStatusText(smallframe, "rotation", (300,20), manual_override_rot)
+ #cv2.putText(smallframe, "XY", (100,20), cv2.FONT_HERSHEY_PLAIN, 1, (0,0,255) if manual_override_xy else (127,127,127))
+ #cv2.putText(smallframe, "height", (250,20), cv2.FONT_HERSHEY_PLAIN, 1, (0,0,255) f manual_override_z else (127,127,127))
+ #cv2.putText(smallframe, "rotation", (400,20), cv2.FONT_HERSHEY_PLAIN, 1, (0,0,255) if manual_override_rot else (127,127,127))
+ cv2.imshow("frame", smallframe)
+ writer.write(global_frame)
+ logfile.write(str(global_phi)+"\t"+str(global_theta)+"\t"+str(global_psi)+"\n")
+ logfile.flush()
+
+
+
+ key = cv2.waitKey(10) & 0xFF
+
+ if key == ord("t"):
+ drone.trim()
+
+ if key == ord("z"):
+ drone.set_max_vz(750.0000)
+ drone.set_max_rotspeed(1.0)
+ drone.set_max_angle(0.1)
+ print "slow"
+ elif key == ord("a"):
+ drone.set_max_vz(10000.0000)
+ drone.set_max_rotspeed(10)
+ drone.set_max_angle(0.6)
+ print "fast"
+
+ if key == ord("1"):
+ manual_override_xy = False
+ elif key == ord("2"):
+ manual_override_z = False
+ elif key == ord("3"):
+ manual_override_rot = False
- cv2.waitKey(50)