diff --git a/.gitignore b/.gitignore index 4db4998..38f9a4f 100644 --- a/.gitignore +++ b/.gitignore @@ -104,5 +104,6 @@ venv.bak/ .mypy_cache/ .vscode/settings.json Server/tests.py +Server/convert_ui.sh Drone/test_animation/ Drone/animation.csv diff --git a/Drone/client.py b/Drone/client.py index 61fae24..d2c6606 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -125,6 +125,7 @@ def animation_player(running_event, stop_event): for frame in frames: running_event.wait() if stop_event.is_set(): + running_animation_event.clear() break play_animation.animate_frame(frame) @@ -141,7 +142,6 @@ running_animation_event = threading.Event() def start_animation(*args, **kwargs): animation_thread = threading.Thread(target=animation_player, args=(running_animation_event, stop_animation_event)) - play_animation.read_animation_file(animation_file) print("Starting animation!") running_animation_event.set() stop_animation_event.clear() @@ -160,7 +160,6 @@ def pause_animation(): def stop_animation(): stop_animation_event.set() - running_animation_event.clear() print("Stopping animation") # animation_thread.join() @@ -220,6 +219,10 @@ try: rospy.Timer(rospy.Duration(dt), start_animation, oneshot=True) elif command == 'takeoff': play_animation.takeoff() + elif command == 'pause': + pause_animation() + elif command == 'resume': + resume_animation() elif command == 'stop': stop_animation() #FlightLib.reach(5, 5, 2) diff --git a/Server/server_gui.py b/Server/server_gui.py new file mode 100644 index 0000000..143a3d1 --- /dev/null +++ b/Server/server_gui.py @@ -0,0 +1,122 @@ +# -*- coding: utf-8 -*- + +# Form implementation generated from reading ui file 'server_gui.ui' +# +# Created by: PyQt5 UI code generator 5.10.1 +# +# WARNING! All changes made in this file will be lost! + +from PyQt5 import QtCore, QtGui, QtWidgets + +class Ui_MainWindow(object): + def setupUi(self, MainWindow): + MainWindow.setObjectName("MainWindow") + MainWindow.resize(1257, 770) + self.centralwidget = QtWidgets.QWidget(MainWindow) + self.centralwidget.setEnabled(True) + self.centralwidget.setObjectName("centralwidget") + self.gridLayout = QtWidgets.QGridLayout(self.centralwidget) + self.gridLayout.setObjectName("gridLayout") + self.horizontalLayout = QtWidgets.QHBoxLayout() + self.horizontalLayout.setSizeConstraint(QtWidgets.QLayout.SetMaximumSize) + self.horizontalLayout.setObjectName("horizontalLayout") + self.tableView = QtWidgets.QTableView(self.centralwidget) + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.tableView.sizePolicy().hasHeightForWidth()) + self.tableView.setSizePolicy(sizePolicy) + self.tableView.setObjectName("tableView") + self.horizontalLayout.addWidget(self.tableView) + self.verticalLayout = QtWidgets.QVBoxLayout() + self.verticalLayout.setSizeConstraint(QtWidgets.QLayout.SetMaximumSize) + self.verticalLayout.setObjectName("verticalLayout") + self.formLayout = QtWidgets.QFormLayout() + self.formLayout.setObjectName("formLayout") + self.check_button = QtWidgets.QPushButton(self.centralwidget) + self.check_button.setEnabled(True) + self.check_button.setObjectName("check_button") + self.formLayout.setWidget(1, QtWidgets.QFormLayout.SpanningRole, self.check_button) + self.start_text = QtWidgets.QLabel(self.centralwidget) + self.start_text.setObjectName("start_text") + self.formLayout.setWidget(2, QtWidgets.QFormLayout.LabelRole, self.start_text) + self.start_delay_spin = QtWidgets.QSpinBox(self.centralwidget) + self.start_delay_spin.setObjectName("start_delay_spin") + self.formLayout.setWidget(2, QtWidgets.QFormLayout.FieldRole, self.start_delay_spin) + self.start_button = QtWidgets.QPushButton(self.centralwidget) + self.start_button.setEnabled(True) + self.start_button.setFlat(False) + self.start_button.setObjectName("start_button") + self.formLayout.setWidget(3, QtWidgets.QFormLayout.SpanningRole, self.start_button) + self.pause_button = QtWidgets.QPushButton(self.centralwidget) + self.pause_button.setObjectName("pause_button") + self.formLayout.setWidget(4, QtWidgets.QFormLayout.SpanningRole, self.pause_button) + self.stop_button = QtWidgets.QPushButton(self.centralwidget) + self.stop_button.setObjectName("stop_button") + self.formLayout.setWidget(5, QtWidgets.QFormLayout.SpanningRole, self.stop_button) + self.verticalLayout.addLayout(self.formLayout) + self.line = QtWidgets.QFrame(self.centralwidget) + self.line.setFrameShape(QtWidgets.QFrame.HLine) + self.line.setFrameShadow(QtWidgets.QFrame.Sunken) + self.line.setObjectName("line") + self.verticalLayout.addWidget(self.line) + self.formLayout_2 = QtWidgets.QFormLayout() + self.formLayout_2.setObjectName("formLayout_2") + self.takeoff_button = QtWidgets.QPushButton(self.centralwidget) + self.takeoff_button.setEnabled(True) + self.takeoff_button.setObjectName("takeoff_button") + self.formLayout_2.setWidget(0, QtWidgets.QFormLayout.SpanningRole, self.takeoff_button) + self.land_button = QtWidgets.QPushButton(self.centralwidget) + self.land_button.setObjectName("land_button") + self.formLayout_2.setWidget(1, QtWidgets.QFormLayout.SpanningRole, self.land_button) + self.disarm_button = QtWidgets.QPushButton(self.centralwidget) + self.disarm_button.setObjectName("disarm_button") + self.formLayout_2.setWidget(2, QtWidgets.QFormLayout.SpanningRole, self.disarm_button) + self.verticalLayout.addLayout(self.formLayout_2) + self.horizontalLayout.addLayout(self.verticalLayout) + self.horizontalLayout.setStretch(0, 1) + self.gridLayout.addLayout(self.horizontalLayout, 0, 0, 1, 1) + MainWindow.setCentralWidget(self.centralwidget) + self.menubar = QtWidgets.QMenuBar(MainWindow) + self.menubar.setGeometry(QtCore.QRect(0, 0, 1257, 39)) + self.menubar.setObjectName("menubar") + self.menuOptions = QtWidgets.QMenu(self.menubar) + self.menuOptions.setObjectName("menuOptions") + MainWindow.setMenuBar(self.menubar) + self.action_send_animations = QtWidgets.QAction(MainWindow) + self.action_send_animations.setObjectName("action_send_animations") + self.action_send_configurations = QtWidgets.QAction(MainWindow) + self.action_send_configurations.setObjectName("action_send_configurations") + self.menuOptions.addAction(self.action_send_animations) + self.menuOptions.addAction(self.action_send_configurations) + self.menubar.addAction(self.menuOptions.menuAction()) + + self.retranslateUi(MainWindow) + QtCore.QMetaObject.connectSlotsByName(MainWindow) + + def retranslateUi(self, MainWindow): + _translate = QtCore.QCoreApplication.translate + MainWindow.setWindowTitle(_translate("MainWindow", "Clever Drone Animation Player")) + self.check_button.setText(_translate("MainWindow", "Preflight check")) + self.start_text.setText(_translate("MainWindow", "Start after")) + self.start_delay_spin.setSuffix(_translate("MainWindow", " seconds")) + self.start_button.setText(_translate("MainWindow", "Start animation")) + self.pause_button.setText(_translate("MainWindow", "Pause")) + self.stop_button.setText(_translate("MainWindow", "Stop")) + self.takeoff_button.setText(_translate("MainWindow", "Takeoff")) + self.land_button.setText(_translate("MainWindow", "Land")) + self.disarm_button.setText(_translate("MainWindow", "Disarm")) + self.menuOptions.setTitle(_translate("MainWindow", "Actions")) + self.action_send_animations.setText(_translate("MainWindow", "Send Animations")) + self.action_send_configurations.setText(_translate("MainWindow", "Send Configurations")) + + +if __name__ == "__main__": + import sys + app = QtWidgets.QApplication(sys.argv) + MainWindow = QtWidgets.QMainWindow() + ui = Ui_MainWindow() + ui.setupUi(MainWindow) + MainWindow.show() + sys.exit(app.exec_()) + diff --git a/Server/server_gui.ui b/Server/server_gui.ui new file mode 100644 index 0000000..6060942 --- /dev/null +++ b/Server/server_gui.ui @@ -0,0 +1,168 @@ + + + MainWindow + + + + 0 + 0 + 1257 + 770 + + + + Clever Drone Animation Player + + + + true + + + + + + QLayout::SetMaximumSize + + + + + + 0 + 0 + + + + + + + + QLayout::SetMaximumSize + + + + + + + true + + + Preflight check + + + + + + + Start after + + + + + + + seconds + + + + + + + true + + + Start animation + + + false + + + + + + + Pause + + + + + + + Stop + + + + + + + + + Qt::Horizontal + + + + + + + + + true + + + Takeoff + + + + + + + Land + + + + + + + Disarm + + + + + + + + + + + + + + + 0 + 0 + 1257 + 39 + + + + + Actions + + + + + + + + + Send Animations + + + + + Send Configurations + + + + + + diff --git a/Server/server_qt.py b/Server/server_qt.py new file mode 100644 index 0000000..1c9786a --- /dev/null +++ b/Server/server_qt.py @@ -0,0 +1,362 @@ +from PyQt5 import QtWidgets +from PyQt5.QtGui import QStandardItem +from PyQt5.QtGui import QStandardItemModel +from PyQt5.QtCore import QModelIndex +from PyQt5.QtCore import Qt +from PyQt5.QtCore import pyqtSlot + +from PyQt5.QtWidgets import QFileDialog + +# Importing gui form +from server_gui import Ui_MainWindow + + +import os +import sys +import glob +import time +import struct +import socket +import threading +import collections +import configparser + +# All imports sorted in pyramid + +# Functions +def get_ip_address(): + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + s.connect(("8.8.8.8", 80)) + ip = s.getsockname()[0] + s.close() + return ip + + +def auto_connect(): + while True: + ServerSocket.listen(1) + c, addr = ServerSocket.accept() + print("Got connection from:", str(addr)) + if not any(client_addr == addr[0] for client_addr in Client.clients.keys()): + client = Client(addr[0]) + print("New client") + else: + print("Reconnected client") + Client.clients[addr[0]].connect(c, addr) + + +def ip_broadcast(ip, port): + 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 + print("Broadcast sent") + time.sleep(5) + + +NTP_DELTA = 2208988800 # 1970-01-01 00:00:00 +NTP_QUERY = b'\x1b' + bytes(47) + + +def get_ntp_time(ntp_host, ntp_port): + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: + s.sendto(NTP_QUERY, (ntp_host, ntp_port)) + msg, _ = s.recvfrom(1024) + return int.from_bytes(msg[-8:], 'big') / 2 ** 32 - NTP_DELTA + + +def requires_connect(f): + def wrapper(*args, **kwargs): + if args[0].connected: + return f(*args, **kwargs) + else: + print("Function requires client to be connected!") + return wrapper + + +class Client: + clients = {} + + def __init__(self, ip): + self.socket = None + self.addr = None + + self._send_queue = collections.deque() + self._received_queue = collections.deque() + self._request_queue = collections.OrderedDict() + + self.copter_id = None + self.malfunction = False + + Client.clients[ip] = self + + self.connected = False + + def connect(self, client_socket, client_addr): + print("Client connected") + # self._send_queue = collections.deque() # comment for resuming queue after reconnection + + self.socket = client_socket + self.addr = client_addr + + self.socket.setblocking(0) + self.connected = True + client_thread = threading.Thread(target=self._run, args=()) + client_thread.start() + if self.copter_id is None: + self.copter_id = self.get_response("id") + print("Got copter id:", self.copter_id) + model.appendRow((QStandardItem(self.copter_id), )) # TODO: get responses for another columns + + def _send_all(self, msg): + self.socket.sendall(struct.pack('>I', len(msg)) + msg) + + def _receive_all(self, n): + data = b'' + while len(data) < n: + packet = self.socket.recv(min(n - len(data), BUFFER_SIZE)) + if not packet: + return None + data += packet + return data + + def _receive_message(self): + raw_msglen = self._receive_all(4) + if not raw_msglen: + return None + msglen = struct.unpack('>I', raw_msglen)[0] + msg = self._receive_all(msglen) + return msg + + def _run(self): + while self.connected: + try: + if self._send_queue: + msg = self._send_queue.popleft() + print("Send", msg, "to", self.addr) + 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) + + try: # check if data in buffer + check = self.socket.recv(BUFFER_SIZE, socket.MSG_PEEK) + if check: + received = self._receive_message() + if received: + received = received.decode("UTF-8") + print("Recived", received, "from", self.addr) + command, args = Client.parse_command(received) + if command == "response": + for key, value in self._request_queue.items(): + if not value: + self._request_queue[key] = args[0] + print("Request successfully closed") + break + else: + self._received_queue.appendleft(received) + except socket.error: + pass + + except socket.error as e: + print("Client error: {}, disconnected".format(e)) + self.connected = False + self.socket.close() + break + # time.sleep(0.05) + + @staticmethod + def form_command(command: str, args=()): # Change for different protocol + return " ".join([command, *args]) + + @staticmethod + def parse_command(command_input): + args = command_input.split() + command = args.pop(0) + return command, args + + @requires_connect + def send(self, *messages): + for message in messages: + self._send_queue.append(bytes(message, "UTF-8")) + + @requires_connect + def get_response(self, requested_value): + self._request_queue[requested_value] = "" + self.send(Client.form_command("request", (requested_value,))) + + while not self._request_queue[requested_value]: + pass + + return self._request_queue.pop(requested_value) + + @staticmethod + def send_to_selected(*messages): + if Client.clients: + for client in Client.clients.values(): # TODO change to selected + client.send(*messages) + else: + print("No clients were connected!") + + @staticmethod + def request_to_selected(requested_value): + if Client.clients: + for client in Client.clients.values(): # TODO change to selected + client.get_response(requested_value) + else: + print("No clients were connected!") + + @staticmethod + def broadcast(message, force_all=False): + 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): + print("Sending file ", dest_filename) + self.send(Client.form_command("writefile", (dest_filename,))) + file = open(filepath, 'rb') + chunk = file.read(BUFFER_SIZE) + while chunk: + self._send_queue.append(chunk) + chunk = file.read(BUFFER_SIZE) + file.close() + self.send(Client.form_command("/endoffile")) + print("File sent") + + +class MainWindow(QtWidgets.QMainWindow): + def __init__(self): + super(MainWindow, self).__init__() + self.ui = Ui_MainWindow() + self.ui.setupUi(self) + self.initUI() + self.show() + + def initUI(self): + #Connecting + self.ui.check_button.clicked.connect(self.check_selected) + self.ui.start_button.clicked.connect(self.send_starttime) + self.ui.pause_button.clicked.connect(self.pause_all) + self.ui.stop_button.clicked.connect(self.stop_all) + self.ui.takeoff_button.clicked.connect(self.takeoff_selected) + self.ui.land_button.clicked.connect(self.land_all) + self.ui.disarm_button.clicked.connect(self.disarm_all) + + self.ui.action_send_animations.triggered.connect(self.send_animations) + + #Initing table and table model + self.ui.tableView.setModel(model) + self.ui.tableView.horizontalHeader().setStretchLastSection(True) + + @pyqtSlot() + def check_selected(self): + #Client.request_to_selected("selfcheck") + self.ui.start_button.setEnabled(True) + self.ui.takeoff_button.setEnabled(True) + + @pyqtSlot() + def send_starttime(self): + dt = self.ui.start_delay_spin.value() + timenow = time.time() # TODO add NTP + print('Now:', time.ctime(timenow), "+ dt =", dt) + Client.send_to_selected(Client.form_command("starttime", (str(timenow + dt),))) + + @pyqtSlot() + def stop_all(self): + Client.broadcast("stop") + + @pyqtSlot() + def pause_all(self): + if self.ui.pause_button.text() == 'Pause': + Client.broadcast('pause') + self.ui.pause_button.setText('Resume') + else: + Client.broadcast('resume') + self.ui.pause_button.setText('Pause') + + @pyqtSlot() + def takeoff_selected(self): + Client.send_to_selected("takeoff") + + @pyqtSlot() + def land_all(self): + Client.broadcast("land") + + @pyqtSlot() + def disarm_all(self): + Client.broadcast("disarm") + + + @pyqtSlot() + def send_animations(self): + path = str(QFileDialog.getExistingDirectory(self, "Select Animation Directory")) + if path: + print("Selected directory:", path) + files = [file for file in glob.glob(path + '/*.csv')] + names = [os.path.basename(file).split(".")[0] for file in files] + print(files) + for file, name in zip(files, names): + for copter in Client.clients.values(): + if name == copter.copter_id: + copter.send_file(file, "animation.csv") # TODO config + else: + print("Filename not matches with any drone connected") + # dr = next(iter(Client.clients.values())) # костыль для тестирования + # ANS = dr.get_response("someshit") + # print(ANS) + +model = QStandardItemModel() +model.setHorizontalHeaderLabels( + ('copter ID', 'animation ID', 'battery V', 'battery %', 'selfcheck', 'time UTC') +) +model.setColumnCount(6) +model.setRowCount(0) + +# Pre-initialization +# reading config +config = configparser.ConfigParser() +config.read("server_config.ini") + +port = int(config['SERVER']['port']) +BUFFER_SIZE = int(config['SERVER']['buffer_size']) +NTP_HOST = config['NTP']['host'] +NTP_PORT = int(config['NTP']['port']) + + +ServerSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +ServerSocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) +host = socket.gethostname() +ip = get_ip_address() + + +if __name__ == "__main__": + app = QtWidgets.QApplication(sys.argv) + window = MainWindow() + + print('Server started on', host, ip, ":", port) + # print('Now:', time.ctime(get_ntp_time(NTP_HOST, NTP_PORT))) + print('Waiting for clients...') + ServerSocket.bind((ip, port)) + + autoconnect_thread = threading.Thread(target=auto_connect) + autoconnect_thread.daemon = True + autoconnect_thread.start() + + broadcast_thread = threading.Thread(target=ip_broadcast, args=(ip, port,)) + broadcast_thread.daemon = True + broadcast_thread.start() + + sys.exit(app.exec_())