diff --git a/Drone/client.py b/Drone/client.py index 16b6023..b51cf36 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -5,10 +5,12 @@ import struct import random import time import errno +import json import logging import threading import ConfigParser from contextlib import closing +from collections import OrderedDict import rospy @@ -33,9 +35,9 @@ NTP_DELTA = 2208988800L # 1970-01-01 00:00:00 NTP_QUERY = '\x1b' + 47 * '\0' -def get_ntp_time(host, port): +def get_ntp_time(ntp_host, ntp_port): with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as s: - s.sendto(NTP_QUERY, (host, port)) + s.sendto(NTP_QUERY, (ntp_host, ntp_port)) msg, address = s.recvfrom(1024) unpacked = struct.unpack(NTP_PACKET_FORMAT, msg[0:struct.calcsize(NTP_PACKET_FORMAT)]) return unpacked[10] + float(unpacked[11]) / 2**32 - NTP_DELTA @@ -62,18 +64,17 @@ def reconnect(timeout=2, attempt_limit=10): if attempt_count >= attempt_limit: 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(("", broadcast_port)) + broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + broadcast_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + broadcast_client.bind(("", broadcast_port)) while True: - data, addr = broadcst_client.recvfrom(1024) - print("Recieved broadcast message %s from %s" % (data, addr)) - comm, args = parse_command(data.decode("UTF-8")) - print(comm, args) - if comm == "server_ip": - host, port = args[0], int(args[1]) + data, addr = broadcast_client.recvfrom(1024) + print("Received broadcast message %s from %s" % (data, addr)) + command, args = parse_message(data.decode("UTF-8")) + if command == "server_ip": + host, port = args["host"], int(args["port"]) print("Binding to new IP: ", host, port) - broadcst_client.close() + broadcast_client.close() attempt_count = 0 break @@ -100,35 +101,39 @@ def recive_message(): return msg -def form_command(command, args=()): - return " ".join([command, args]) +def form_message(str_command, dict_arguments): + msg_dict = {str_command: str(dict_arguments).replace(",", '').replace("'", '')[1:-1]} + msg = json.dumps(msg_dict) + return msg -def parse_command(command_input): - args = command_input.split() - command = args.pop(0) - return command, args +def parse_message(msg): + try: + j_message = json.loads(msg) + except json.decoder.JSONDecodeError: + print("Json string not in correct format") + return None + + str_command = list(j_message.keys())[0] + + arguments = list(j_message.values())[0].replace(":", '').split() + dict_arguments = OrderedDict(zip(arguments[::2], arguments[1::2])) + return str_command, dict_arguments def recive_file(filename): - print("Reciving file:", filename) + print("Receiving file:", filename) with open(filename, 'wb') as file: # TODO add directory while True: data = recive_message() #clientSocket.recv(BUFFER_SIZE) if data: print(data) - if parse_command(data.decode("UTF-8"))[0] == "/endoffile": - print("File recived") + if parse_message(data.decode("UTF-8"))[0] == "/endoffile": + print("File received") break file.write(data) -def write_to_config(section, option, value): - config.set(section, option, value) - with open(CONFIG_PATH, 'w') as file: - config.write(file) - - def animation_player(running_event, stop_event): print("Animation thread activated") frames = play_animation.read_animation_file() @@ -183,33 +188,48 @@ def selfcheck(): return FlightLib.selfcheck(), telemetry.voltage -CONFIG_PATH = "client_config.ini" -config = ConfigParser.ConfigParser() -config.read(CONFIG_PATH) +def write_to_config(section, option, value): + config.set(section, option, value) + with open(CONFIG_PATH, 'w') as file: + config.write(file) -broadcast_port = config.getint('SERVER', 'broadcast_port') -port = config.getint('SERVER', 'port') -host = config.get('SERVER', 'host') -BUFFER_SIZE = config.getint('SERVER', 'buffer_size') -USE_NTP = config.getboolean('NTP', 'use_ntp') -NTP_HOST = config.get('NTP', 'host') -NTP_PORT = config.getint('NTP', 'port') -files_directory = config.get('FILETRANSFER', 'files_directory') -animation_file = config.get('FILETRANSFER', 'animation_file') +def load_config(): + global config, CONFIG_PATH + global broadcast_port, port, host, BUFFER_SIZE + global USE_NTP, NTP_HOST, NTP_PORT + global files_directory, animation_file + global TAKEOFF_HEIGHT, TAKEOFF_TIME + global USE_LEDS, COPTER_ID + CONFIG_PATH = "client_config.ini" + config = ConfigParser.ConfigParser() + config.read(CONFIG_PATH) -TAKEOFF_HEIGHT = config.getfloat('COPTERS', 'takeoff_height') -TAKEOFF_TIME = config.getint('COPTERS', 'takeoff_time') + broadcast_port = config.getint('SERVER', 'broadcast_port') + port = config.getint('SERVER', 'port') + host = config.get('SERVER', 'host') + BUFFER_SIZE = config.getint('SERVER', 'buffer_size') + USE_NTP = config.getboolean('NTP', 'use_ntp') + NTP_HOST = config.get('NTP', 'host') + NTP_PORT = config.getint('NTP', 'port') -USE_LEDS = config.getboolean('PRIVATE', 'use_leds') -play_animation.USE_LEDS = USE_LEDS + files_directory = config.get('FILETRANSFER', 'files_directory') + animation_file = config.get('FILETRANSFER', 'animation_file') -COPTER_ID = config.get('PRIVATE', 'id') -if COPTER_ID == 'default': - COPTER_ID = 'copter' + str(random.randrange(9999)).zfill(4) - write_to_config('PRIVATE', 'id', COPTER_ID) -elif COPTER_ID == '/hostname': - COPTER_ID = socket.gethostname() + TAKEOFF_HEIGHT = config.getfloat('COPTERS', 'takeoff_height') + TAKEOFF_TIME = config.getint('COPTERS', 'takeoff_time') + + USE_LEDS = config.getboolean('PRIVATE', 'use_leds') + play_animation.USE_LEDS = USE_LEDS + + COPTER_ID = config.get('PRIVATE', 'id') + if COPTER_ID == 'default': + COPTER_ID = 'copter' + str(random.randrange(9999)).zfill(4) + write_to_config('PRIVATE', 'id', COPTER_ID) + elif COPTER_ID == '/hostname': + COPTER_ID = socket.gethostname() + +load_config() rospy.init_node('Swarm_client', anonymous=True) if USE_LEDS: @@ -230,12 +250,12 @@ try: message = recive_message() if message: message = message.decode("UTF-8") - command, args = parse_command(message) + command, args = parse_message(message) print("Command from server:", command, args) if command == "writefile": - recive_file(args[0]) + recive_file(list(args.values)[0]) elif command == "starttime": - starttime = float(args[0]) + starttime = float(list(args.values)[0]) print("Starting on:", time.ctime(starttime)) dt = starttime - get_ntp_time(NTP_HOST, NTP_PORT) print("Until start:", dt) @@ -250,12 +270,12 @@ try: stop_animation() FlightLib.interrupt() elif command == 'land': - play_animation.land() # TODO dont forget change back to land + play_animation.land() elif command == 'disarm': FlightLib.arming(False) elif command == 'request': - request_target = args[0] + request_target = args['value'] print("Got request for:", request_target) response = "" if request_target == 'test': @@ -264,7 +284,7 @@ try: response = COPTER_ID elif request_target == 'selfcheck': response = selfcheck() - send_all(bytes(form_command("response", response))) + send_all(bytes(form_message("response", {"status": "ok", "value": response}))) print("Request responded with:", response) except socket.error as e: if e.errno != errno.EINTR: diff --git a/Server/server_qt.py b/Server/server_qt.py index 98b297d..46a520a 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -15,6 +15,7 @@ import os import sys import glob import time +import json import struct import socket import threading @@ -48,7 +49,7 @@ def auto_connect(): def ip_broadcast(server_ip, server_port): - msg = bytes(Client.form_command("server_ip ", (server_ip, str(server_port))), "UTF-8") + msg = bytes(Client.form_message("server_ip ", {"host": server_ip, "port": str(server_port)}), "UTF-8") 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) @@ -154,12 +155,12 @@ class Client: received = self._receive_message() if received: received = received.decode("UTF-8") - print("Recived", received, "from", self.addr) - command, args = Client.parse_command(received) + print("Received", received, "from", self.addr) + command, args = Client.parse_message(received) if command == "response": for key, value in self._request_queue.items(): if not value: - self._request_queue[key] = args[0] + self._request_queue[key] = args['value'] print("Request successfully closed") break else: @@ -175,14 +176,26 @@ class Client: # time.sleep(0.05) @staticmethod - def form_command(command: str, args=()): # Change for different protocol - return " ".join([command, *args]) + def form_message(command: str, dict_arguments: dict = None): + if dict_arguments is None: + dict_arguments = {} + msg_dict = {command: str(dict_arguments).replace(",", '').replace("'", '')[1:-1]} + msg = json.dumps(msg_dict) + return msg @staticmethod - def parse_command(command_input): - args = command_input.split() - command = args.pop(0) - return command, args + def parse_message(msg): + try: + j_message = json.loads(msg) + except json.decoder.JSONDecodeError: + print("Json string not in correct format") + return None + + str_command = list(j_message.keys())[0] + + arguments = list(j_message.values())[0].replace(":", '').split() + dict_arguments = collections.OrderedDict(zip(arguments[::2], arguments[1::2])) + return str_command, dict_arguments @requires_connect def send(self, *messages): @@ -192,7 +205,7 @@ class Client: @requires_connect def get_response(self, requested_value): self._request_queue[requested_value] = "" - self.send(Client.form_command("request", (requested_value,))) + self.send(Client.form_message("request", {"value": requested_value})) while not self._request_queue[requested_value]: pass @@ -227,14 +240,14 @@ class Client: @requires_connect def send_file(self, filepath, dest_filename): print("Sending file ", dest_filename) - self.send(Client.form_command("writefile", (dest_filename,))) + self.send(Client.form_message("writefile", {"filename": dest_filename})) file = open(filepath, 'rb') chunk = file.read(BUFFER_SIZE) while chunk: self._send_queue.append(chunk) # TODO os.sendfile chunk = file.read(BUFFER_SIZE) file.close() - self.send(Client.form_command("/endoffile")) + self.send(Client.form_message("/endoffile")) print("File sent") @@ -276,7 +289,7 @@ class MainWindow(QtWidgets.QMainWindow): else: timenow = time.time() print('Now:', time.ctime(timenow), "+ dt =", dt) - Client.send_to_selected(Client.form_command("starttime", (str(timenow + dt),))) + Client.send_to_selected(Client.form_message("starttime", {"time": str(timenow + dt)})) @pyqtSlot() def stop_all(self):