diff --git a/Drone/client.py b/Drone/client.py index b8e93dc..a38570a 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -58,14 +58,16 @@ class Client(object): self.NTP_HOST = self.config.get('NTP', 'host') self.NTP_PORT = self.config.getint('NTP', 'port') - self.files_directory = self.config.get('FILETRANSFER', 'files_directory') + self.files_directory = self.config.get('FILETRANSFER', 'files_directory') # not used?! self.client_id = self.config.get('PRIVATE', 'id') - if self.client_id == 'default': + if self.client_id == '/default': self.client_id = 'copter' + str(random.randrange(9999)).zfill(4) - self.write_config(False, 'PRIVATE', 'id', self.client_id) + self.write_config(False, ConfigOption('PRIVATE', 'id', self.client_id)) elif self.client_id == '/hostname': self.client_id = socket.gethostname() + elif self.client_id == '/ip': + self.client_id = messaging.get_ip_address() def rewrite_config(self): with open(self.config_path, 'w') as file: @@ -210,20 +212,28 @@ class Client(object): return -@messaging.request_callback("id") -def _response_id(): - return active_client.client_id - -@messaging.request_callback("time") -def _response_time(): - return active_client.time_now() - @messaging.message_callback("config_write") def _command_config_write(*args, **kwargs): options = [ConfigOption(**raw_option) for raw_option in kwargs["options"]] logger.info("Writing config options: {}".format(options)) active_client.write_config(kwargs["reload"], *options) + +@messaging.request_callback("id") +def _response_id(*args, **kwargs): + new_id = kwargs.get("new_id", None) + if new_id is not None: + cfg = ConfigOption("PRIVATE", "id", new_id) + active_client.write_config(True, cfg) + + return active_client.client_id + + +@messaging.request_callback("time") +def _response_time(*args, **kwargs): + return active_client.time_now() + + if __name__ == "__main__": client = Client() client.start() diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 92cd73b..b1ac6b9 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -131,7 +131,7 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1): @messaging.request_callback("selfcheck") -def _response_selfcheck(): +def _response_selfcheck(*args, **kwargs): if check_state_topic(wait_new_status=True): check = FlightLib.selfcheck() return check if check else "OK" @@ -141,7 +141,7 @@ def _response_selfcheck(): @messaging.request_callback("anim_id") -def _response_animation_id(): +def _response_animation_id(*args, **kwargs): # Load animation result = animation.get_id() if result != 'No animation': @@ -163,7 +163,7 @@ def _response_animation_id(): return result @messaging.request_callback("batt_voltage") -def _response_batt(): +def _response_batt(*args, **kwargs): if check_state_topic(wait_new_status=True): return FlightLib.get_telemetry('body').voltage else: @@ -172,7 +172,7 @@ def _response_batt(): @messaging.request_callback("cell_voltage") -def _response_cell(): +def _response_cell(*args, **kwargs): if check_state_topic(wait_new_status=True): return FlightLib.get_telemetry('body').cell_voltage else: @@ -180,37 +180,37 @@ def _response_cell(): return float('nan') @messaging.request_callback("sys_status") -def _response_sys_status(): +def _response_sys_status(*args, **kwargs): return get_sys_status() @messaging.request_callback("cal_status") -def _response_cal_status(): +def _response_cal_status(*args, **kwargs): return get_calibration_status() @messaging.request_callback("position") -def _response_position(): +def _response_position(*args, **kwargs): telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) return "{:.2f} {:.2f} {:.2f} {:.1f} {}".format( telem.x, telem.y, telem.z, math.degrees(telem.yaw), client.active_client.FRAME_ID) @messaging.request_callback("calibrate_gyro") -def _calibrate_gyro(): +def _calibrate_gyro(*args, **kwargs): calibrate('gyro') return get_calibration_status() @messaging.request_callback("calibrate_level") -def _calibrate_level(): +def _calibrate_level(*args, **kwargs): calibrate('level') return get_calibration_status() @messaging.message_callback("test") -def _command_test(**kwargs): +def _command_test(*args, **kwargs): logger.info("logging info test") print("stdout test") @messaging.message_callback("move_start") -def _command_move_start_to_current_position(**kwargs): +def _command_move_start_to_current_position(*args, **kwargs): # Load animation frames = animation.load_animation(os.path.abspath("animation.csv"), x0=client.active_client.X0_COMMON, @@ -232,7 +232,7 @@ def _command_move_start_to_current_position(**kwargs): print ("Start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0)) @messaging.message_callback("reset_start") -def _command_reset_start(**kwargs): +def _command_reset_start(*args, **kwargs): client.active_client.config.set('PRIVATE', 'x0', 0) client.active_client.config.set('PRIVATE', 'y0', 0) client.active_client.rewrite_config() @@ -240,7 +240,7 @@ def _command_reset_start(**kwargs): print ("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0)) @messaging.message_callback("set_z_to_ground") -def _command_set_z(**kwargs): +def _command_set_z(*args, **kwargs): telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) client.active_client.config.set('PRIVATE', 'z0', telem.z) client.active_client.rewrite_config() @@ -248,7 +248,7 @@ def _command_set_z(**kwargs): print ("Set z offset to {:.2f}".format(client.active_client.Z0)) @messaging.message_callback("reset_z_offset") -def _command_reset_z(**kwargs): +def _command_reset_z(*args, **kwargs): client.active_client.config.set('PRIVATE', 'z0', 0) client.active_client.rewrite_config() client.active_client.load_config() @@ -256,36 +256,36 @@ def _command_reset_z(**kwargs): @messaging.message_callback("update_repo") -def _command_update_repo(**kwargs): +def _command_update_repo(*args, **kwargs): os.system("git reset --hard origin/master") os.system("git fetch") os.system("git pull") os.system("chown -R pi:pi ~/CleverSwarm") @messaging.message_callback("reboot_fcu") -def _command_reboot(): +def _command_reboot(*args, **kwargs): reboot_fcu() @messaging.message_callback("service_restart") -def _command_service_restart(**kwargs): +def _command_service_restart(*args, **kwargs): restart_service(kwargs["name"]) @messaging.message_callback("repair_chrony") -def _command_chrony_repair(): +def _command_chrony_repair(*args, **kwargs): configure_chrony_ip(client.active_client.server_host) restart_service("chrony") @messaging.message_callback("led_test") -def _command_led_test(**kwargs): +def _command_led_test(*args, **kwargs): LedLib.chase(255, 255, 255) time.sleep(2) LedLib.off() @messaging.message_callback("led_fill") -def _command_led_fill(**kwargs): +def _command_led_fill(*args, **kwargs): r = kwargs.get("red", 0) g = kwargs.get("green", 0) b = kwargs.get("blue", 0) @@ -294,11 +294,11 @@ def _command_led_fill(**kwargs): @messaging.message_callback("flip") -def _copter_flip(): +def _copter_flip(*args, **kwargs): FlightLib.flip(frame_id=client.active_client.FRAME_ID) @messaging.message_callback("takeoff") -def _command_takeoff(**kwargs): +def _command_takeoff(*args, **kwargs): task_manager.add_task(time.time(), 0, animation.takeoff, task_kwargs={ "z": client.active_client.TAKEOFF_HEIGHT, @@ -310,7 +310,7 @@ def _command_takeoff(**kwargs): @messaging.message_callback("land") -def _command_land(**kwargs): +def _command_land(*args, **kwargs): task_manager.reset() task_manager.add_task(0, 0, animation.land, task_kwargs={ @@ -323,7 +323,7 @@ def _command_land(**kwargs): @messaging.message_callback("disarm") -def _command_disarm(**kwargs): +def _command_disarm(*args, **kwargs): task_manager.reset() task_manager.add_task(-5, 0, FlightLib.arming_wrapper, task_kwargs={ @@ -333,22 +333,22 @@ def _command_disarm(**kwargs): @messaging.message_callback("stop") -def _command_stop(**kwargs): +def _command_stop(*args, **kwargs): task_manager.reset() @messaging.message_callback("pause") -def _command_pause(**kwargs): +def _command_pause(*args, **kwargs): task_manager.pause() @messaging.message_callback("resume") -def _command_resume(**kwargs): +def _command_resume(*args, **kwargs): task_manager.resume(time_to_start_next_task=kwargs.get("time", 0)) @messaging.message_callback("start") -def _play_animation(**kwargs): +def _play_animation(*args, **kwargs): start_time = float(kwargs["time"]) # Check if animation file is available if animation.get_id() == 'No animation': diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index 6bf7b01..bffce18 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -2,11 +2,13 @@ import sys import re import collections import indexed +from server import ConfigOption from PyQt5 import QtCore, QtGui, QtWidgets from PyQt5.QtCore import Qt as Qt +ModelDataRole = 998 ModelStateRole = 999 @@ -76,6 +78,9 @@ class CopterDataModel(QtCore.QAbstractTableModel): self.headers = ('copter ID', ' animation ID ', 'batt V', 'batt %', ' system ', 'calibration', 'selfcheck', 'current x y z yaw frame_id', 'time delta') self.data_contents = [] + + self.on_id_changed = None + self.first_col_is_checked = False def insertRows(self, contents, position='last', parent=QtCore.QModelIndex()): @@ -118,13 +123,21 @@ class CopterDataModel(QtCore.QAbstractTableModel): contents = contents or self.data_contents return filter(lambda x: calibration_ready_check(x), contents) - def get_row_by_id(self, copter_id): + def get_row_index(self, row_data): try: - row = next(filter(lambda x: x.copter_id == copter_id, self.data_contents)) + index = self.data_contents.index(row_data) + except ValueError: + return None + else: + return index + + def get_row_by_attr(self, attr, value): + try: + row_data = next(filter(lambda x: getattr(x, attr, None) == value, self.data_contents)) except StopIteration: return None else: - return self.data_contents.index(row) + return row_data def rowCount(self, n=None): return len(self.data_contents) @@ -140,7 +153,7 @@ class CopterDataModel(QtCore.QAbstractTableModel): def data(self, index, role=Qt.DisplayRole): row = index.row() col = index.column() - if role == Qt.DisplayRole: + if role == Qt.DisplayRole or role == Qt.EditRole: # Separate editRole in case of editing non-text return self.data_contents[row][col] or "" elif role == Qt.BackgroundRole: @@ -180,12 +193,26 @@ class CopterDataModel(QtCore.QAbstractTableModel): if not index.isValid(): return False + col = index.column() + row = index.row() + if role == Qt.CheckStateRole: - self.data_contents[index.row()].states.checked = value - elif role == Qt.EditRole: - self.data_contents[index.row()][index.column()] = value + self.data_contents[row].states.checked = value + elif role == Qt.EditRole: # For user actions with data + if col == 0 and self.on_id_changed: + self.data_contents[row][col] = "Awaiting for response" + self.data_contents[row].states.copter_id = None + + self.data_contents[row].client.get_response("id", self.on_id_changed, + request_args={"new_id": value}, + callback_args=(self.data_contents[row],)) + else: + self.data_contents[row][col] = value + + elif role == ModelDataRole: # For inner setting\editing of data + self.data_contents[row][col] = value elif role == ModelStateRole: - self.data_contents[index.row()].states[index.column()] = value + self.data_contents[row].states[col] = value else: return False @@ -201,7 +228,7 @@ class CopterDataModel(QtCore.QAbstractTableModel): def flags(self, index): roles = Qt.ItemIsSelectable | Qt.ItemIsEnabled if index.column() == 0: - roles |= Qt.ItemIsUserCheckable #| Qt.ItemIsEditable + roles |= Qt.ItemIsUserCheckable | Qt.ItemIsEditable return roles @QtCore.pyqtSlot(int, int, QtCore.QVariant, QtCore.QVariant) diff --git a/Server/server.py b/Server/server.py index c765806..7d55229 100644 --- a/Server/server.py +++ b/Server/server.py @@ -40,7 +40,7 @@ class Server(messaging.Singleton): self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.host = socket.gethostname() - self.ip = Server.get_ip_address() + self.ip = messaging.get_ip_address() # Init configs self.config_path = config_path @@ -112,16 +112,6 @@ class Server(messaging.Singleton): sys.exit("Stopped") - @staticmethod - def get_ip_address(): - try: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as ip_socket: - ip_socket.connect(("8.8.8.8", 80)) - return ip_socket.getsockname()[0] - except OSError: - logging.warning("No network connection detected, starting on localhost") - return "localhost" - @staticmethod def get_ntp_time(ntp_host, ntp_port): NTP_DELTA = 2208988800 # 1970-01-01 00:00:00 @@ -218,7 +208,7 @@ class Server(messaging.Singleton): try: while self.listener_thread_running.is_set(): - data, addr = broadcast_client.recvfrom(1024) # TODO nonblock + data, addr = broadcast_client.recvfrom(1024) # TODO nonblock message = messaging.MessageManager() message.income_raw = data message.process_message() diff --git a/Server/server_qt.py b/Server/server_qt.py index ac8a49f..85ac2c1 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -45,11 +45,10 @@ def confirmation_required(text="Are you sure?", label="Confirm operation?"): QMessageBox.Yes | QMessageBox.No, QMessageBox.No ) if reply == QMessageBox.Yes: - print("Dialog accepted") + logging.debug("Dialog accepted") return f(*args, **kwargs) - else: - print("Dialog declined") + logging.debug("Dialog declined") return wrapper @@ -75,6 +74,8 @@ class MainWindow(QtWidgets.QMainWindow): self.show() def init_model(self): + self.model.on_id_changed = self.set_copter_id + self.proxy_model.setDynamicSortFilter(True) self.proxy_model.setSourceModel(self.model) @@ -111,9 +112,10 @@ class MainWindow(QtWidgets.QMainWindow): self.signals.add_client_signal.emit(StatedCopterData(copter_id=client.copter_id, client=client)) def client_connection_changed(self, client: Client): - row_num = self.model.get_row_by_id(client.copter_id) + row_data = self.model.get_row_by_attr("client", client) + row_num = self.model.get_row_index(row_data) if row_num is not None: - if Server().remove_disconnected: + if Server().remove_disconnected and (not client.connected): client.remove() self.signals.remove_client_signal.emit(row_num) else: @@ -163,20 +165,20 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def selfcheck_selected(self): - for copter in self.model.user_selected(): - client = copter.client + for copter_data_row in self.model.user_selected(): + client = copter_data_row.client - 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("sys_status", self._set_copter_data, callback_args=(4, copter.copter_id)) - client.get_response("cal_status", self._set_copter_data, callback_args=(5, copter.copter_id)) - client.get_response("selfcheck", self._set_copter_data, callback_args=(6, copter.copter_id)) - client.get_response("position", self._set_copter_data, callback_args=(7, copter.copter_id)) - client.get_response("time", self._set_copter_data, callback_args=(8, copter.copter_id)) + client.get_response("anim_id", self.set_copter_data, callback_args=(1, copter_data_row)) + client.get_response("batt_voltage", self.set_copter_data, callback_args=(2, copter_data_row)) + client.get_response("cell_voltage", self.set_copter_data, callback_args=(3, copter_data_row)) + client.get_response("sys_status", self.set_copter_data, callback_args=(4, copter_data_row)) + client.get_response("cal_status", self.set_copter_data, callback_args=(5, copter_data_row)) + client.get_response("selfcheck", self.set_copter_data, callback_args=(6, copter_data_row)) + client.get_response("position", self.set_copter_data, callback_args=(7, copter_data_row)) + client.get_response("time", self.set_copter_data, callback_args=(8, copter_data_row)) - def _set_copter_data(self, value, col, copter_id): - row = self.model.get_row_by_id(copter_id) + def set_copter_data(self, value, col, copter_data_row): + row = self.model.get_row_index(copter_data_row) if row is None: logging.error("No such client!") return @@ -197,16 +199,25 @@ class MainWindow(QtWidgets.QMainWindow): elif col == 7: data = str(value) elif col == 8: - #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) + copter_data_row.client.send_message("repair_chrony") else: - print("No column matched for response") + logging.error("No column matched for response") return - self.signals.update_data_signal.emit(row, col, data, Qt.EditRole) + self.signals.update_data_signal.emit(row, col, data, ModelDataRole) + + def set_copter_id(self, value, copter_data_row): + col = 0 + row = self.model.get_row_index(copter_data_row) + if row is None: + logging.error("No such client!") + return + + copter_data_row.client.copter_id = value + self.signals.update_data_signal.emit(row, col, value, ModelDataRole) + self.signals.update_data_signal.emit(row, col, True, ModelStateRole) @pyqtSlot() def remove_selected(self): @@ -290,35 +301,33 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def calibrate_gyro_selected(self): - for copter in self.model.user_selected(): - client = copter.client + for copter_data_row in self.model.user_selected(): + client = copter_data_row.client # Update calibration status - row = self.model.data_contents.index(next(filter( - lambda x: x.copter_id == client.copter_id, self.model.data_contents))) + row = self.model.get_row_index(copter_data_row) col = 5 data = 'CALIBRATING' - self.signals.update_data_signal.emit(row, col, data, Qt.EditRole) + self.signals.update_data_signal.emit(row, col, data, ModelDataRole) # Send request - client.get_response("calibrate_gyro", self._get_calibration_info, callback_args=(5, copter.copter_id)) + client.get_response("calibrate_gyro", self._get_calibration_info, callback_args=(copter_data_row, )) @pyqtSlot() def calibrate_level_selected(self): - for copter in self.model.user_selected(): - client = copter.client + for copter_data_row in self.model.user_selected(): + client = copter_data_row.client # Update calibration status - row = self.model.data_contents.index(next(filter( - lambda x: x.copter_id == client.copter_id, self.model.data_contents))) + row = self.model.get_row_index(copter_data_row) col = 5 data = 'CALIBRATING' - self.signals.update_data_signal.emit(row, col, data, Qt.EditRole) + self.signals.update_data_signal.emit(row, col, data, ModelDataRole) # Send request - client.get_response("calibrate_level", self._get_calibration_info, callback_args=(5, copter.copter_id)) + client.get_response("calibrate_level", self._get_calibration_info, callback_args=(copter_data_row, )) - def _get_calibration_info(self, value, col, copter_id): - row = self.model.data_contents.index(next( - filter(lambda x: x.copter_id == copter_id, self.model.data_contents))) + def _get_calibration_info(self, value, copter_data_row): + col = 5 + row = self.model.get_row_index(copter_data_row) data = str(value) - self.signals.update_data_signal.emit(row, col, data, Qt.EditRole) + self.signals.update_data_signal.emit(row, col, data, ModelDataRole) @pyqtSlot() def send_animations(self): diff --git a/messaging_lib.py b/messaging_lib.py index d1c9368..2e9a9b8 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -8,6 +8,8 @@ import logging import threading import collections +from contextlib import closing + try: import selectors except ImportError: @@ -24,6 +26,16 @@ logger = logging.getLogger(__name__) # logger = logging_lib.Logger(_logger, True) +def get_ip_address(): + try: + with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as ip_socket: + ip_socket.connect(("8.8.8.8", 80)) + return ip_socket.getsockname()[0] + except OSError: + logging.warning("No network connection detected, using localhost") + return "localhost" + + class _Singleton(type): """ A metaclass that creates a Singleton base class when called. """ _instances = {} @@ -323,7 +335,7 @@ class ConnectionManager(object): command = message.content["command"] args = message.content["args"] try: - self.messages_callbacks[command](**args) + self.messages_callbacks[command](self, **args) except KeyError: logger.warning("Command {} does not exist!".format(command)) except Exception as error: @@ -334,7 +346,7 @@ class ConnectionManager(object): request_id = message.content["request_id"] args = message.content["args"] try: - value = self.requests_callbacks[command](**args) + value = self.requests_callbacks[command](self, **args) except KeyError: logger.warning("Request {} does not exist!".format(command)) except Exception as error: # TODO send response error\cancel