From 386dd4b9e83c43a830c4861f76c4509669e41688 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Thu, 7 Mar 2019 15:27:22 +0300 Subject: [PATCH] New commands for server+client, server ip autosearch, little improvements --- Drone/client.py | 42 ++++++++++++++++++++++------- Drone/play_animation.py | 22 +++++++--------- Server/server.py | 58 +++++++++++++++++++++++++++++++++++------ 3 files changed, 91 insertions(+), 31 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index 8d10610c..0c9e2555 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -31,19 +31,36 @@ def get_ntp_time(host, port): return unpacked[10] + float(unpacked[11]) / 2**32 - NTP_DELTA -def reconnect(t=1): +def reconnect(t=2): + global clientSocket, host, port print("Trying to connect to", host, ":", port, "...") connected = False - global clientSocket + attempt_count = 0 while not connected: + print("Waiting for connection, attempt", attempt_count) try: clientSocket = socket.socket() + clientSocket.settimeout(3) clientSocket.connect((host, port)) connected = True print("Connection successful") except socket.error as e: print("Waiting for connection:", e) time.sleep(t) + attempt_count +=1 + if attempt_count >= 15: + print("Too many attempts. Trying to get new server IP") + broadcst_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + broadcst_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + broadcst_client.bind(("", 8181)) + while True: + data, addr = broadcst_client.recvfrom(1024) + print("Recieved broadcast message %s from %s"%(data, addr)) + if parse_command(data.decode("UTF-8"))[0] == "server_ip": + print("Binding to new IP: ", addr) + host, port = addr + broadcst_client.close() + break def send_all(msg): @@ -100,16 +117,15 @@ def write_to_config(section, option, value): def animation_player(running_event, stop_event): print("Animation thread activated") - rate = rospy.Rate(1000 / 100) - # first_frame = play_animation.get_frames()[0] - # play_animation.takeoff(round(float(first_frame['x']), 4), round(float(first_frame['y']), 4), round(float(first_frame['z']), 4)) + frames = play_animation.read_animation_file() + rate = rospy.Rate(1000 / 125) play_animation.takeoff() - for current_frame in play_animation.get_frames(): + for frame in frames: running_event.wait() if stop_event.is_set(): break - play_animation.do_next_animation(current_frame) + play_animation.animate_frame(frame) rate.sleep() else: play_animation.land() @@ -200,16 +216,22 @@ def stop_animation(): dt = starttime - get_ntp_time(NTP_HOST, NTP_PORT) print("Until start:", dt) rospy.Timer(rospy.Duration(dt), start_animation, oneshot=True) + elif command == 'takeoff': + play_animation.takeoff() elif command == 'stop': stop_animation() - #FlightLib.takeoff(2) #FlightLib.reach(5, 5, 2) + elif command == 'land': + FlightLib.land1() # TODO dont forget change back to land + elif command == 'disarm': + FlightLib.arming(False) + elif command == 'request': request_target = args[0] print("Got request for:", request_target) response = "" - if request_target == 'someshit': - response = "dont_have_any" + if request_target == 'test': + response = "test_succsess" elif request_target == 'id': response = COPTER_ID send_all(bytes(form_command("response", response))) diff --git a/Drone/play_animation.py b/Drone/play_animation.py index 9b58b39f..a66c2728 100644 --- a/Drone/play_animation.py +++ b/Drone/play_animation.py @@ -6,14 +6,13 @@ from FlightLib.FlightLib import LedLib animation_file_path = 'test_animation/test_1.csv' -frames = [] USE_LEDS = True def takeoff(): if USE_LEDS: LedLib.wipe_to(255, 0, 0) - FlightLib.takeoff1() + FlightLib.takeoff1() # TODO dont forget change back to takeoff def land(): @@ -24,20 +23,21 @@ def land(): LedLib.off() -def do_next_animation(current_frame, x0 = 0, y0 = 0): - FlightLib.navto(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'], yaw = 1.57) +def animate_frame(current_frame, x0=0.0, y0=0.0): + FlightLib.navto(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'], yaw=1.57) if USE_LEDS: LedLib.fill(current_frame['red'], current_frame['green'], current_frame['blue']) def read_animation_file(filepath=animation_file_path): + imporetd_frames = [] with open(filepath) as animation_file: csv_reader = csv.reader( animation_file, delimiter=',', quotechar='|' ) for row in csv_reader: frame_number, x, y, z, yaw, red, green, blue = row - frames.append({ + imporetd_frames.append({ 'number': int(frame_number), 'x': float(x), 'y': float(y), @@ -47,11 +47,7 @@ def read_animation_file(filepath=animation_file_path): 'green': int(green), 'blue': int(blue), }) - - -def get_frames(): - global frames - return frames + return imporetd_frames if __name__ == '__main__': @@ -60,12 +56,12 @@ def get_frames(): LedLib.init_led() X0 = 0.5 Y0 = 1.0 - read_animation_file() + frames = read_animation_file() rate = rospy.Rate(8) takeoff() - FlightLib.reach(x=frames[0]['x']+X0, y=frames[0]['y']+Y0, z=frames[0]['z'], yaw = 1.57) + FlightLib.reach(x=frames[0]['x']+X0, y=frames[0]['y']+Y0, z=frames[0]['z'], yaw=11.57) for frame in frames: - do_next_animation(frame, x0 = X0, y0 = Y0) + animate_frame(frame, x0=X0, y0=Y0) rate.sleep() land() time.sleep(1) diff --git a/Server/server.py b/Server/server.py index 6f331358..35089c1d 100644 --- a/Server/server.py +++ b/Server/server.py @@ -37,6 +37,17 @@ def auto_connect(): Client.clients[addr[0]].connect(c, addr) +def ip_broadcast(ip): + ip = ip + broadcast_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + broadcast_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + broadcast_sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + while True: + msg = bytes(Client.form_command("server_ip ", ip), "UTF-8") + broadcast_sock.sendto(msg, ('255.255.255.255', 8181)) #TODO to config + time.sleep(5) + + NTP_DELTA = 2208988800 # 1970-01-01 00:00:00 NTP_QUERY = b'\x1b' + bytes(47) @@ -77,7 +88,7 @@ def __init__(self, ip): def connect(self, client_socket, client_addr): print("Client connected") - self._send_queue = collections.deque() # comment for resuming queue after reconnection + # self._send_queue = collections.deque() # comment for resuming queue after reconnection self.socket = client_socket self.addr = client_addr @@ -117,7 +128,12 @@ def _run(self): if self._send_queue: msg = self._send_queue.popleft() print("Send", msg, "to", self.addr) - self._send_all(msg) + try: + self._send_all(msg) + except socket.error as e: + print("Attempt to send failed") + self._send_queue.appendleft(msg) + raise e else: msg = "ping" # self._send_all(msg) @@ -142,7 +158,7 @@ def _run(self): pass except socket.error as e: - print("Client error, disconnected", e) + print("Client error: {}, disconnected".format(e)) self.connected = False self.socket.close() break @@ -165,9 +181,12 @@ def send(self, *messages): @staticmethod def broadcast(message, force_all=False): - for client in Client.clients.values(): - if (not client.malfunction) or force_all: - client.send(message) + if Client.clients: + for client in Client.clients.values(): + if (not client.malfunction) or force_all: + client.send(message) + else: + print("No clients were connected!") @requires_connect def send_file(self, filepath, dest_filename): @@ -198,6 +217,18 @@ def stop_swarm(): Client.broadcast("stop") # для тестирования +def land_all(): + Client.broadcast("land") + + +def disarm_all(): + Client.broadcast("disarm") + + +def takeoff_all(): + Client.broadcast("takeoff") + + def send_animations(): path = filedialog.askdirectory(title="Animation directory") if path: @@ -245,14 +276,24 @@ def send_starttime(dt=15): button_frame = Frame(leftFrame, borderwidth=1, relief="solid") button_frame.pack(fill=BOTH, expand=True) +land_all_btn = ttk.Button(button_frame, text="Disarm all", command=disarm_all) +land_all_btn.pack(side=RIGHT, padx=5, pady=5) + +land_all_btn = ttk.Button(button_frame, text="Land all", command=land_all) +land_all_btn.pack(side=RIGHT, padx=5, pady=5) + stop_all_btn = ttk.Button(button_frame, text="Stop swarm", command=stop_swarm) stop_all_btn.pack(side=RIGHT, padx=5, pady=5) + send_animation_btn = ttk.Button(button_frame, text="Send animations", command=send_animations) -send_animation_btn.pack(side=RIGHT, padx=5, pady=5) +send_animation_btn.pack(side=LEFT, padx=5, pady=5) + +send_starttime_btn = Button(button_frame, bg='red', text="Takeoff all", command=takeoff_all) +send_starttime_btn.pack(side=LEFT, padx=5, pady=5) send_starttime_btn = ttk.Button(button_frame, text="Start animation after...", command=send_starttime) -send_starttime_btn.pack(side=RIGHT, padx=5, pady=5) +send_starttime_btn.pack(side=LEFT, padx=5, pady=5) def gui_update(): @@ -283,6 +324,7 @@ def gui_update(): autoconnect_thread.daemon = True autoconnect_thread.start() +broadcast_thread = threading.Thread(target=ip_broadcast, args=(ip, port, )) if __name__ == '__main__': try: