From 0fa7d1a4b71ea36a32019e2f30da6addc259fcbd Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Sun, 16 Jun 2019 16:57:42 +0300
Subject: [PATCH 01/27] New! Realization of table model for storing all copter
 data. Auto refresh, sorting, data checks and coloring based on checks,
 thread-safety

---
 .gitignore                    |  10 ++
 Server/copter_table_models.py | 247 ++++++++++++++++++++++++++++++++++
 Server/server_qt.py           | 147 +++++++++++---------
 3 files changed, 338 insertions(+), 66 deletions(-)
 create mode 100644 Server/copter_table_models.py

diff --git a/.gitignore b/.gitignore
index 2c2068e3..88cd199a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -111,3 +111,13 @@ Drone/client_logs
 images/
 
 \.idea/
+
+Drone/_copter_client_old_\.py
+
+Drone/test_cl\.py
+
+Server/testj\.ipynb
+
+Server/tst_client\.py
+
+Server/tst\.py
diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py
new file mode 100644
index 00000000..b36332e1
--- /dev/null
+++ b/Server/copter_table_models.py
@@ -0,0 +1,247 @@
+import sys
+import re
+from operator import itemgetter
+
+from PyQt5 import QtCore, QtGui, QtWidgets
+from PyQt5.QtCore import Qt as Qt
+
+
+class CopterData:
+    class_attrs = {'copter_id': None, 'anim_id': None, 'batt_v': None, 'batt_p': None, 'selfcheck': None,
+                   'time_utc': None, "time_delta": None, "client": None, "checked": 0}
+
+    def __init__(self, **kwargs):
+        self.attrs = self.class_attrs.copy()
+        self.attrs.update(kwargs)
+
+        for attr, value in self.attrs.items():
+            setattr(self, attr, value)
+
+    def __getitem__(self, key):
+        return getattr(self, list(self.attrs.keys())[key])
+
+    def __setitem__(self, key, value):
+        setattr(self, list(self.attrs.keys())[key], value)
+
+
+class CopterDataModel(QtCore.QAbstractTableModel):
+    checks = {}
+    #selected_available = QtCore.pyqtSignal(bool)
+
+    def __init__(self, parent=None):
+        super(CopterDataModel, self).__init__(parent)
+        self.headers = ('copter ID', 'animation ID', 'battery (V.)', 'battery (%)', 'selfcheck', 'time UTC', "time delta")
+        self.data_contents = []
+
+    def insertRows(self, contents, position='last', parent=QtCore.QModelIndex()):
+        rows = len(contents)
+        position = len(self.data_contents) if position == 'last' else position
+
+        self.beginInsertRows(parent, position, position + rows - 1)
+        self.data_contents[position:position] = contents
+
+        self.endInsertRows()
+
+    def user_selected(self):
+        return filter(lambda x: x.checked == Qt.Checked, self.data_contents)
+
+    def self_checked(self):
+        return filter(lambda x: all_checks(x), self.data_contents)
+
+    def rowCount(self, n=None):
+        return len(self.data_contents)
+
+    def columnCount(self, n=None):
+        return len(self.headers)
+
+    def headerData(self, section, orientation, role=Qt.DisplayRole):
+        if role == Qt.DisplayRole:
+            if orientation == Qt.Horizontal:
+                return self.headers[section]
+
+    def data(self, index, role=Qt.DisplayRole):
+        row = index.row()
+        col = index.column()
+        #print('row {}, col {}, role {}'.format(row, col, role))
+        if role == Qt.DisplayRole:
+            #print(self.data_contents[row][col])
+            return self.data_contents[row][col] or ""
+
+        elif role == Qt.BackgroundRole:
+            if col in self.checks.keys():
+                item = self.data_contents[row][col]
+                result = self.checks[col](item)
+                if result is None:
+                    return QtGui.QBrush(Qt.yellow)
+                if result:
+                    return QtGui.QBrush(Qt.green)
+                else:
+                    return QtGui.QBrush(Qt.red)
+
+        elif role == Qt.CheckStateRole and col == 0:
+            return self.data_contents[row].checked
+
+    def update_model(self, index=QtCore.QModelIndex()):
+        #self.modelReset.emit()
+        self.dataChanged.emit(index, index, (QtCore.Qt.EditRole,))
+
+    @QtCore.pyqtSlot()
+    def setData(self, index, value, role=Qt.EditRole):
+        if not index.isValid():
+            return False
+
+        if role == Qt.CheckStateRole:
+            self.data_contents[index.row()].checked = value
+        elif role == Qt.EditRole:
+            self.data_contents[index.row()][index.column()] = value
+            self.update_model(index)
+        else:
+            return False
+
+        return True
+
+    def flags(self, index):
+        roles = Qt.ItemIsSelectable | Qt.ItemIsEnabled
+        if index.column() == 0:
+            roles |= Qt.ItemIsUserCheckable
+        return roles
+
+    @QtCore.pyqtSlot(int, int, QtCore.QVariant)
+    def update_item(self, row, col, value):
+        self.setData(self.index(row, col), value)
+
+    @QtCore.pyqtSlot(object)
+    def add_client(self, client):
+        self.insertRows([client])
+
+
+def col_check(col):
+    def inner(f):
+        CopterDataModel.checks[col] = f
+
+        def wrapper(*args, **kwargs):
+            return f(*args, **kwargs)
+
+        return wrapper
+
+    return inner
+
+
+@col_check(1)
+def check_anim(item):
+    if not item:
+        return None
+    else:
+        return True
+
+
+@col_check(2)
+def check_bat_v(item):
+    if not item:
+        return None
+    if float(item) > 3.2:  # todo config
+        return True
+    else:
+        return False
+
+
+@col_check(3)
+def check_bat_v(item):
+    if not item:
+        return None
+    if float(item) > 15:  # todo config
+        return True
+    else:
+        return False
+
+
+@col_check(4)
+def check_selfcheck(item):
+    if not item:
+        return None
+    if item == "OK":
+        return True
+    else:
+        return False
+
+
+def all_checks(copter_item):
+    for col, check in CopterDataModel.checks.items():
+        if not check(copter_item[col]):
+            return False
+    return True
+
+
+class CopterProxyModel(QtCore.QSortFilterProxyModel):
+    def __init__(self, parent=None):
+        super(CopterProxyModel, self).__init__(parent)
+
+    @staticmethod
+    def human_sort_prepare(item):
+        if item:
+            item = [int(x) if x.isdigit() else x.lower() for x in re.split('([0-9]+)', str(item))]
+        else:
+            item = []
+        return item
+
+    def lessThan(self, left, right):
+        leftData = self.sourceModel().data(left)
+        rightData = self.sourceModel().data(right)
+
+        return self.human_sort_prepare(leftData) < self.human_sort_prepare(rightData)
+
+
+'''
+    def sort(self, col, order):
+        self.layoutAboutToBeChanged.emit()
+
+        self.data_contents = sorted(self.data_contents, key=lambda item: self.sorter(item[col]),
+                                    reverse=(order == Qt.DescendingOrder))
+        
+        self.layoutChanged.emit()
+'''
+
+
+class SignalManager(QtCore.QObject):
+    update_data_signal = QtCore.pyqtSignal(int, int, QtCore.QVariant)
+    add_client_signal = QtCore.pyqtSignal(object)
+
+
+if __name__ == '__main__':
+    import threading
+    import time
+
+    def timer():
+        idc = 1001
+        while True:
+            myModel.setData(myModel.index(0, 0), idc)
+            idc += 1
+            time.sleep(1)
+
+    app = QtWidgets.QApplication.instance()
+    if app is None:
+        app = QtWidgets.QApplication(sys.argv)
+    tableView = QtWidgets.QTableView()
+    myModel = CopterDataModel(None)
+    proxyModel = CopterProxyModel()
+
+    proxyModel.setDynamicSortFilter(True)
+    proxyModel.setSourceModel(myModel)
+
+    tableView.setModel(proxyModel)
+
+    tableView.verticalHeader().hide()
+    tableView.setSortingEnabled(True)
+
+    tableView.show()
+    myModel.add_client(CopterData(copter_id=1000, checked=0, time_utc=1))
+    myModel.add_client(CopterData(checked=2, selfcheck="OK", time_utc=2))
+    myModel.add_client(CopterData(checked=2, selfcheck="not ok", time_utc="no"))
+
+    myModel.setData(myModel.index(0, 1), "test")
+
+    t = threading.Thread(target=timer, daemon=True)
+    t.start()
+    print(QtCore.QT_VERSION_STR)
+
+    app.exec_()
diff --git a/Server/server_qt.py b/Server/server_qt.py
index b0c98882..6dc7804f 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -3,7 +3,7 @@
 
 from PyQt5 import QtWidgets
 from PyQt5.QtGui import QStandardItemModel, QStandardItem
-from PyQt5.QtCore import Qt, pyqtSlot
+from PyQt5.QtCore import Qt, pyqtSlot, QAbstractTableModel
 
 from PyQt5.QtWidgets import QFileDialog, QMessageBox
 
@@ -11,21 +11,50 @@
 from server_gui import Ui_MainWindow
 
 from server import *
+from copter_table_models import *
 from emergency import *
 
 
+class MyTableModel(QAbstractTableModel):
+    def __init__(self, parent, headers, *args):
+        QAbstractTableModel.__init__(self, parent, *args)
+
+
 # noinspection PyArgumentList,PyCallByClass
 class MainWindow(QtWidgets.QMainWindow):
     def __init__(self):
         super(MainWindow, self).__init__()
+
         self.ui = Ui_MainWindow()
         self.ui.setupUi(self)
         self.init_ui()
+
+        self.model = CopterDataModel()
+        self.proxy_model = CopterProxyModel()
+        self.signals = SignalManager()
+
+        self.init_model()
+        
         self.show()
+        
+    def init_model(self):
+        self.proxy_model.setDynamicSortFilter(True)
+        self.proxy_model.setSourceModel(self.model)
+
+        # Initing table and table self.model
+        self.ui.tableView.setModel(self.proxy_model)
+        self.ui.tableView.horizontalHeader().setStretchLastSection(True)
+        self.ui.tableView.setSortingEnabled(True)
+
+        self.signals.update_data_signal.connect(self.model.update_item)
+        self.signals.add_client_signal.connect(self.model.add_client)
+
+    def client_connected(self, client: Client):
+        self.signals.add_client_signal.emit(CopterData(copter_id=client.copter_id, client=client))
 
     def init_ui(self):
         # Connecting
-        self.ui.check_button.clicked.connect(self.check_selected)
+        self.ui.check_button.clicked.connect(self.selfcheck_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)
@@ -40,41 +69,42 @@ def init_ui(self):
         self.ui.action_send_configurations.triggered.connect(self.send_configurations)
         self.ui.action_send_Aruco_map.triggered.connect(self.send_aruco)
 
-        # Initing table and table model
-        self.ui.tableView.setModel(model)
-        self.ui.tableView.horizontalHeader().setStretchLastSection(True)
-
     @pyqtSlot()
-    def check_selected(self):
-        for row_num in range(model.rowCount()):
-            item = model.item(row_num, 0)
-            if item.isCheckable() and item.checkState() == Qt.Checked:
-                print("Copter {} checked".format(model.item(row_num, 0).text()))
-                copter = Client.get_by_id(item.text())
-                copter.get_response("anim_id", self._set_copter_data, callback_args=(row_num, 1))
-                copter.get_response("batt_voltage", self._set_copter_data, callback_args=(row_num, 2))
-                copter.get_response("cell_voltage", self._set_copter_data, callback_args=(row_num, 3))
-                copter.get_response("selfcheck", self._set_copter_data, callback_args=(row_num, 4))
-                copter.get_response("time", self._set_copter_data, callback_args=(row_num, 5))
-
-        self.ui.start_button.setEnabled(True)
-        self.ui.takeoff_button.setEnabled(True)
-
-    def _set_copter_data(self, value, row, col):
+    def selfcheck_selected(self):
+        for copter_data in self.model.user_selected():
+            copter = copter_data.client
+
+            copter.get_response("anim_id", self._set_copter_data, callback_args=(1, copter_data.copter_id))
+            copter.get_response("batt_voltage", self._set_copter_data, callback_args=(2, copter_data.copter_id))
+            copter.get_response("cell_voltage", self._set_copter_data, callback_args=(3, copter_data.copter_id))
+            copter.get_response("selfcheck", self._set_copter_data, callback_args=(4, copter_data.copter_id))
+            copter.get_response("time", self._set_copter_data, callback_args=(5, copter_data.copter_id))
+
+        #self.ui.start_button.setEnabled(True)
+        #self.ui.takeoff_button.setEnabled(True)
+
+    def _set_copter_data(self, value, col, copter_id):
+        row = self.model.data_contents.index(next(
+            filter(lambda x: x.copter_id == copter_id, self.model.data_contents)))
+
         if col == 1:
-            model.setData(model.index(row, col), value)
+            data = value
         elif col == 2:
-            model.setData(model.index(row, col), "{} V".format(round(float(value), 3)))
+            data = "{} V.".format(round(float(value), 3))
         elif col == 3:
-            batt_percent = ((float(value) - 3.2) / (4.2 - 3.2)) * 100
-            model.setData(model.index(row, col), "{} %".format(round(batt_percent, 3)))
+            batt_percent = ((float(value) - 3.2) / (4.2 - 3.2)) * 100  # TODO config
+            data = "{} %".format(round(batt_percent, 3))
         elif col == 4:
-            if value != "OK":
-                model.setData(model.index(row, col), str(value))  # TODO different handling
-            else:
-                model.setData(model.index(row, col), str(value))
+            data = str(value)
         elif col == 5:
-            model.setData(model.index(row, col), time.ctime(int(value)))
+            data = time.ctime(int(value))
+            data2 = "{} sec.".format(round(int(value) - time.time(), 3))
+            self.signals.update_data_signal.emit(row, col + 1, data2)
+        else:
+            print("No column matched for response")
+            return
+
+        self.signals.update_data_signal.emit(row, col, data)
 
     @pyqtSlot()
     def send_starttime(self):
@@ -86,8 +116,8 @@ def send_starttime(self):
         )
         if reply == QMessageBox.Yes:
             print("Accepted")
-            for row_num in range(model.rowCount()):
-                item = model.item(row_num, 0)
+            for row_num in range(self.model.rowCount()):
+                item = self.model.item(row_num, 0)
                 if item.isCheckable() and item.checkState() == Qt.Checked:
                     if True:  # TODO checks for batt/selfckeck here
                         copter = Client.get_by_id(item.text())
@@ -110,10 +140,10 @@ def pause_all(self):
 
     @pyqtSlot()
     def test_leds(self):
-        for row_num in range(model.rowCount()):
-            item = model.item(row_num, 0)
+        for row_num in range(self.model.rowCount()):
+            item = self.model.item(row_num, 0)
             if item.isCheckable() and item.checkState() == Qt.Checked:
-                if True:  # TODO checks for batt/selfckeck here
+                if True:
                     copter = Client.get_by_id(item.text())
                     copter.send_message("led_test")
 
@@ -126,8 +156,8 @@ def takeoff_selected(self):
         )
         if reply == QMessageBox.Yes:
             print("Accepted")
-            for row_num in range(model.rowCount()):
-                item = model.item(row_num, 0)
+            for row_num in range(self.model.rowCount()):
+                item = self.model.item(row_num, 0)
                 if item.isCheckable() and item.checkState() == Qt.Checked:
                     if True:  # TODO checks for batt/selfckeck here
                         copter = Client.get_by_id(item.text())
@@ -154,8 +184,8 @@ def send_animations(self):
             names = [os.path.basename(file).split(".")[0] for file in files]
             print(files)
             for file, name in zip(files, names):
-                for row_num in range(model.rowCount()):
-                    item = model.item(row_num, 0)
+                for row_num in range(self.model.rowCount()):
+                    item = self.model.item(row_num, 0)
                     if item.isCheckable() and item.checkState() == Qt.Checked:
                         copter = Client.get_by_id(item.text())
                         if name == copter.copter_id:
@@ -176,8 +206,9 @@ def send_configurations(self):
                     value = sendable_config[section][option]
                     logging.debug("Got item from config:".format(section, option, value))
                     options.append(ConfigOption(section, option, value))
-            for row_num in range(model.rowCount()):
-                item = model.item(row_num, 0)
+                    
+            for row_num in range(self.model.rowCount()):
+                item = self.model.item(row_num, 0)
                 if item.isCheckable() and item.checkState() == Qt.Checked:
                     copter = Client.get_by_id(item.text())
                     copter.send_config_options(*options)
@@ -188,16 +219,16 @@ def send_aruco(self):
         if path:
             filename = os.path.basename(path)
             print("Selected file:", path, filename)
-            for row_num in range(model.rowCount()):
-                item = model.item(row_num, 0)
+            for row_num in range(self.model.rowCount()):
+                item = self.model.item(row_num, 0)
                 if item.isCheckable() and item.checkState() == Qt.Checked:
                     copter = Client.get_by_id(item.text())
                     copter.send_file(path, "/home/pi/catkin_ws/src/clever/aruco_pose/map/animation_map.txt")
                     copter.send_message("service_restart", {"name": "clever"})
     @pyqtSlot()
     def emergency(self):
-        for row_num in range(model.rowCount()):
-                item = model.item(row_num, 0)
+        for row_num in range(self.model.rowCount()):
+                item = self.model.item(row_num, 0)
                 if item.isCheckable() and item.checkState() == Qt.Checked:
                     copter = Client.get_by_id(item.text())
                     copter.send_message("emergency")
@@ -216,8 +247,8 @@ def flip(self):
         )
         if reply == QMessageBox.Yes:
             print("Accepted")
-            for row_num in range(model.rowCount()):
-                item = model.item(row_num, 0)
+            for row_num in range(self.model.rowCount()):
+                item = self.model.item(row_num, 0)
                 if item.isCheckable() and item.checkState() == Qt.Checked:
                     if True:  # TODO checks for batt/selfckeck here
                         copter = Client.get_by_id(item.text())
@@ -226,29 +257,13 @@ def flip(self):
             print("Cancelled")
             pass
 
-        
-
-model = QStandardItemModel()
-model.setHorizontalHeaderLabels(
-    ('copter ID', 'animation ID', 'battery V', 'battery %', 'selfcheck', 'time UTC')
-)
-model.setColumnCount(6)
-model.setRowCount(0)
-
-
-def client_connected(self: Client):
-    copter_id_item = QStandardItem(self.copter_id)
-    copter_id_item.setCheckable(True)
-    model.appendRow((copter_id_item, ))
-
-
-Client.on_first_connect = client_connected
-
 
 if __name__ == "__main__":
     app = QtWidgets.QApplication(sys.argv)
     window = MainWindow()
 
+    Client.on_first_connect = window.client_connected
+
     server = Server(on_stop=app.quit)
     server.start()
 

From a5021d4b6e133c04a16732cdaa9e7baf089c221a Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Sun, 16 Jun 2019 17:08:01 +0300
Subject: [PATCH 02/27] Modify flip function to work better in aruco_map frame
 with both Clever 3 & 4

---
 Drone/FlightLib/FlightLib.py | 8 ++++----
 Drone/copter_client.py       | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py
index 334caeb7..b08e59b1 100644
--- a/Drone/FlightLib/FlightLib.py
+++ b/Drone/FlightLib/FlightLib.py
@@ -359,10 +359,10 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY,
     #print("Takeoff succeeded!")
     return 'success'
 
-def flip(min_z = FLIP_MIN_Z): #TODO Flip in different directions
+def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different directions
     logger.info("Flip started!")
 
-    start_telemetry = get_telemetry()  # memorize starting position
+    start_telemetry = get_telemetry(frame_id=frame_id)  # memorize starting position
     
     if start_telemetry.z < min_z - TOLERANCE:
         logger.warning("Can't do flip! Flip failed!")
@@ -377,9 +377,9 @@ def flip(min_z = FLIP_MIN_Z): #TODO Flip in different directions
         while True:
             telem = get_telemetry()
 
-            if -math.pi + 0.1 < telem.roll < -0.2:
+            if abs(telem.roll) > math.pi/2:
                 break
 
         logger.info('Flip succeeded!')
         #print('Flip succeeded!')
-        navto(x=start_telemetry.x, y=start_telemetry.y, z=start_telemetry.z, yaw=start_telemetry.yaw)   # finish flip
+        navto(x=start_telemetry.x, y=start_telemetry.y, z=start_telemetry.z, yaw=start_telemetry.yaw, frame_id=frame_id)   # finish flip
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index facecb45..8f2bbdc9 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -108,7 +108,7 @@ def _command_emergency_led_fill(**kwargs):
 
 @messaging.message_callback("flip")
 def _copter_flip():
-    FlightLib.flip()
+    FlightLib.flip(frame_id=client.active_client.FRAME_ID)
 
 @messaging.message_callback("takeoff")
 def _command_takeoff(**kwargs):

From a9b9694add83b5fd6869f2aeddf3bdfeabd4b8d3 Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Sun, 16 Jun 2019 19:06:51 +0300
Subject: [PATCH 03/27] Disabling and enabling some buttons based on selected
 copters

---
 Server/copter_table_models.py | 21 +++++++--------------
 Server/server_qt.py           | 14 ++++++++++----
 2 files changed, 17 insertions(+), 18 deletions(-)

diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py
index b36332e1..527c8ae8 100644
--- a/Server/copter_table_models.py
+++ b/Server/copter_table_models.py
@@ -26,7 +26,7 @@ def __setitem__(self, key, value):
 
 class CopterDataModel(QtCore.QAbstractTableModel):
     checks = {}
-    #selected_available = QtCore.pyqtSignal(bool)
+    selected_ready_signal = QtCore.pyqtSignal(bool)
 
     def __init__(self, parent=None):
         super(CopterDataModel, self).__init__(parent)
@@ -45,8 +45,9 @@ def insertRows(self, contents, position='last', parent=QtCore.QModelIndex()):
     def user_selected(self):
         return filter(lambda x: x.checked == Qt.Checked, self.data_contents)
 
-    def self_checked(self):
-        return filter(lambda x: all_checks(x), self.data_contents)
+    def selfchecked_ready(self, contents=()):
+        contents = contents or self.data_contents
+        return filter(lambda x: all_checks(x), contents)
 
     def rowCount(self, n=None):
         return len(self.data_contents)
@@ -92,6 +93,9 @@ def setData(self, index, value, role=Qt.EditRole):
 
         if role == Qt.CheckStateRole:
             self.data_contents[index.row()].checked = value
+            # check if all selected are selfcheck and ok (ready)
+            self.selected_ready_signal.emit(set(self.user_selected()).issubset(self.selfchecked_ready()))
+
         elif role == Qt.EditRole:
             self.data_contents[index.row()][index.column()] = value
             self.update_model(index)
@@ -191,17 +195,6 @@ def lessThan(self, left, right):
         return self.human_sort_prepare(leftData) < self.human_sort_prepare(rightData)
 
 
-'''
-    def sort(self, col, order):
-        self.layoutAboutToBeChanged.emit()
-
-        self.data_contents = sorted(self.data_contents, key=lambda item: self.sorter(item[col]),
-                                    reverse=(order == Qt.DescendingOrder))
-        
-        self.layoutChanged.emit()
-'''
-
-
 class SignalManager(QtCore.QObject):
     update_data_signal = QtCore.pyqtSignal(int, int, QtCore.QVariant)
     add_client_signal = QtCore.pyqtSignal(object)
diff --git a/Server/server_qt.py b/Server/server_qt.py
index 6dc7804f..f1caf2d8 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -41,14 +41,19 @@ def init_model(self):
         self.proxy_model.setDynamicSortFilter(True)
         self.proxy_model.setSourceModel(self.model)
 
-        # Initing table and table self.model
+        # Initiate table and table self.model
         self.ui.tableView.setModel(self.proxy_model)
         self.ui.tableView.horizontalHeader().setStretchLastSection(True)
         self.ui.tableView.setSortingEnabled(True)
 
+        # Connect signals to manipulate model from threads
         self.signals.update_data_signal.connect(self.model.update_item)
         self.signals.add_client_signal.connect(self.model.add_client)
 
+        # Connect model signals to UI
+        self.model.selected_ready_signal.connect(self.ui.start_button.setEnabled)
+        self.model.selected_ready_signal.connect(self.ui.takeoff_button.setEnabled)
+
     def client_connected(self, client: Client):
         self.signals.add_client_signal.emit(CopterData(copter_id=client.copter_id, client=client))
 
@@ -69,6 +74,10 @@ def init_ui(self):
         self.ui.action_send_configurations.triggered.connect(self.send_configurations)
         self.ui.action_send_Aruco_map.triggered.connect(self.send_aruco)
 
+        # Set most safety-important buttons disabled
+        self.ui.start_button.setEnabled(False)
+        self.ui.takeoff_button.setEnabled(False)
+
     @pyqtSlot()
     def selfcheck_selected(self):
         for copter_data in self.model.user_selected():
@@ -80,9 +89,6 @@ def selfcheck_selected(self):
             copter.get_response("selfcheck", self._set_copter_data, callback_args=(4, copter_data.copter_id))
             copter.get_response("time", self._set_copter_data, callback_args=(5, copter_data.copter_id))
 
-        #self.ui.start_button.setEnabled(True)
-        #self.ui.takeoff_button.setEnabled(True)
-
     def _set_copter_data(self, value, col, copter_id):
         row = self.model.data_contents.index(next(
             filter(lambda x: x.copter_id == copter_id, self.model.data_contents)))

From 2c08053c4c0a98e1c9a887d3786aec329cbd49fe Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Sun, 16 Jun 2019 22:50:01 +0300
Subject: [PATCH 04/27] This commit group closing #28 and closing #27

Resolved all model integration issues, improved qt syntax and code organization
---
 Server/server_qt.py | 194 +++++++++++++++++++-------------------------
 1 file changed, 82 insertions(+), 112 deletions(-)

diff --git a/Server/server_qt.py b/Server/server_qt.py
index 20de5f1a..a869bd9b 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -15,6 +15,27 @@
 from copter_table_models import *
 from emergency import *
 
+
+def confirmation_required(text="Are you sure?", label="Confirm operation?"):
+    def inner(f):
+
+        def wrapper(*args, **kwargs):
+            reply = QMessageBox.question(
+                args[0], label,
+                text,
+                QMessageBox.Yes | QMessageBox.No, QMessageBox.No
+            )
+            if reply == QMessageBox.Yes:
+                print("Dialog accepted")
+                return f(*args, **kwargs)
+            else:
+                print("Dialog declined")
+
+        return wrapper
+
+    return inner
+
+
 # noinspection PyArgumentList,PyCallByClass
 class MainWindow(QtWidgets.QMainWindow):
     def __init__(self):
@@ -75,14 +96,14 @@ def init_ui(self):
 
     @pyqtSlot()
     def selfcheck_selected(self):
-        for copter_data in self.model.user_selected():
-            copter = copter_data.client
+        for copter in self.model.user_selected():
+            client = copter.client
 
-            copter.get_response("anim_id", self._set_copter_data, callback_args=(1, copter_data.copter_id))
-            copter.get_response("batt_voltage", self._set_copter_data, callback_args=(2, copter_data.copter_id))
-            copter.get_response("cell_voltage", self._set_copter_data, callback_args=(3, copter_data.copter_id))
-            copter.get_response("selfcheck", self._set_copter_data, callback_args=(4, copter_data.copter_id))
-            copter.get_response("time", self._set_copter_data, callback_args=(5, copter_data.copter_id))
+            client.get_response("anim_id", self._set_copter_data, callback_args=(1, copter.copter_id))
+            client.get_response("batt_voltage", self._set_copter_data, callback_args=(2, copter.copter_id))
+            client.get_response("cell_voltage", self._set_copter_data, callback_args=(3, copter.copter_id))
+            client.get_response("selfcheck", self._set_copter_data, callback_args=(4, copter.copter_id))
+            client.get_response("time", self._set_copter_data, callback_args=(5, copter.copter_id))
 
     def _set_copter_data(self, value, col, copter_id):
         row = self.model.data_contents.index(next(
@@ -108,23 +129,31 @@ def _set_copter_data(self, value, col, copter_id):
         self.signals.update_data_signal.emit(row, col, data)
 
     @pyqtSlot()
+    @confirmation_required("This operation will takeoff selected copters with delay and start animation. Proceed?")
     def send_starttime(self):
         dt = self.ui.start_delay_spin.value()
-        reply = QMessageBox.question(
-            self, "Confirm operation",
-            "This operation will takeoff selected copters and start animation after {} seconds. Proceed?".format(dt),
-            QMessageBox.Yes | QMessageBox.No, QMessageBox.No
-        )
-        if reply == QMessageBox.Yes:
-            print("Accepted")
-            for row_num in range(self.model.rowCount()):
-                item = self.model.item(row_num, 0)
-                if item.isCheckable() and item.checkState() == Qt.Checked:
-                    if True:  # TODO checks for batt/selfckeck here
-                        copter = Client.get_by_id(item.text())
-                        server.send_starttime(copter, dt)
-        else:
-            print("Cancelled")
+        for copter in self.model.user_selected():
+            if all_checks(copter):
+                server.send_starttime(copter.client, dt)
+
+    @pyqtSlot()
+    @confirmation_required("This operation will takeoff copters immediately. Proceed?")
+    def takeoff_selected(self):
+        for copter in self.model.user_selected():
+            if all_checks(copter):
+                copter.client.send_message("takeoff")
+
+    @pyqtSlot()
+    @confirmation_required("This operation will flip(!!!) copters immediately. Proceed?")
+    def flip(self):
+        for copter in self.model.user_selected():
+            if all_checks(copter):
+                copter.client.send_message("flip")
+
+    @pyqtSlot()
+    def test_leds(self):
+        for copter in self.model.user_selected():
+            copter.client.send_message("led_test")
 
     @pyqtSlot()
     def stop_all(self):
@@ -139,34 +168,6 @@ def pause_all(self):
             Client.broadcast_message('resume')
             self.ui.pause_button.setText('Pause')
 
-    @pyqtSlot()
-    def test_leds(self):
-        for row_num in range(self.model.rowCount()):
-            item = self.model.item(row_num, 0)
-            if item.isCheckable() and item.checkState() == Qt.Checked:
-                if True:
-                    copter = Client.get_by_id(item.text())
-                    copter.send_message("led_test")
-
-    @pyqtSlot()
-    def takeoff_selected(self):
-        reply = QMessageBox.question(
-            self, "Confirm operation",
-            "This operation will takeoff copters immediately. Proceed?",
-            QMessageBox.Yes | QMessageBox.No, QMessageBox.No
-        )
-        if reply == QMessageBox.Yes:
-            print("Accepted")
-            for row_num in range(self.model.rowCount()):
-                item = self.model.item(row_num, 0)
-                if item.isCheckable() and item.checkState() == Qt.Checked:
-                    if True:  # TODO checks for batt/selfckeck here
-                        copter = Client.get_by_id(item.text())
-                        copter.send_message("takeoff")
-        else:
-            print("Cancelled")
-            pass
-
     @pyqtSlot()
     def land_all(self):
         Client.broadcast_message("land")
@@ -185,14 +186,11 @@ def send_animations(self):
             names = [os.path.basename(file).split(".")[0] for file in files]
             print(files)
             for file, name in zip(files, names):
-                for row_num in range(self.model.rowCount()):
-                    item = self.model.item(row_num, 0)
-                    if item.isCheckable() and item.checkState() == Qt.Checked:
-                        copter = Client.get_by_id(item.text())
-                        if name == copter.copter_id:
-                            copter.send_file(file, "animation.csv")  # TODO config
-                        else:
-                            print("Filename not matches with any drone connected")
+                for copter in self.model.user_selected():
+                    if name == copter.copter_id:
+                        copter.client.send_file(file, "animation.csv")  # TODO config
+                else:
+                    print("Filename has no matches with any drone selected")
 
     @pyqtSlot()
     def send_configurations(self):
@@ -207,12 +205,9 @@ def send_configurations(self):
                     value = sendable_config[section][option]
                     logging.debug("Got item from config:".format(section, option, value))
                     options.append(ConfigOption(section, option, value))
-                    
-            for row_num in range(self.model.rowCount()):
-                item = self.model.item(row_num, 0)
-                if item.isCheckable() and item.checkState() == Qt.Checked:
-                    copter = Client.get_by_id(item.text())
-                    copter.send_config_options(*options)
+
+            for copter in self.model.user_selected():
+                copter.client.send_config_options(*options)
 
     @pyqtSlot()
     def send_aruco(self):
@@ -220,81 +215,56 @@ def send_aruco(self):
         if path:
             filename = os.path.basename(path)
             print("Selected file:", path, filename)
-            for row_num in range(self.model.rowCount()):
-                item = self.model.item(row_num, 0)
-                if item.isCheckable() and item.checkState() == Qt.Checked:
-                    copter = Client.get_by_id(item.text())
-                    copter.send_file(path, "/home/pi/catkin_ws/src/clever/aruco_pose/map/animation_map.txt")
-                    copter.send_message("service_restart", {"name": "clever"})
+            for copter in self.model.user_selected():
+                copter.client.send_file(path, "/home/pi/catkin_ws/src/clever/aruco_pose/map/animation_map.txt")
+                copter.client.send_message("service_restart", {"name": "clever"})
+
     @pyqtSlot()
     def emergency(self):
         client_row_min = 0
-        client_row_max = model.rowCount() - 1
+        client_row_max = self.model.rowCount() - 1
         result = -1
-        while (result!=0) and (result != 3) and (result != 4):
+        while (result != 0) and (result != 3) and (result != 4):
             # light_green_red(min, max)
             client_row_mid = int(math.ceil((client_row_max+client_row_min) / 2.0))
             print(client_row_min, client_row_mid, client_row_max)
             for row_num in range(client_row_min, client_row_mid):
-                item = model.item(row_num, 0)
-                copter = Client.get_by_id(item.text())
-                copter.send_message("led_fill", {"green": 255})
+                self.model.data_contents[row_num].client\
+                    .send_message("led_fill", {"green": 255})
             for row_num in range(client_row_mid, client_row_max + 1):
-                item = model.item(row_num, 0)
-                copter = Client.get_by_id(item.text())
-                copter.send_message("led_fill", {"red": 255})
+                self.model.data_contents[row_num].client \
+                    .send_message("led_fill", {"red": 255})
+
             Dialog = QtWidgets.QDialog()    
             ui = Ui_Dialog()
             ui.setupUi(Dialog)
             Dialog.show()
             result = Dialog.exec()
             print("Dialog result: {}".format(result))
-            if (client_row_max != client_row_min):
-                if (result == 1): 
+
+            if client_row_max != client_row_min:
+                if result == 1:
                     for row_num in range(client_row_mid, client_row_max + 1):
-                        item = model.item(row_num, 0)
-                        copter = Client.get_by_id(item.text())
-                        copter.send_message("led_fill")
+                        self.model.data_contents[row_num].client \
+                            .send_message("led_fill")
                     client_row_max = client_row_mid - 1
                    
-                elif (result == 2):
+                elif result == 2:
                     for row_num in range(client_row_min, client_row_mid):
-                        item = model.item(row_num, 0)
-                        copter = Client.get_by_id(item.text())
-                        copter.send_message("led_fill")
+                        self.model.data_contents[row_num].client \
+                            .send_message("led_fill")
                     client_row_min = client_row_mid
 
         if result == 0:
             Client.broadcast_message("led_fill")
         elif result == 3:
             for row_num in range(client_row_min, client_row_max + 1):
-                item = model.item(row_num, 0)
-                copter = Client.get_by_id(item.text())
-                copter.send_message("land")
+                self.model.data_contents[row_num].client \
+                    .send_message("land")
         elif result == 4:
             for row_num in range(client_row_min, client_row_max + 1):
-                item = model.item(row_num, 0)
-                copter = Client.get_by_id(item.text())
-                copter.send_message("disarm")
- 
-    @pyqtSlot()    
-    def flip(self):
-        reply = QMessageBox.question(
-            self, "Confirm operation",
-            "You are ready to turn the copter?",
-            QMessageBox.Yes | QMessageBox.No, QMessageBox.No
-        )
-        if reply == QMessageBox.Yes:
-            print("Accepted")
-            for row_num in range(self.model.rowCount()):
-                item = self.model.item(row_num, 0)
-                if item.isCheckable() and item.checkState() == Qt.Checked:
-                    if True:  # TODO checks for batt/selfckeck here
-                        copter = Client.get_by_id(item.text())
-                        copter.send_message("flip")
-        else:
-            print("Cancelled")
-            pass
+                self.model.data_contents[row_num].client \
+                    .send_message("disarm")
 
 
 if __name__ == "__main__":

From 32cca2ecc473c66af06608f6e6dc6f6eb85060e1 Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Sun, 16 Jun 2019 23:19:48 +0300
Subject: [PATCH 05/27] Added check for ROS services available, closes #21

---
 Drone/FlightLib/FlightLib.py | 24 ++++++++++++++++++++----
 1 file changed, 20 insertions(+), 4 deletions(-)

diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py
index 334caeb7..63dff9a5 100644
--- a/Drone/FlightLib/FlightLib.py
+++ b/Drone/FlightLib/FlightLib.py
@@ -22,6 +22,8 @@
 arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
 landing = rospy.ServiceProxy('/land', Trigger)
 
+services_list = [navigate, set_position, set_rates, set_mode, get_telemetry, arming,landing]
+
 logger.info("Proxy services inited")
 
 # globals
@@ -41,9 +43,11 @@
 
 checklist = []
 
-def arming_wrapper(state=False, interrupter=INTERRUPTER):
+
+def arming_wrapper(state=False, *args, **kwargs):
     arming(state)
 
+
 def interrupt():
     logger.info("Performing function interrupt")
     INTERRUPTER.set()
@@ -63,10 +67,8 @@ def check(check_name):
     def inner(f):
         def wrapper(*args, **kwargs):
             failures = f(*args, **kwargs)
-            print(failures)
             msgs = []
             for failure in failures:
-                #print(failure)
                 msg = "[{}]: Failure: {}".format(check_name, failure)
                 msgs.append(msg)
                 logger.warning(msg)
@@ -87,6 +89,16 @@ def _check_nans(*values):
     return any(math.isnan(x) for x in values)
 
 
+@check("Ros services")
+def check_ros_services():
+    timeout = 5.0
+    for service in services_list:
+        try:
+            service.wait_for_service(timeout=timeout)
+        except (rospy.ServiceException, rospy.ROSException) as e:
+            yield ("ROS service {} is not available!".format(service.name))
+
+
 @check("FCU connection")
 def check_connection():
     telemetry = get_telemetry()
@@ -359,7 +371,8 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY,
     #print("Takeoff succeeded!")
     return 'success'
 
-def flip(min_z = FLIP_MIN_Z): #TODO Flip in different directions
+
+def flip(min_z=FLIP_MIN_Z):  # TODO Flip in different directions
     logger.info("Flip started!")
 
     start_telemetry = get_telemetry()  # memorize starting position
@@ -367,6 +380,7 @@ def flip(min_z = FLIP_MIN_Z): #TODO Flip in different directions
     if start_telemetry.z < min_z - TOLERANCE:
         logger.warning("Can't do flip! Flip failed!")
         #print("Can't do flip! Flip failed!")
+        return False
     else:                    
         # Flip!    
         set_rates(thrust=1)  # bump up
@@ -383,3 +397,5 @@ def flip(min_z = FLIP_MIN_Z): #TODO Flip in different directions
         logger.info('Flip succeeded!')
         #print('Flip succeeded!')
         navto(x=start_telemetry.x, y=start_telemetry.y, z=start_telemetry.z, yaw=start_telemetry.yaw)   # finish flip
+
+        return True

From 1a3b5830736d1828076b17743e29594f0c235b42 Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Mon, 17 Jun 2019 12:32:48 +0300
Subject: [PATCH 06/27] Added client broadcast binding timeouts and attempts,
 Closes #30

---
 Drone/client.py | 47 ++++++++++++++++++++++++++++-------------------
 1 file changed, 28 insertions(+), 19 deletions(-)

diff --git a/Drone/client.py b/Drone/client.py
index 3ec3b062..8b5d6fe9 100644
--- a/Drone/client.py
+++ b/Drone/client.py
@@ -15,6 +15,7 @@
 parent_dir = os.path.dirname(current_dir)
 sys.path.insert(0, parent_dir) 
 
+#logging.basicConfig(level=logging.INFO)
 logger = logging.getLogger(__name__)
 
 import messaging_lib as messaging
@@ -107,7 +108,7 @@ def start(self):
             logger.critical("Caught interrupt, exiting!")
             self.selector.close()
 
-    def _reconnect(self, timeout=2, attempt_limit=5):
+    def _reconnect(self, timeout=3.0, attempt_limit=5):
         logger.info("Trying to connect to {}:{} ...".format(self.server_host, self.server_port))
         attempt_count = 0
         while not self.connected:
@@ -133,7 +134,7 @@ def _reconnect(self, timeout=2, attempt_limit=5):
 
             if attempt_count >= attempt_limit:
                 logger.info("Too many attempts. Trying to get new server IP")
-                self.broadcast_bind()
+                self.broadcast_bind(timeout*2, attempt_limit)
                 attempt_count = 0
 
 
@@ -146,27 +147,35 @@ def _connect(self):
         self._process_connections()
 
 
-    def broadcast_bind(self):
+    def broadcast_bind(self, timeout=3.0, attempt_limit=5):
         broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
         broadcast_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
         broadcast_client.bind(("", self.broadcast_port))
+        broadcast_client.settimeout(timeout)
+
+        attempt_count = 0
         try:
-            while True:
-                data, addr = broadcast_client.recvfrom(self.BUFFER_SIZE)
-                message = messaging.MessageManager()
-                message.income_raw = data
-                message.process_message()
-                if message.content:
-                    logger.info("Received broadcast message {} from {}".format(message.content, addr))
-                    if message.content["command"] == "server_ip":
-                        args = message.content["args"]
-                        self.server_port = int(args["port"])
-                        self.server_host = args["host"]
-                        self.write_config(False,
-                                          ConfigOption("SERVER", "port", self.server_port),
-                                          ConfigOption("SERVER", "host", self.server_host))
-                        logger.info("Binding to new IP: {}:{}".format(self.server_host, self.server_port))
-                        break
+            while attempt_count <= attempt_limit:
+                try:
+                    data, addr = broadcast_client.recvfrom(self.BUFFER_SIZE)
+                except socket.error as error:
+                    logger.warning("Could not receive broadcast due error: {}".format(error))
+                    attempt_count += 1
+                else:
+                    message = messaging.MessageManager()
+                    message.income_raw = data
+                    message.process_message()
+                    if message.content:
+                        logger.info("Received broadcast message {} from {}".format(message.content, addr))
+                        if message.content["command"] == "server_ip":
+                            args = message.content["args"]
+                            self.server_port = int(args["port"])
+                            self.server_host = args["host"]
+                            self.write_config(False,
+                                              ConfigOption("SERVER", "port", self.server_port),
+                                              ConfigOption("SERVER", "host", self.server_host))
+                            logger.info("Binding to new IP: {}:{}".format(self.server_host, self.server_port))
+                            break
         finally:
             broadcast_client.close()
 

From 46d85bc3e1657c1d75eeb5fc79a89d4713bafede Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Tue, 18 Jun 2019 12:29:33 +0300
Subject: [PATCH 07/27] Replaced default mutable argument

---
 Drone/tasking_lib.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/Drone/tasking_lib.py b/Drone/tasking_lib.py
index 8cf40bdc..2e8b4c30 100644
--- a/Drone/tasking_lib.py
+++ b/Drone/tasking_lib.py
@@ -42,7 +42,10 @@ def __init__(self):
         self._timeshift = 0.0
 
     def add_task(self, timestamp, priority, task_function,
-                 task_args=(), task_kwargs={}, task_delayable=False):
+                 task_args=(), task_kwargs=None, task_delayable=False):
+
+        if task_kwargs is None:
+            task_kwargs = {}
 
         self._wait_interrupt_event.set()
         self._running_event.clear()

From 33e5a5b71f91437e410a862afafde92252387f25 Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Tue, 18 Jun 2019 13:11:27 +0300
Subject: [PATCH 08/27] Code reformatting to match PEP, resume command now
 accepts time

---
 Drone/animation_lib.py |  34 ++++++------
 Drone/copter_client.py | 115 +++++++++++++++++++++--------------------
 Drone/tasking_lib.py   |  18 ++++---
 3 files changed, 89 insertions(+), 78 deletions(-)

diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py
index 0bc52652..77122762 100644
--- a/Drone/animation_lib.py
+++ b/Drone/animation_lib.py
@@ -13,16 +13,20 @@
 
 interrupt_event = threading.Event()
 
-id = "Empty id"
+anim_id = "Empty id"
+
+# TODO refactor as class
+# TODO separate code for frames transformations (e.g. for gps)
+
 
 def get_id(filepath="animation.csv"):
-    global id
+    global anim_id
     try:
         animation_file = open(filepath)
     except IOError:
         logging.error("File {} can't be opened".format(filepath))
-        id = "No animation"
-        return id
+        anim_id = "No animation"
+        return anim_id
     else:
         with animation_file:
             csv_reader = csv.reader(
@@ -30,20 +34,21 @@ def get_id(filepath="animation.csv"):
             )
             row_0 = csv_reader.next()
             if len(row_0) == 1:
-                id = row_0[0]
-                print("Got animation_id: {}".format(id))
+                anim_id = row_0[0]
+                print("Got animation_id: {}".format(anim_id))
             else:
                 print("No animation id in file")
-    return id
+    return anim_id
+
 
 def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0):
     imported_frames = []
-    global id
+    global anim_id
     try:
         animation_file = open(filepath)
     except IOError:
         logging.error("File {} can't be opened".format(filepath))
-        id = "No animation"
+        anim_id = "No animation"
     else:
         with animation_file:
             csv_reader = csv.reader(
@@ -51,8 +56,8 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0):
             )
             row_0 = csv_reader.next()
             if len(row_0) == 1:
-                id = row_0[0]
-                print("Got animation_id: {}".format(id))
+                anim_id = row_0[0]
+                print("Got animation_id: {}".format(anim_id))
             else:
                 print("No animation id in file")
                 frame_number, x, y, z, yaw, red, green, blue = row_0
@@ -87,7 +92,6 @@ def convert_frame(frame):
 
 def execute_frame(point=(), color=(), yaw=float('Nan'), frame_id='aruco_map', use_leds=True,
                   flight_func=FlightLib.navto, flight_kwargs=None, interrupter=interrupt_event):
-
     if flight_kwargs is None:
         flight_kwargs = {}
 
@@ -112,7 +116,7 @@ def execute_animation(frames, frame_delay, frame_id='aruco_map', use_leds=True,
         tasking.wait(next_frame_time, interrupter)
 
 
-def takeoff(z=1.5, safe_takeoff=True, frame_id = 'map', timeout=5.0, use_leds=True,
+def takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, use_leds=True,
             interrupter=interrupt_event):
     print(interrupter.is_set())
     if use_leds:
@@ -120,9 +124,9 @@ def takeoff(z=1.5, safe_takeoff=True, frame_id = 'map', timeout=5.0, use_leds=Tr
     if interrupter.is_set():
         return
     result = FlightLib.takeoff(z=z, wait=False, timeout_takeoff=timeout, frame_id=frame_id, emergency_land=safe_takeoff,
-                      interrupter=interrupter)
+                               interrupter=interrupter)
     if result == 'not armed':
-        raise Exception('STOP')         # Raise exception to clear task_manager if copter can't arm
+        raise Exception('STOP')  # Raise exception to clear task_manager if copter can't arm
     if interrupter.is_set():
         return
     if use_leds:
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index facecb45..ee677791 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -12,7 +12,7 @@
 import tasking_lib as tasking
 import animation_lib as animation
 
-#logging.basicConfig(  # TODO all prints as logs
+# logging.basicConfig(  # TODO all prints as logs
 #    level=logging.DEBUG, # INFO
 #    format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s",
 #    handlers=[
@@ -21,7 +21,8 @@
 
 logger = logging.getLogger(__name__)
 
-#import ros_logging
+
+# import ros_logging
 
 class CopterClient(client.Client):
     def load_config(self):
@@ -45,7 +46,7 @@ def start(self, task_manager_instance):
         rospy.init_node('Swarm_client', anonymous=True)
         if self.USE_LEDS:
             LedLib.init_led(self.LED_PIN)
-        
+
         task_manager_instance.start()
 
         super(CopterClient, self).start()
@@ -56,10 +57,12 @@ def _response_selfcheck():
     check = FlightLib.selfcheck()
     return check if check else "OK"
 
+
 @messaging.request_callback("anim_id")
 def _response_animation_id():
     return animation.get_id()
 
+
 @messaging.request_callback("batt_voltage")
 def _response_batt():
     return FlightLib.get_telemetry('body').voltage
@@ -69,47 +72,50 @@ def _response_batt():
 def _response_cell():
     return FlightLib.get_telemetry('body').cell_voltage
 
+
 @messaging.message_callback("test")
 def _command_test(**kwargs):
     print("test")
 
+
 @messaging.message_callback("service_restart")
 def _command_service_restart(**kwargs):
     os.system("systemctl restart" + kwargs["name"])
 
 
 @messaging.message_callback("led_test")
-def _command_led_test(*args, **kwargs):
+def _command_led_test(**kwargs):
     LedLib.chase(255, 255, 255)
     time.sleep(2)
     LedLib.off()
 
+
 @messaging.message_callback("led_fill")
-def _command_emergency_led_fill(**kwargs):
+def _command_led_fill(**kwargs):
     r = g = b = 0
-    
+
     try:
         r = kwargs["red"]
     except KeyError:
         pass
-    
+
     try:
         g = kwargs["green"]
     except KeyError:
         pass
-    try:    
+    try:
         b = kwargs["blue"]
-    except KeyError: 
+    except KeyError:
         pass
-    
-    LedLib.fill(r, g, b)
 
+    LedLib.fill(r, g, b)
 
 
 @messaging.message_callback("flip")
 def _copter_flip():
     FlightLib.flip()
 
+
 @messaging.message_callback("takeoff")
 def _command_takeoff(**kwargs):
     task_manager.add_task(time.time(), 0, animation.takeoff,
@@ -139,10 +145,10 @@ def _command_land(**kwargs):
 def _command_disarm(**kwargs):
     task_manager.reset()
     task_manager.add_task(-5, 0, FlightLib.arming_wrapper,
-                        task_kwargs={
-                            "state": False
-                        }
-    )
+                          task_kwargs={
+                              "state": False
+                          }
+                          )
 
 
 @messaging.message_callback("stop")
@@ -157,18 +163,18 @@ def _command_stop(**kwargs):
 
 @messaging.message_callback("resume")
 def _command_stop(**kwargs):
-    task_manager.resume()
+    task_manager.resume(time_to_start_next_task=kwargs.get("time", 0))
 
 
 @messaging.message_callback("start")
 def _play_animation(**kwargs):
     start_time = float(kwargs["time"])  # TODO
 
-    if (animation.get_id() == 'No animation'):
+    if animation.get_id() == 'No animation':
         print("Can't start animation without animation file!")
         return
 
-    print("Start time = {}, wait for {} seconds".format(start_time, time.time()-start_time))
+    print("Start time = {}, wait for {} seconds".format(start_time, time.time() - start_time))
 
     frames = animation.load_animation(os.path.abspath("animation.csv"),
                                       x0=client.active_client.X0 + client.active_client.X0_COMMON,
@@ -176,59 +182,58 @@ def _play_animation(**kwargs):
                                       )
 
     task_manager.add_task(start_time, 0, animation.takeoff,
-                        task_kwargs={
-                            "z": client.active_client.TAKEOFF_HEIGHT,
-                            "timeout": client.active_client.TAKEOFF_TIME,
-                            "safe_takeoff": client.active_client.SAFE_TAKEOFF,
-                            #"frame_id": client.active_client.FRAME_ID,
-                            "use_leds": client.active_client.USE_LEDS,
-                        }
-                        )
+                          task_kwargs={
+                              "z": client.active_client.TAKEOFF_HEIGHT,
+                              "timeout": client.active_client.TAKEOFF_TIME,
+                              "safe_takeoff": client.active_client.SAFE_TAKEOFF,
+                              # "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                          }
+                          )
 
     rfp_time = start_time + client.active_client.TAKEOFF_TIME
     task_manager.add_task(rfp_time, 0, animation.execute_frame,
-                        task_kwargs={
-                            "point": animation.convert_frame(frames[0])[0],
-                            "color": animation.convert_frame(frames[0])[1],
-                            "frame_id": client.active_client.FRAME_ID,
-                            "use_leds": client.active_client.USE_LEDS,
-                            "flight_func": FlightLib.reach_point,
-                        }
-                        )
+                          task_kwargs={
+                              "point": animation.convert_frame(frames[0])[0],
+                              "color": animation.convert_frame(frames[0])[1],
+                              "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                              "flight_func": FlightLib.reach_point,
+                          }
+                          )
 
     frame_time = rfp_time + client.active_client.RFP_TIME
     frame_delay = 0.125  # TODO from animation file
     for frame in frames:
+        point, color = animation.convert_frame(frame)  # TODO add param to calculate delta
         task_manager.add_task(frame_time, 0, animation.execute_frame,
-                        task_kwargs={
-                            "point": animation.convert_frame(frame)[0],
-                            "color": animation.convert_frame(frame)[1],
-                            "frame_id": client.active_client.FRAME_ID,
-                            "use_leds": client.active_client.USE_LEDS,
-                            "flight_func": FlightLib.navto,
-                            }
-                        )
+                              task_kwargs={
+                                  "point": point,
+                                  "color": color,
+                                  "frame_id": client.active_client.FRAME_ID,
+                                  "use_leds": client.active_client.USE_LEDS,
+                                  "flight_func": FlightLib.navto,
+                              }
+                              )
         frame_time += frame_delay
 
     land_time = frame_time + client.active_client.LAND_TIME
     task_manager.add_task(land_time, 0, animation.land,
-                        task_kwargs={
-                            "timeout": client.active_client.TAKEOFF_TIME,
-                            "frame_id": client.active_client.FRAME_ID,
-                            "use_leds": client.active_client.USE_LEDS,
-                        },
-                        )
+                          task_kwargs={
+                              "timeout": client.active_client.TAKEOFF_TIME,
+                              "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                          },
+                          )
 
 
 if __name__ == "__main__":
-    
     copter_client = CopterClient()
     task_manager = tasking.TaskManager()
-    
-    copter_client.start(task_manager)
 
-    #ros_logging.route_logger_to_ros()
-    #ros_logging.route_logger_to_ros("__main__")
-    #ros_logging.route_logger_to_ros("client")
-    #ros_logging.route_logger_to_ros("messaging")
+    copter_client.start(task_manager)
 
+    # ros_logging.route_logger_to_ros()
+    # ros_logging.route_logger_to_ros("__main__")
+    # ros_logging.route_logger_to_ros("client")
+    # ros_logging.route_logger_to_ros("messaging")
diff --git a/Drone/tasking_lib.py b/Drone/tasking_lib.py
index 2e8b4c30..bf1f8553 100644
--- a/Drone/tasking_lib.py
+++ b/Drone/tasking_lib.py
@@ -10,7 +10,9 @@
 
 INTERRUPTER = threading.Event()
 
-def wait(end, interrupter=INTERRUPTER, maxsleep=0.1):  # Added features to interrupter sleep and set max sleeping interval
+
+def wait(end, interrupter=INTERRUPTER, maxsleep=0.1):
+    # Added features to interrupter sleep and set max sleeping interval
 
     while not interrupter.is_set():  # Basic implementation of pause module until()
         now = time.time()
@@ -83,7 +85,7 @@ def pop_task(self):
                 return heapq.heappop(self.task_queue)
             raise KeyError('Pop from an empty priority queue')
 
-    def start(self, timeouts=False):
+    def start(self):
         #print("Task manager is started")
         #logger.info("Task manager is started")
         self._processor_thread.start()
@@ -95,13 +97,13 @@ def stop(self):
         with self._task_queue_lock:
             del self.task_queue[:]
 
-    def shutdown(self):
+    def shutdown(self, timeout=5.0):
         self.stop()
         self._shutdown_event.set()
-        self._wait_interrupt_event.set()
-        self._task_interrupt_event.set()
-        self._running_event.clear()
-        self._processor_thread.join(timeout=5)
+        # self._wait_interrupt_event.set()
+        # self._task_interrupt_event.set()
+        # self._running_event.clear() #already exists in pause
+        self._processor_thread.join(timeout=timeout)
 
     def pause(self, interrupt=True):
         if interrupt:
@@ -111,7 +113,7 @@ def pause(self, interrupt=True):
         #logger.info("Task queue paused")
         #print("Task queue paused")
 
-    def resume(self, time_to_start_next_task = 0):
+    def resume(self, time_to_start_next_task=0.0):
         if self.task_queue:
             next_task_time = self.task_queue[0][0]
             if time_to_start_next_task > next_task_time:

From 0bbabb6363bb97493922c8e026928e429400eeb9 Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Tue, 18 Jun 2019 13:31:29 +0300
Subject: [PATCH 09/27] Server now can send resume time stamp for sync, closes
 #33

---
 Drone/copter_client.py |  4 ++--
 Server/server_qt.py    | 13 +++++++++----
 2 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index ee677791..17f5d636 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -157,12 +157,12 @@ def _command_stop(**kwargs):
 
 
 @messaging.message_callback("pause")
-def _command_stop(**kwargs):
+def _command_pause(**kwargs):
     task_manager.pause()
 
 
 @messaging.message_callback("resume")
-def _command_stop(**kwargs):
+def _command_resume(**kwargs):
     task_manager.resume(time_to_start_next_task=kwargs.get("time", 0))
 
 
diff --git a/Server/server_qt.py b/Server/server_qt.py
index a869bd9b..02060d50 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -77,7 +77,7 @@ def init_ui(self):
         # Connecting
         self.ui.check_button.clicked.connect(self.selfcheck_selected)
         self.ui.start_button.clicked.connect(self.send_starttime)
-        self.ui.pause_button.clicked.connect(self.pause_all)
+        self.ui.pause_button.clicked.connect(self.pause_resume_all)
         self.ui.stop_button.clicked.connect(self.stop_all)
         self.ui.emergency_button.clicked.connect(self.emergency)
 
@@ -160,13 +160,18 @@ def stop_all(self):
         Client.broadcast_message("stop")
 
     @pyqtSlot()
-    def pause_all(self):
+    def pause_resume_all(self):
         if self.ui.pause_button.text() == 'Pause':
             Client.broadcast_message('pause')
             self.ui.pause_button.setText('Resume')
         else:
-            Client.broadcast_message('resume')
-            self.ui.pause_button.setText('Pause')
+            self._resume_all()
+
+    @confirmation_required("This operation will resume ALL copter tasks with given delay. Proceed?")
+    def _resume_all(self):
+        dt = self.ui.start_delay_spin.value()
+        Client.broadcast_message('resume', {"time": 0 if dt == 0 else server.time_now()})
+        self.ui.pause_button.setText('Pause')
 
     @pyqtSlot()
     def land_all(self):

From b7018288fd4fbb7fc72f00bfdb7217ff2b6dc463 Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Tue, 18 Jun 2019 13:35:00 +0300
Subject: [PATCH 10/27] led_fill code improvement

---
 Drone/copter_client.py | 18 +++---------------
 1 file changed, 3 insertions(+), 15 deletions(-)

diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index 17f5d636..9874b382 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -92,21 +92,9 @@ def _command_led_test(**kwargs):
 
 @messaging.message_callback("led_fill")
 def _command_led_fill(**kwargs):
-    r = g = b = 0
-
-    try:
-        r = kwargs["red"]
-    except KeyError:
-        pass
-
-    try:
-        g = kwargs["green"]
-    except KeyError:
-        pass
-    try:
-        b = kwargs["blue"]
-    except KeyError:
-        pass
+    r = kwargs.get("red", 0)
+    g = kwargs.get("green", 0)
+    b = kwargs.get("blue", 0)
 
     LedLib.fill(r, g, b)
 

From a7067115c4c48a911975babab8351b693b763d4d Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Tue, 18 Jun 2019 13:37:55 +0300
Subject: [PATCH 11/27] Should fix #31, reopen if not

---
 Drone/copter_client.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index 9874b382..e2a8538e 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -80,7 +80,7 @@ def _command_test(**kwargs):
 
 @messaging.message_callback("service_restart")
 def _command_service_restart(**kwargs):
-    os.system("systemctl restart" + kwargs["name"])
+    os.system("systemctl restart {}".format(kwargs["name"]))
 
 
 @messaging.message_callback("led_test")

From 657d4c72bb367e4fd0f4313cdf56a93d9401bc85 Mon Sep 17 00:00:00 2001
From: artem30801 <38689676+artem30801@users.noreply.github.com>
Date: Sat, 22 Jun 2019 11:49:38 +0300
Subject: [PATCH 12/27] Update issue templates

---
 .github/ISSUE_TEMPLATE/bug_report.md      | 38 +++++++++++++++++++++++
 .github/ISSUE_TEMPLATE/feature_request.md | 20 ++++++++++++
 2 files changed, 58 insertions(+)
 create mode 100644 .github/ISSUE_TEMPLATE/bug_report.md
 create mode 100644 .github/ISSUE_TEMPLATE/feature_request.md

diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md
new file mode 100644
index 00000000..f3d5c415
--- /dev/null
+++ b/.github/ISSUE_TEMPLATE/bug_report.md
@@ -0,0 +1,38 @@
+---
+name: Bug report
+about: Create a report to help us improve
+title: ''
+labels: bug
+assignees: ''
+
+---
+
+**Describe the bug**
+A clear and concise description of what the bug is.
+
+**To Reproduce**
+Steps to reproduce the behavior:
+1. Go to '...'
+2. Click on '....'
+3. Scroll down to '....'
+4. See error
+
+**Expected behavior**
+A clear and concise description of what you expected to happen.
+
+**Screenshots**
+If applicable, add screenshots to help explain your problem.
+
+**Desktop (please complete the following information):**
+ - OS: [e.g. iOS]
+ - Browser [e.g. chrome, safari]
+ - Version [e.g. 22]
+
+**Smartphone (please complete the following information):**
+ - Device: [e.g. iPhone6]
+ - OS: [e.g. iOS8.1]
+ - Browser [e.g. stock browser, safari]
+ - Version [e.g. 22]
+
+**Additional context**
+Add any other context about the problem here.
diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md
new file mode 100644
index 00000000..11fc491e
--- /dev/null
+++ b/.github/ISSUE_TEMPLATE/feature_request.md
@@ -0,0 +1,20 @@
+---
+name: Feature request
+about: Suggest an idea for this project
+title: ''
+labels: enhancement
+assignees: ''
+
+---
+
+**Is your feature request related to a problem? Please describe.**
+A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
+
+**Describe the solution you'd like**
+A clear and concise description of what you want to happen.
+
+**Describe alternatives you've considered**
+A clear and concise description of any alternative solutions or features you've considered.
+
+**Additional context**
+Add any other context or screenshots about the feature request here.

From d7e8e29f803462e8f8f8f244e2773c10da94f439 Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Tue, 25 Jun 2019 18:35:39 +0300
Subject: [PATCH 13/27] Display fix

---
 Server/server.py    | 2 +-
 Server/server_qt.py | 6 +++---
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/Server/server.py b/Server/server.py
index 897d2b37..1ae4ca62 100644
--- a/Server/server.py
+++ b/Server/server.py
@@ -159,7 +159,7 @@ def _connect_client(self, sock):
         logging.info("Got connection from: {}".format(str(addr)))
         conn.setblocking(False)
 
-        if not any(client_addr == addr[0] for client_addr in Client.clients.keys()):
+        if not any([client_addr == addr[0] for client_addr in Client.clients.keys()]):
             client = Client(addr[0])
             logging.info("New client")
         else:
diff --git a/Server/server_qt.py b/Server/server_qt.py
index 02060d50..9d8c5fb3 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -112,15 +112,15 @@ def _set_copter_data(self, value, col, copter_id):
         if col == 1:
             data = value
         elif col == 2:
-            data = "{} V.".format(round(float(value), 3))
+            data = "{}".format(round(float(value), 3))
         elif col == 3:
             batt_percent = ((float(value) - 3.2) / (4.2 - 3.2)) * 100  # TODO config
-            data = "{} %".format(round(batt_percent, 3))
+            data = "{}".format(round(batt_percent, 3))
         elif col == 4:
             data = str(value)
         elif col == 5:
             data = time.ctime(int(value))
-            data2 = "{} sec.".format(round(int(value) - time.time(), 3))
+            data2 = "{}".format(round(int(value) - time.time(), 3))
             self.signals.update_data_signal.emit(row, col + 1, data2)
         else:
             print("No column matched for response")

From cd9baa6a095fa7ec25e767691b3d5f87867c5c98 Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Tue, 25 Jun 2019 18:46:04 +0300
Subject: [PATCH 14/27] Fixed sel unregister issue

---
 Server/server.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Server/server.py b/Server/server.py
index 1ae4ca62..51f6e27f 100644
--- a/Server/server.py
+++ b/Server/server.py
@@ -140,7 +140,7 @@ def _client_processor(self):
         self.sel.register(self.server_socket, selectors.EVENT_READ | selectors.EVENT_WRITE, data=None)
 
         while self.client_processor_thread_running.is_set():
-            events = self.sel.select(timeout=0)
+            events = self.sel.select()
             for key, mask in events:
                 if key.data is None:
                     self._connect_client(key.fileobj)

From ed02db6dbf4aa6ab5e453353ecdac848916fac10 Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Tue, 25 Jun 2019 19:23:55 +0300
Subject: [PATCH 15/27] Fix for python <3.6

---
 Server/copter_table_models.py | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py
index 527c8ae8..976e0b31 100644
--- a/Server/copter_table_models.py
+++ b/Server/copter_table_models.py
@@ -1,14 +1,15 @@
 import sys
 import re
-from operator import itemgetter
+import collections
 
 from PyQt5 import QtCore, QtGui, QtWidgets
 from PyQt5.QtCore import Qt as Qt
 
 
 class CopterData:
-    class_attrs = {'copter_id': None, 'anim_id': None, 'batt_v': None, 'batt_p': None, 'selfcheck': None,
-                   'time_utc': None, "time_delta": None, "client": None, "checked": 0}
+    class_attrs = collections.OrderedDict([('copter_id', None), ('anim_id', None), ('batt_v', None), ('batt_p', None),
+                                           ('selfcheck', None), ('time_utc', None), ("time_delta", None),
+                                           ("client", None), ("checked", 0)], )
 
     def __init__(self, **kwargs):
         self.attrs = self.class_attrs.copy()

From 85bfcd181fb686cd9ac37774e508863b9461bf67 Mon Sep 17 00:00:00 2001
From: artem30801 <38689676+artem30801@users.noreply.github.com>
Date: Wed, 26 Jun 2019 00:28:58 +0300
Subject: [PATCH 16/27] Quick fix (probably)

---
 Drone/copter_client.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index ac22c1f8..7c2d6804 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -193,7 +193,7 @@ def _play_animation(**kwargs):
     frame_time = rfp_time + client.active_client.RFP_TIME
     frame_delay = 0.125  # TODO from animation file
     for frame in frames:
-        point, color = animation.convert_frame(frame)  # TODO add param to calculate delta
+        point, color, yaw = animation.convert_frame(frame)  # TODO add param to calculate delta
         task_manager.add_task(frame_time, 0, animation.execute_frame,
                               task_kwargs={
                                   "point": point,

From c04bff12ac17f30ba2cce409641101bbac86ef2a Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Wed, 26 Jun 2019 20:11:01 +0300
Subject: [PATCH 17/27] added on_broadcast() for client-side

---
 Drone/client.py        | 4 ++++
 Drone/copter_client.py | 4 ++++
 2 files changed, 8 insertions(+)

diff --git a/Drone/client.py b/Drone/client.py
index 8b5d6fe9..f824e5ee 100644
--- a/Drone/client.py
+++ b/Drone/client.py
@@ -175,10 +175,14 @@ def broadcast_bind(self, timeout=3.0, attempt_limit=5):
                                               ConfigOption("SERVER", "port", self.server_port),
                                               ConfigOption("SERVER", "host", self.server_host))
                             logger.info("Binding to new IP: {}:{}".format(self.server_host, self.server_port))
+                            self.on_broadcast_bind()
                             break
         finally:
             broadcast_client.close()
 
+    def on_broadcast_bind(self):
+        pass
+
     def _process_connections(self):
         while True:
             events = self.selector.select(timeout=1)
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index 7c2d6804..9c21ac5e 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -41,6 +41,10 @@ def load_config(self):
         self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds')
         self.LED_PIN = self.config.getint('PRIVATE', 'led_pin')
 
+    def on_broadcast_bind(self):
+        #TODO change chony config
+        _command_service_restart(name="chrony")
+
     def start(self, task_manager_instance):
         client.logger.info("Init ROS node")
         rospy.init_node('Swarm_client', anonymous=True)

From fe76cdfe087276ce50fa0413fadad502d4707a0c Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Thu, 27 Jun 2019 11:06:20 +0300
Subject: [PATCH 18/27] Added chrony ip configuration on start; close #20

---
 Drone/copter_client.py | 46 ++++++++++++++++++++++++++++++++++++++----
 1 file changed, 42 insertions(+), 4 deletions(-)

diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index 9c21ac5e..e12855f0 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -42,8 +42,8 @@ def load_config(self):
         self.LED_PIN = self.config.getint('PRIVATE', 'led_pin')
 
     def on_broadcast_bind(self):
-        #TODO change chony config
-        _command_service_restart(name="chrony")
+        configure_chrony_ip(self.server_host)
+        restart_service("chrony")
 
     def start(self, task_manager_instance):
         client.logger.info("Init ROS node")
@@ -56,6 +56,43 @@ def start(self, task_manager_instance):
         super(CopterClient, self).start()
 
 
+def restart_service(name):
+    os.system("systemctl restart {}".format(name))
+
+
+def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):
+    try:
+        with open(path, 'r') as f:
+            raw_content = f.read()
+    except IOError as e:
+        print("Reading error {}".format(e))
+        return False
+
+    content = raw_content.split(" ")
+
+    try:
+        current_ip = content[ip_index]
+    except IndexError:
+        print("Something wrong with config")
+        return False
+
+    if "." not in current_ip:
+        print("That's not ip!")
+        return False
+
+    if current_ip != ip:
+        content[ip_index] = ip
+
+        try:
+            with open(path, 'w') as f:
+                f.write(" ".join(content))
+        except IOError:
+            print("Error writing")
+            return False
+
+    return True
+
+
 @messaging.request_callback("selfcheck")
 def _response_selfcheck():
     check = FlightLib.selfcheck()
@@ -79,12 +116,13 @@ def _response_cell():
 
 @messaging.message_callback("test")
 def _command_test(**kwargs):
-    print("test")
+    logger.info("logging info test")
+    print("stdout test")
 
 
 @messaging.message_callback("service_restart")
 def _command_service_restart(**kwargs):
-    os.system("systemctl restart {}".format(kwargs["name"]))
+    restart_service(kwargs["name"])
 
 
 @messaging.message_callback("led_test")

From d973d7c3ed713bbb9f19cc63eb992b2cf79cd385 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Tue, 2 Jul 2019 16:08:58 +0300
Subject: [PATCH 19/27] Update .gitignore

---
 .gitignore | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.gitignore b/.gitignore
index 88cd199a..f9b589a5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -109,7 +109,7 @@ Drone/test_animation/
 Drone/animation.csv
 Drone/client_logs
 images/
-
+.vscode/
 \.idea/
 
 Drone/_copter_client_old_\.py

From 0a2f259517732a21b413517129fba6641885892b Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Thu, 4 Jul 2019 22:51:32 +0300
Subject: [PATCH 20/27] Activate optical flow positioning by default

---
 builder/image-configure.sh | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/builder/image-configure.sh b/builder/image-configure.sh
index 8f965402..3ad8cf53 100755
--- a/builder/image-configure.sh
+++ b/builder/image-configure.sh
@@ -44,7 +44,7 @@ sed -i '/<arg name="aruco_vpe"/c \    <arg name="aruco_vpe" default="true"/>' /h
 sed -i '/<param name="map"/c \        <param name="map" value="\$\(find aruco_pose\)/map/animation_map.txt"/>' /home/pi/catkin_ws/src/clever/clever/launch/aruco.launch
 sed -i '/<arg name="aruco"/c \    <arg name="aruco" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/clever.launch
 sed -i '/<arg name="rangefinder_vl53l1x"/c \    <arg name="rangefinder_vl53l1x" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/clever.launch
-#sed -i '/<arg name="optical_flow"/c \    <arg name="optical_flow" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/clever.launch
+sed -i '/<arg name="optical_flow"/c \    <arg name="optical_flow" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/clever.launch
 
 echo_stamp "Image was configured!" "SUCCESS"
 

From 686cc2f25628c4d0dc4b577ea3a16a9c7085cb38 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Thu, 4 Jul 2019 22:53:39 +0300
Subject: [PATCH 21/27] service: Remove requirement of clever service

---
 builder/assets/clever-show.service | 1 -
 1 file changed, 1 deletion(-)

diff --git a/builder/assets/clever-show.service b/builder/assets/clever-show.service
index 0c571934..5297b18c 100644
--- a/builder/assets/clever-show.service
+++ b/builder/assets/clever-show.service
@@ -1,6 +1,5 @@
 [Unit]
 Description=Clever Show Client Service
-Requires=clever.service
 After=clever.service
 
 [Service]

From bfe705f10d811d2f33bd94cb53cf79844aed067e Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Fri, 5 Jul 2019 14:23:36 +0300
Subject: [PATCH 22/27] Update table model and signal emitting condition

---
 Server/copter_table_models.py | 21 ++++++++++++++++-----
 Server/server_qt.py           |  8 +++++---
 2 files changed, 21 insertions(+), 8 deletions(-)

diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py
index 976e0b31..5251ef41 100644
--- a/Server/copter_table_models.py
+++ b/Server/copter_table_models.py
@@ -31,7 +31,7 @@ class CopterDataModel(QtCore.QAbstractTableModel):
 
     def __init__(self, parent=None):
         super(CopterDataModel, self).__init__(parent)
-        self.headers = ('copter ID', 'animation ID', 'battery (V.)', 'battery (%)', 'selfcheck', 'time UTC', "time delta")
+        self.headers = ('copter ID', 'animation ID', 'battery V', 'battery %', 'selfcheck', 'time delta')
         self.data_contents = []
 
     def insertRows(self, contents, position='last', parent=QtCore.QModelIndex()):
@@ -62,6 +62,7 @@ def headerData(self, section, orientation, role=Qt.DisplayRole):
                 return self.headers[section]
 
     def data(self, index, role=Qt.DisplayRole):
+        self.selected_ready_signal.emit(set(self.user_selected()).issubset(self.selfchecked_ready()))
         row = index.row()
         col = index.column()
         #print('row {}, col {}, role {}'.format(row, col, role))
@@ -94,8 +95,6 @@ def setData(self, index, value, role=Qt.EditRole):
 
         if role == Qt.CheckStateRole:
             self.data_contents[index.row()].checked = value
-            # check if all selected are selfcheck and ok (ready)
-            self.selected_ready_signal.emit(set(self.user_selected()).issubset(self.selfchecked_ready()))
 
         elif role == Qt.EditRole:
             self.data_contents[index.row()][index.column()] = value
@@ -136,6 +135,8 @@ def wrapper(*args, **kwargs):
 def check_anim(item):
     if not item:
         return None
+    if str(item) == 'No animation':
+        return False
     else:
         return True
 
@@ -151,13 +152,14 @@ def check_bat_v(item):
 
 
 @col_check(3)
-def check_bat_v(item):
+def check_bat_p(item):
     if not item:
         return None
-    if float(item) > 15:  # todo config
+    if float(item) > 30:  # todo config
         return True
     else:
         return False
+        #return True #For testing
 
 
 @col_check(4)
@@ -169,6 +171,15 @@ def check_selfcheck(item):
     else:
         return False
 
+@col_check(5)
+def check_time_delta(item):
+    if not item:
+        return None
+    if abs(float(item)) < 1:
+        return True
+    else:
+        return False
+
 
 def all_checks(copter_item):
     for col, check in CopterDataModel.checks.items():
diff --git a/Server/server_qt.py b/Server/server_qt.py
index 9d8c5fb3..cfe69fac 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -119,9 +119,11 @@ def _set_copter_data(self, value, col, copter_id):
         elif col == 4:
             data = str(value)
         elif col == 5:
-            data = time.ctime(int(value))
-            data2 = "{}".format(round(int(value) - time.time(), 3))
-            self.signals.update_data_signal.emit(row, col + 1, data2)
+            #data = time.ctime(int(value))
+            data = "{}".format(round(float(value) - time.time(), 3))
+            if abs(float(data)) > 1:
+                Client.get_by_id(copter_id).send_message("repair_chrony")
+            #self.signals.update_data_signal.emit(row, col + 1, data2)
         else:
             print("No column matched for response")
             return

From 0c7982aa891eef09d971f05b47b8b4cc460097bd Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Fri, 5 Jul 2019 14:24:33 +0300
Subject: [PATCH 23/27] Add repair_chrony message callback

---
 Drone/copter_client.py | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index e12855f0..fcb37214 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -124,6 +124,11 @@ def _command_test(**kwargs):
 def _command_service_restart(**kwargs):
     restart_service(kwargs["name"])
 
+@messaging.message_callback("repair_chrony")
+def _command_chrony_repair():
+    configure_chrony_ip(client.active_client.server_host)
+    restart_service("chrony")
+
 
 @messaging.message_callback("led_test")
 def _command_led_test(**kwargs):

From d5dc7203f34f20c89d82f20fdf38a1e3da90b5af Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Fri, 5 Jul 2019 14:25:35 +0300
Subject: [PATCH 24/27] service: Set KillSignal to SIGKILL to kill all
 processes in service

---
 builder/assets/clever-show.service | 1 +
 1 file changed, 1 insertion(+)

diff --git a/builder/assets/clever-show.service b/builder/assets/clever-show.service
index 5297b18c..c93c431b 100644
--- a/builder/assets/clever-show.service
+++ b/builder/assets/clever-show.service
@@ -6,6 +6,7 @@ After=clever.service
 WorkingDirectory=/home/pi/CleverSwarm/Drone
 EnvironmentFile=/lib/systemd/system/roscore.env
 ExecStart=/usr/bin/python /home/pi/CleverSwarm/Drone/copter_client.py
+KillSignal=SIGKILL
 Restart=on-failure
 RestartSec=3
 

From f724e9e830ae44520d9f8216ff3e31e21ea5399a Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Fri, 5 Jul 2019 15:41:06 +0300
Subject: [PATCH 25/27] Server: Add separate check for takeoff button

---
 Server/copter_table_models.py | 13 ++++++++++++-
 Server/server_qt.py           |  2 +-
 2 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py
index 5251ef41..e93fc16c 100644
--- a/Server/copter_table_models.py
+++ b/Server/copter_table_models.py
@@ -28,6 +28,7 @@ def __setitem__(self, key, value):
 class CopterDataModel(QtCore.QAbstractTableModel):
     checks = {}
     selected_ready_signal = QtCore.pyqtSignal(bool)
+    selected_takeoff_ready_signal = QtCore.pyqtSignal(bool)
 
     def __init__(self, parent=None):
         super(CopterDataModel, self).__init__(parent)
@@ -50,6 +51,10 @@ def selfchecked_ready(self, contents=()):
         contents = contents or self.data_contents
         return filter(lambda x: all_checks(x), contents)
 
+    def takeoff_ready(self, contents=()):
+        contents = contents or self.data_contents
+        return filter(lambda x: takeoff_checks(x), contents)
+
     def rowCount(self, n=None):
         return len(self.data_contents)
 
@@ -62,7 +67,6 @@ def headerData(self, section, orientation, role=Qt.DisplayRole):
                 return self.headers[section]
 
     def data(self, index, role=Qt.DisplayRole):
-        self.selected_ready_signal.emit(set(self.user_selected()).issubset(self.selfchecked_ready()))
         row = index.row()
         col = index.column()
         #print('row {}, col {}, role {}'.format(row, col, role))
@@ -86,6 +90,8 @@ def data(self, index, role=Qt.DisplayRole):
 
     def update_model(self, index=QtCore.QModelIndex()):
         #self.modelReset.emit()
+        self.selected_ready_signal.emit(set(self.user_selected()).issubset(self.selfchecked_ready()))
+        self.selected_takeoff_ready_signal.emit(set(self.user_selected()).issubset(self.takeoff_ready()))
         self.dataChanged.emit(index, index, (QtCore.Qt.EditRole,))
 
     @QtCore.pyqtSlot()
@@ -187,6 +193,11 @@ def all_checks(copter_item):
             return False
     return True
 
+def takeoff_checks(copter_item):
+    for i in range(3):
+        if not check_selfcheck(copter_item[2+i]):
+            return False
+    return True
 
 class CopterProxyModel(QtCore.QSortFilterProxyModel):
     def __init__(self, parent=None):
diff --git a/Server/server_qt.py b/Server/server_qt.py
index cfe69fac..6e56d41b 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -68,7 +68,7 @@ def init_model(self):
 
         # Connect model signals to UI
         self.model.selected_ready_signal.connect(self.ui.start_button.setEnabled)
-        self.model.selected_ready_signal.connect(self.ui.takeoff_button.setEnabled)
+        self.model.selected_takeoff_ready_signal.connect(self.ui.takeoff_button.setEnabled)
 
     def client_connected(self, client: Client):
         self.signals.add_client_signal.emit(CopterData(copter_id=client.copter_id, client=client))

From 445567850ab9a3d6045a5fb1ca173f901246369c Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Fri, 5 Jul 2019 18:34:38 +0300
Subject: [PATCH 26/27] Drone: Increase linear speed check limit

---
 Drone/FlightLib/FlightLib.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py
index ac37f913..cb653a13 100644
--- a/Drone/FlightLib/FlightLib.py
+++ b/Drone/FlightLib/FlightLib.py
@@ -107,7 +107,7 @@ def check_connection():
 
 
 @check("Linear velocity estimation")
-def check_linear_speeds(speed_limit=0.1):
+def check_linear_speeds(speed_limit=0.15):
     telemetry = get_telemetry(frame_id='body')
 
     if _check_nans(telemetry.vx, telemetry.vy, telemetry.vz):

From 7c34cfcde539cc39eb630d3215d398e995d8ebb0 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Fri, 5 Jul 2019 18:36:48 +0300
Subject: [PATCH 27/27] Server: Repaired bugs with takeoff ready check,
 confirmation_required decorators, resume time sending

---
 Server/copter_table_models.py |  2 +-
 Server/server_qt.py           | 33 ++++++++++++++++++---------------
 2 files changed, 19 insertions(+), 16 deletions(-)

diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py
index e93fc16c..57fe84f2 100644
--- a/Server/copter_table_models.py
+++ b/Server/copter_table_models.py
@@ -195,7 +195,7 @@ def all_checks(copter_item):
 
 def takeoff_checks(copter_item):
     for i in range(3):
-        if not check_selfcheck(copter_item[2+i]):
+        if not CopterDataModel.checks[2+i](copter_item[2+i]):
             return False
     return True
 
diff --git a/Server/server_qt.py b/Server/server_qt.py
index 6e56d41b..ed3339ff 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -27,7 +27,8 @@ def wrapper(*args, **kwargs):
             )
             if reply == QMessageBox.Yes:
                 print("Dialog accepted")
-                return f(*args, **kwargs)
+                #print(args)
+                return f(args[0])
             else:
                 print("Dialog declined")
 
@@ -77,7 +78,7 @@ def init_ui(self):
         # Connecting
         self.ui.check_button.clicked.connect(self.selfcheck_selected)
         self.ui.start_button.clicked.connect(self.send_starttime)
-        self.ui.pause_button.clicked.connect(self.pause_resume_all)
+        self.ui.pause_button.clicked.connect(self.pause_resume_selected)
         self.ui.stop_button.clicked.connect(self.stop_all)
         self.ui.emergency_button.clicked.connect(self.emergency)
 
@@ -130,24 +131,24 @@ def _set_copter_data(self, value, col, copter_id):
 
         self.signals.update_data_signal.emit(row, col, data)
 
-    @pyqtSlot()
     @confirmation_required("This operation will takeoff selected copters with delay and start animation. Proceed?")
-    def send_starttime(self):
+    @pyqtSlot()
+    def send_starttime(self, **kwargs):
         dt = self.ui.start_delay_spin.value()
         for copter in self.model.user_selected():
             if all_checks(copter):
                 server.send_starttime(copter.client, dt)
 
-    @pyqtSlot()
     @confirmation_required("This operation will takeoff copters immediately. Proceed?")
-    def takeoff_selected(self):
+    @pyqtSlot()
+    def takeoff_selected(self, **kwargs):
         for copter in self.model.user_selected():
             if all_checks(copter):
                 copter.client.send_message("takeoff")
 
-    @pyqtSlot()
     @confirmation_required("This operation will flip(!!!) copters immediately. Proceed?")
-    def flip(self):
+    @pyqtSlot()
+    def flip(self, **kwargs):
         for copter in self.model.user_selected():
             if all_checks(copter):
                 copter.client.send_message("flip")
@@ -162,17 +163,19 @@ def stop_all(self):
         Client.broadcast_message("stop")
 
     @pyqtSlot()
-    def pause_resume_all(self):
+    def pause_resume_selected(self):
         if self.ui.pause_button.text() == 'Pause':
-            Client.broadcast_message('pause')
+            for copter in self.model.user_selected():
+                copter.client.send_message("pause")
             self.ui.pause_button.setText('Resume')
         else:
-            self._resume_all()
+            self._resume_selected()
 
-    @confirmation_required("This operation will resume ALL copter tasks with given delay. Proceed?")
-    def _resume_all(self):
-        dt = self.ui.start_delay_spin.value()
-        Client.broadcast_message('resume', {"time": 0 if dt == 0 else server.time_now()})
+    #@confirmation_required("This operation will resume ALL copter tasks with given delay. Proceed?")
+    def _resume_selected(self, **kwargs):
+        time_gap = 0.1
+        for copter in self.model.user_selected():
+            copter.client.send_message('resume', {"time": server.time_now() + time_gap})
         self.ui.pause_button.setText('Pause')
 
     @pyqtSlot()