diff options
Diffstat (limited to 'server.py')
| -rw-r--r-- | server.py | 149 |
1 files changed, 130 insertions, 19 deletions
@@ -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) |