From ce36c6f1e37be8044dfd2f8ff5a356221b7d6674 Mon Sep 17 00:00:00 2001 From: artem30801 <38689676+artem30801@users.noreply.github.com> Date: Thu, 5 Dec 2019 15:10:21 +0300 Subject: [PATCH] Feature branch: IMPORTANT connection+telemetry+table fixes and improvements (#55) * .client_connected > .new_client_connected * Fixed 'confirmation_required' wrapper * Logging impr * Changed and optimized a lot checks behaviour * Added indication of connected/disconnected copters * update_data_signal changed signature * Added client removing functionality * Option for automatically remove disconnected copters from table * Renaming copters from QT server table on the go + some improvements * Server: Check if self.clients list is not empty when trying to pop element from it * Probably fixes behaviour of non-immidiate data sending from server * Added changing hostname of copter * Updated config * Preview of selfchecheck results on double click * Delete doc_2019-10-16_17-57-17.bashrc * Update table data models for selfcheck * Server: modify set id request to message * Update client_config default file * Client: modify set new id function * Client: add avahi-daemon to restart when restarting network * Client: add new hostname to ssh motd message, do not change hostname if no network restart in config * Client: add newline to motd message * Optimized request behaviour * Client: fix service file and restart order * Client: Add SO_KEEPALIVE and TCP_NODELAY options to client socket * Modify to last tests with ping * Client: remove ping * Client: select reboot option when change id and add execute command * Server: Add SO_KEEPALIVE option to server socket * Server: Change removing copter * Request resending after disconnection * Resending improval (for furthrer functionality & fixes * Fix of client removing behaviour * Debugging * Revert dubug code; 'Remove' fix confirmed * do not clear requests queue * Update requirements.txt * Added namespace class to fix resend * Improvements and simplification of notifier + port to client * Refactor of telemetry thread * Simplify lambdas * Compress hostname check to single regex * Changes in telemetry * Refactored formatting of telemetry in table. NOT DONE * Fix * Git checkout. REVERT later! * Conection fix * Compability fixes * Update start position * Fix for reconnection with notifier socket * Added traceback for pyqt5 * Fixes in new telemetry display * Added lock to Telemetry * Fixes for table display * Fix of doubling line of client in table * Fix of mass-removing clients from table * Fix for clinet double-connection+removal * Fix lock in Telemetry * Changed signature of response callbacks for better syntax & fixes (all tested) * Revert "Git checkout. REVERT later!" This reverts commit 6122352380b8b973957640d3f4f9cfb113ffeee9. * Server: fix formatters * Client: Remove telemetry_loop, small refactor of Telemetry class * Server: Add formatters * Server: Very small refactor * Server: Fix checks and formatters * Client: Fix check_failsafe function, small code refactor * Client: update default config file --- Drone/client.py | 9 +- Drone/client_config.ini | 1 - Drone/copter_client.py | 363 ++++++++++++++++++++------------- Drone/mavros_mavlink.py | 4 +- Server/copter_table_models.py | 371 +++++++++++++++++++++------------- Server/server.py | 31 +-- Server/server_qt.py | 180 +++++++++-------- messaging_lib.py | 100 +++++---- requirements.txt | 1 + 9 files changed, 635 insertions(+), 425 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index a52e704..5d37e16 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -105,6 +105,8 @@ class Client(object): def start(self): logger.info("Starting client") + messaging.NotifierSock().init(self.selector) + try: while True: self._reconnect() @@ -195,6 +197,7 @@ class Client(object): # self.server_connection.send_message("ping") # self._last_ping_time = time.time() # logging.debug("tick") + for key, mask in events: # TODO add notifier to client! connection = key.data if connection is None: @@ -214,8 +217,10 @@ class Client(object): if error.errno == errno.EINTR: raise KeyboardInterrupt - - if not self.selector.get_map(): + mapping = self.selector.get_map().values() + notifier_key = self.selector.get_key(messaging.NotifierSock().get_sock()) + notify_only= len(mapping) == 1 and notifier_key in mapping + if notify_only or not mapping: logger.warning("No active connections left!") return diff --git a/Drone/client_config.ini b/Drone/client_config.ini index 251c7ad..cc0e86f 100644 --- a/Drone/client_config.ini +++ b/Drone/client_config.ini @@ -23,7 +23,6 @@ timeout_to_disarm_after_watchdog_action = 10.0 [TELEMETRY] frequency = 1 transmit = True -clear_tasks_when_emergency = True land_if_pos_delta_bigger_than = 3.0 log_cpu_and_memory = True diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 49422d0..148215a 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -29,11 +29,8 @@ from tf.transformations import quaternion_from_euler, euler_from_quaternion, qua import tf2_ros static_bloadcaster = tf2_ros.StaticTransformBroadcaster() -Telemetry = namedtuple("Telemetry", "git_version animation_id battery_v battery_p system_status calibration_status mode selfcheck current_position start_position armed") -telemetry = Telemetry('nan', 'No animation', 'nan', 'nan', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_POS', 'NO_POS', False) -emergency = False -# get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) +emergency = False logging.basicConfig( # TODO all prints as logs level=logging.DEBUG, # INFO @@ -78,7 +75,6 @@ class CopterClient(client.Client): super(CopterClient, self).load_config() self.TELEM_FREQ = self.config.getfloat('TELEMETRY', 'frequency') self.TELEM_TRANSMIT = self.config.getboolean('TELEMETRY', 'transmit') - self.CLEAR_TASKS_WHEN_EMERGENCY = self.config.getboolean('TELEMETRY', 'clear_tasks_when_emergency') self.LOG_CPU_AND_MEMORY = self.config.getboolean('TELEMETRY', 'log_cpu_and_memory') self.LAND_POS_DELTA = self.config.getfloat('TELEMETRY', 'land_if_pos_delta_bigger_than') self.FRAME_ID = self.config.get('COPTERS', 'frame_id') @@ -134,7 +130,8 @@ class CopterClient(client.Client): else: rospy.logerror("Can't make floor frame!") start_subscriber() - telemetry_thread.start() + + telemetry.start_loop() super(CopterClient, self).start() def start_floor_frame_broadcast(self): @@ -307,9 +304,10 @@ def _response_selfcheck(*args, **kwargs): stop_subscriber() return "NOT_CONNECTED_TO_FCU" + @messaging.request_callback("telemetry") def _response_telemetry(*args, **kwargs): - return create_telemetry_message(telemetry) + return telemetry.create_msg_contents() @messaging.request_callback("anim_id") @@ -663,148 +661,239 @@ def _play_animation(*args, **kwargs): }, ) -def telemetry_loop(): - global telemetry, emergency - last_state = [] - equal_state_counter = 0 - max_count = 2 - tasks_cleared = False - rate = rospy.Rate(client.active_client.TELEM_FREQ) - while not rospy.is_shutdown(): - telemetry = telemetry._replace(animation_id = animation.get_id()) - telemetry = telemetry._replace(git_version = subprocess.check_output("git log --pretty=format:'%h' -n 1", shell=True)) +class Telemetry: + params_default_dict = { + "git_version": None, + "animation_id": None, + "battery": None, + "armed": False, + "system_status": None, + "calibration_status": None, + "mode": None, + "selfcheck": None, + "current_position": None, + "start_position": None, + "time": None, + } + + def __init__(self): + self._lock = threading.Lock() + self._last_state = [] + self._interruption_counter = 0 + self._max_interruptions = 2 + self._tasks_cleared = False + + for key, value in self.params_default_dict.items(): + setattr(self, key, value) + + def __setattr__(self, key, value): + if key in self.params_default_dict: + with self.__dict__['_lock']: + self.__dict__[key] = value + else: + self.__dict__[key] = value + + def __getattr__(self, item): + if item in self.params_default_dict: + with self.__dict__['_lock']: + return self.__dict__[item] + + return self.__dict__[item] + + @classmethod + def get_git_version(cls): + return subprocess.check_output("git log --pretty=format:'%h' -n 1", shell=True) + + @classmethod + def get_start_position(cls): x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"), - x_ratio=client.active_client.X_RATIO, - y_ratio=client.active_client.Y_RATIO, - ) + x_ratio=client.active_client.X_RATIO, + y_ratio=client.active_client.Y_RATIO, + ) x_delta = client.active_client.X0 + client.active_client.X0_COMMON y_delta = client.active_client.Y0 + client.active_client.Y0_COMMON z_delta = client.active_client.Z0 + client.active_client.Z0_COMMON - if not math.isnan(x_start): - telemetry = telemetry._replace(start_position = '{:.2f} {:.2f} {:.2f}'.format(x_start+x_delta, y_start+y_delta, z_delta)) - else: - telemetry = telemetry._replace(start_position = 'NO_POS') - services_unavailable = FlightLib.check_ros_services_unavailable() - if not services_unavailable: - try: - ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID) - if ros_telemetry.connected: - telemetry = telemetry._replace(armed = ros_telemetry.armed) - telemetry = telemetry._replace(battery_v = '{:.2f}'.format(ros_telemetry.voltage)) - batt_empty_param = get_param('BAT_V_EMPTY') - batt_charged_param = get_param('BAT_V_CHARGED') - batt_cells_param = get_param('BAT_N_CELLS') - if batt_empty_param.success and batt_charged_param.success and batt_cells_param.success: - batt_empty = batt_empty_param.value.real - batt_charged = batt_charged_param.value.real - batt_cells = batt_cells_param.value.integer - try: - telemetry = telemetry._replace(battery_p = '{}'.format(int(min((ros_telemetry.voltage/batt_cells - batt_empty)/(batt_charged - batt_empty)*100., 100)))) - except ValueError: - telemetry = telemetry._replace(battery_p = 'nan') - else: - telemetry = telemetry._replace(battery_p = 'nan') - telemetry = telemetry._replace(calibration_status = get_calibration_status()) - telemetry = telemetry._replace(system_status = get_sys_status()) - telemetry = telemetry._replace(mode = ros_telemetry.mode) - check = FlightLib.selfcheck() - if not check: - check = "OK" - telemetry = telemetry._replace(selfcheck = str(check)) - if not math.isnan(ros_telemetry.x): - telemetry = telemetry._replace(current_position = '{:.2f} {:.2f} {:.2f} {:.1f} {}'.format(ros_telemetry.x, ros_telemetry.y, ros_telemetry.z, - math.degrees(ros_telemetry.yaw), client.active_client.FRAME_ID)) - else: - telemetry = telemetry._replace(current_position = 'NO_POS in {}'.format(client.active_client.FRAME_ID)) - else: - telemetry = telemetry._replace(battery_v = 'nan') - telemetry = telemetry._replace(battery_p = 'nan') - telemetry = telemetry._replace(calibration_status = 'NO_FCU') - telemetry = telemetry._replace(system_status = 'NO_FCU') - telemetry = telemetry._replace(mode = 'NO_FCU') - telemetry = telemetry._replace(selfcheck = 'NO_FCU') - telemetry = telemetry._replace(current_position = 'NO_POS') - except rospy.ServiceException: - logger.debug("Some service is unavailable") - except AttributeError as e: - logger.debug(e) - except rospy.TransportException as e: - logger.debug(e) - else: - telemetry = telemetry._replace(selfcheck = 'WAIT_ROS') - if client.active_client.TELEM_TRANSMIT: - try: - client.active_client.server_connection.send_message('telem', args={'message':create_telemetry_message(telemetry)}) - except AttributeError as e: - logger.debug(e) - if client.active_client.CLEAR_TASKS_WHEN_EMERGENCY: - mode = telemetry.mode - armed = telemetry.armed - last_task = task_manager.get_last_task_name() - state = [mode, armed, last_task] - if state == last_state: - equal_state_counter += 1 - else: - equal_state_counter = 0 - external_interruption = (mode != "OFFBOARD" and armed == True and last_task not in [None, 'land']) - log_msg = '' - if emergency and external_interruption: - log_msg = "emergency and external interruption" - elif emergency: - log_msg = "emergency" - elif external_interruption: - log_msg = "external interruption" - logger.info("Possible expernal interruption, state_counter = {}".format(equal_state_counter)) - if emergency or (external_interruption and equal_state_counter >= max_count): - if not tasks_cleared: - logger.info("Clear task manager because of {}".format(log_msg)) - logger.info("Mode: {} | armed: {} | last task: {} ".format(mode, armed, last_task)) - task_manager.reset() - FlightLib.reset_delta() - tasks_cleared = True - equal_state_counter = 0 - else: - tasks_cleared = False - last_state = state - if client.active_client.LOG_CPU_AND_MEMORY: - cpu_usage = psutil.cpu_percent(interval=None, percpu=True) - mem_usage = psutil.virtual_memory().percent - cpu_temp_info = psutil.sensors_temperatures()['cpu-thermal'][0] - cpu_temp = cpu_temp_info.current - # https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md - throttled_hex = subprocess.check_output("vcgencmd get_throttled", shell=True).split('=')[1] - under_voltage = bool(int(bin(int(throttled_hex,16))[2:][-1])) - power_state = 'normal' if not under_voltage else 'under voltage!' - if cpu_temp_info.critical: - cpu_temp_state = 'critical' - elif cpu_temp_info.high: - cpu_temp_state = 'high' - else: - cpu_temp_state = 'normal' - logger.info("CPU usage: {} | Memory: {} % | T: {} ({}) | Power: {}".format(cpu_usage, mem_usage, cpu_temp, cpu_temp_state, power_state)) - delta = FlightLib.get_delta() - logger.info("Delta: {}".format(delta)) - if delta > client.active_client.LAND_POS_DELTA: - _command_land() - rate.sleep() + x = x_start + x_delta + y = y_start + y_delta + if not FlightLib._check_nans(x, y, z_delta): + return x, y, z_delta + return 'NO_POS' -def create_telemetry_message(telemetry): - msg = client.active_client.client_id + '`' - for key in telemetry.__dict__: - if key != 'armed': - msg += telemetry.__dict__[key] + '`' - msg += repr(time.time()) - return msg + @classmethod + def get_battery(cls, ros_telemetry): + battery_v = ros_telemetry.voltage + + batt_empty_param = get_param('BAT_V_EMPTY') + batt_charged_param = get_param('BAT_V_CHARGED') + batt_cells_param = get_param('BAT_N_CELLS') + + if batt_empty_param.success and batt_charged_param.success and batt_cells_param.success: + batt_empty = batt_empty_param.value.real + batt_charged = batt_charged_param.value.real + batt_cells = batt_cells_param.value.integer + + battery_p = (ros_telemetry.voltage / batt_cells - batt_empty) / (batt_charged - batt_empty) * 1. + battery_p = max(min(battery_p, 1.), 0.) + else: + battery_p = float('nan') + + return battery_v, battery_p + + @classmethod + def get_selfcheck(cls): + check = FlightLib.selfcheck() + if not check: + check = "OK" + return check + + @classmethod + def get_position(cls, ros_telemetry): + x, y, z = ros_telemetry.x, ros_telemetry.y, ros_telemetry.z + if not math.isnan(x): + return x, y, z, math.degrees(ros_telemetry.yaw), client.active_client.FRAME_ID + return 'NO_POS' + + def update_telemetry(self): + self.animation_id = animation.get_id() + self.git_version = self.get_git_version() + self.start_position = self.get_start_position() + try: + ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID) + if ros_telemetry.connected: + self.battery = self.get_battery(ros_telemetry) + self.armed = ros_telemetry.armed + self.calibration_status = get_calibration_status() + self.system_status = get_sys_status() + self.mode = ros_telemetry.mode + self.selfcheck = self.get_selfcheck() + self.current_position = self.get_position(ros_telemetry) + else: + self.reset_telemetry_values() + except rospy.ServiceException: + rospy.logdebug("Some service is unavailable") + self.selfcheck = ["WAIT_ROS"] + except AttributeError as e: + rospy.logdebug(e) + except rospy.TransportException as e: + rospy.logdebug(e) + self.time = time.time() + + def round_telemetry(self): + round_list = ["battery", "start_position", "current_position"] + for key in round_list: + if self.__dict__[key] not in [None, 'NO_POS', 'NO_FCU']: + self.__dict__[key] = [round(v,2) if type(v) == float else v for v in self.__dict__[key]] + + def reset_telemetry_values(self): + self.battery = float('nan'), float('nan') + self.calibration_status = 'NO_FCU' + self.system_status = 'NO_FCU' + self.mode = 'NO_FCU' + self.selfcheck = ['NO_FCU'] + self.current_position = 'NO_POS' + + def check_failsafe(self): + global emergency + # check current state + state = [self.mode, self.armed, task_manager.get_last_task_name()] + mode, armed, last_task = state + # check external interruption + external_interruption = (mode != "OFFBOARD" and armed == True and last_task not in [None, 'land']) + log_msg = '' + if emergency: + log_msg += 'emergency and ' + if external_interruption: + log_msg += 'external interruption and ' + # count interruptions to avoid px4 mode glitches + if state == self._last_state: + self._interruption_counter += 1 + else: + self._interruption_counter = 0 + logger.info("Possible expernal interruption, state_counter = {}".format(self._interruption_counter)) + # delete last ' end ' from log message + if len(log_msg) > 5: + log_msg = log_msg[:-5] + # clear task manager if emergency or external interruption + if emergency or (external_interruption and self._interruption_counter >= self._max_interruptions): + if not self._tasks_cleared: + logger.info("Clear task manager because of {}".format(log_msg)) + logger.info("Mode: {} | armed: {} | last task: {} ".format(mode, armed, last_task)) + task_manager.reset() + FlightLib.reset_delta() + self._tasks_cleared = True + self._interruption_counter = 0 + else: + self._tasks_cleared = False + self._last_state = state + # check position delta + if not emergency: + delta = FlightLib.get_delta() + if delta > client.active_client.LAND_POS_DELTA: + logger.info("Delta: {}".format(delta)) + _command_land() + + def transmit_message(self): + try: + client.active_client.server_connection.send_message('telemetry', args={'value': self.create_msg_contents()}) + except AttributeError as e: + logger.debug(e) + + @classmethod + def log_cpu_and_memory(cls): + cpu_usage = psutil.cpu_percent(interval=None, percpu=True) + mem_usage = psutil.virtual_memory().percent + cpu_temp_info = psutil.sensors_temperatures()['cpu-thermal'][0] + cpu_temp = cpu_temp_info.current + # https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md + throttled_hex = subprocess.check_output("vcgencmd get_throttled", shell=True).split('=')[1] + under_voltage = bool(int(bin(int(throttled_hex,16))[2:][-1])) + power_state = 'normal' if not under_voltage else 'under voltage!' + if cpu_temp_info.critical: + cpu_temp_state = 'critical' + elif cpu_temp_info.high: + cpu_temp_state = 'high' + else: + cpu_temp_state = 'normal' + logger.info("CPU usage: {} | Memory: {} % | T: {} ({}) | Power: {}".format( + cpu_usage, mem_usage, cpu_temp, cpu_temp_state, power_state)) + + def _update_loop(self, freq): # TODO extract? + rate = rospy.Rate(freq) + while not rospy.is_shutdown(): + + self.update_telemetry() + self.round_telemetry() + self.check_failsafe() + + if client.active_client.TELEM_TRANSMIT and client.active_client.connected: + self.transmit_message() + + if client.active_client.LOG_CPU_AND_MEMORY: + self.log_cpu_and_memory() + + rate.sleep() + + def start_loop(self): + if client.active_client.TELEM_FREQ > 0: + telemetry_thread = threading.Thread(target=self._update_loop, name="Telemetry getting thread", + args=(client.active_client.TELEM_FREQ,)) # TODO MOVE? Daemon? + telemetry_thread.start() + else: + logger.info("Don't create telemetry loop because of zero or negative telemetry frequency") + + def create_msg_contents(self, keys=None): # keys: set or list + if keys is None: + keys = self.params_default_dict.keys() + # return only existing keys from 'keys' + return {k: self.__dict__[k] for k in keys if k in self.params_default_dict} def emergency_callback(data): global emergency emergency = data.data -telemetry_thread = threading.Thread(target=telemetry_loop, name="Telemetry getting thread") - if __name__ == "__main__": - + telemetry = Telemetry() copter_client = CopterClient() task_manager = tasking.TaskManager() rospy.Subscriber('/emergency', Bool, emergency_callback) diff --git a/Drone/mavros_mavlink.py b/Drone/mavros_mavlink.py index 3b093da..7a6005e 100644 --- a/Drone/mavros_mavlink.py +++ b/Drone/mavros_mavlink.py @@ -108,9 +108,9 @@ def get_sys_status(): mavlink.MAV_STATE_EMERGENCY: "EMERGENCY", mavlink.MAV_STATE_POWEROFF: "POWEROFF", mavlink.MAV_STATE_FLIGHT_TERMINATION: "TERMINATION" - }.get(system_status, "NOT_CONNECTED_TO_FCU") + }.get(system_status, "NO_FCU") return status_text - return "NOT_CONNECTED_TO_FCU" + return "NO_FCU" def start_subscriber(): global heartbeat_sub, heartbeat_sub_status diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index 977cdfc..1d9e62e 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -1,10 +1,10 @@ -import sys import re +import sys +import time import math import configparser import collections import indexed -from server import ConfigOption from PyQt5 import QtCore, QtGui, QtWidgets from PyQt5.QtCore import Qt as Qt @@ -16,11 +16,100 @@ ModelStateRole = 999 config = configparser.ConfigParser() config.read("server_config.ini") +battery_min = config.getfloat('CHECKS', 'battery_percentage_min') +start_pos_delta_max = config.getfloat('CHECKS', 'start_pos_delta_max') +time_delta_max = config.getfloat('CHECKS', 'time_delta_max') + +class ModelChecks: + checks_dict = {} + takeoff_checklist = (3, 4, 6, 7, 8) + + @classmethod + def col_check(cls, col): + def inner(f): + def wrapper(item): + if item is not None: + return f(item) + return None + + cls.checks_dict[col] = wrapper + return wrapper + + return inner + + @classmethod + def all_checks(cls, copter_item): + for col, check in cls.checks_dict.items(): + if not check(copter_item[col]): + return False + return True + + @classmethod + def takeoff_checks(cls, copter_item): + for col in cls.takeoff_checklist: + if not cls.checks_dict[col](copter_item[col]): + return False + return True + + +@ModelChecks.col_check(1) +def check_ver(item): + return True # TODO git version! + + +@ModelChecks.col_check(2) +def check_anim(item): + return str(item) != 'No animation' + + +@ModelChecks.col_check(3) +def check_bat(item): + if item == "NO_INFO": + return False + return item[1]*100 > battery_min + + +@ModelChecks.col_check(4) +def check_sys_status(item): + return item == "STANDBY" + + +@ModelChecks.col_check(5) +def check_cal_status(item): + return item == "OK" + + +@ModelChecks.col_check(6) +def check_mode(item): + return (item != "NO_FCU") and not ("CMODE" in item) + + +@ModelChecks.col_check(7) +def check_selfcheck(item): + return item == "OK" + + +@ModelChecks.col_check(8) +def check_pos_status(item): + if item == 'NO_POS': + return False + return not math.isnan(item[0]) + + +@ModelChecks.col_check(9) +def check_start_pos_status(item): + return item != 'NO_POS' + + +@ModelChecks.col_check(10) +def check_time_delta(item): + return abs(item) < time_delta_max + class CopterData: class_basic_attrs = indexed.IndexedOrderedDict([('copter_id', None), ('git_ver', None), ('anim_id', None), ('battery', None), ('sys_status', None), ('cal_status', None), - ('mode', None), ('selfcheck', None), ('position', None), + ('mode', None), ('selfcheck', None), ('position', None), ('start_pos', None), ('time_delta', None), ('client', None)]) def __init__(self, **kwargs): @@ -41,8 +130,9 @@ class StatedCopterData(CopterData): class_basic_states = indexed.IndexedOrderedDict([("checked", 0), ("selfchecked", None), ("takeoff_ready", None), ("copter_id", True), ]) - def __init__(self, **kwargs): + def __init__(self, checks_class=ModelChecks, **kwargs): self.states = CopterData(**self.class_basic_states) + self.checks = ModelChecks super(StatedCopterData, self).__init__(**kwargs) @@ -52,31 +142,30 @@ class StatedCopterData(CopterData): if key in self.class_basic_attrs.keys(): try: self.states.__dict__[key] = \ - Checks.all_checks[self.attrs_dict.keys().index(key)](value) + ModelChecks.checks_dict[self.attrs_dict.keys().index(key)](value) if key == 'start_pos': if (self.__dict__['position'] is not None) and (self.__dict__['start_pos'] is not None): current_pos = get_position(self.__dict__['position']) start_pos = get_position(self.__dict__['start_pos']) delta = get_position_delta(current_pos, start_pos) if delta != 'NO_POS': - self.states.__dict__[key] = (delta < Checks.start_pos_delta_max) + self.states.__dict__[key] = (delta < start_pos_delta_max) except KeyError: # No check present for that col pass else: # update selfchecked and takeoff_ready self.states.__dict__["selfchecked"] = all( - [self.states[i] for i in Checks.all_checks.keys()] + [self.states[i] for i in ModelChecks.checks_dict.keys()] ) self.states.__dict__["takeoff_ready"] = all( - [self.states[i] for i in Checks.takeoff_checklist] + [self.states[i] for i in ModelChecks.takeoff_checklist] ) -def get_position(pos_string): - pos = [] - pos_str = pos_string.split(' ') - if pos_str[0] != 'nan' and pos_str[0] != 'NO_POS': +def get_position(pos_array): + if pos_array[0] != 'nan' and pos_array != 'NO_POS': + pos = [] for i in range(3): - pos.append(float(pos_str[i])) + pos.append(pos_array[i]) else: pos = 'NO_POS' return pos @@ -91,27 +180,120 @@ def get_position_delta(pos1, pos2): return 'NO_POS' -class Checks: - all_checks = {} - takeoff_checklist = (3, 4, 6, 7, 8) - battery_min = config.getfloat('CHECKS', 'battery_percentage_min') - start_pos_delta_max = config.getfloat('CHECKS', 'start_pos_delta_max') - time_delta_max = config.getfloat('CHECKS', 'time_delta_max') +class ModelFormatter: + view_formatters = {} + place_formatters = {} + VIEW_FORMATTER = False + PLACE_FORMATTER = True + + @classmethod + def format_view(cls, col, value): + if col in cls.view_formatters: + return cls.view_formatters[col](value) + return value + + @classmethod + def format_place(cls, col, value): + if col in cls.place_formatters: + return cls.place_formatters[col](value) + return value + + @classmethod + def col_format(cls, col, format_type): + def inner(f): + if format_type: + cls.place_formatters[col] = f + else: + cls.view_formatters[col] = f + + def wrapper(*args, **kwargs): + return f(*args, **kwargs) + + return wrapper + + return inner + + +@ModelFormatter.col_format(0, ModelFormatter.PLACE_FORMATTER) +def place_id(value): + value = value.stip() + # check user hostname spelling http://man7.org/linux/man-pages/man7/hostname.7.html + # '-' (hyphen) not first; latin letters/numbers/hyphens; length form 1 to 63 + # or matches command pattern + if re.match("^(?!-)[A-Za-z0-9-]{1,63}$", value) or re.match("^/[A-Za-z0-9]*$", value): + return value + else: + msgbox = QtWidgets.QMessageBox() + msgbox.setWindowTitle("Wrong input for the copter name!") + msgbox.setIcon(QtWidgets.QMessageBox.Critical) + msgbox.setText( + "Wrong input for the copter name!\n" + "Please use only A-Z, a-z, 0-9, and '-' chars.\n" + "Don't use '-' as first char.") + msgbox.exec_() + return None + + +@ModelFormatter.col_format(3, ModelFormatter.PLACE_FORMATTER) +def place_battery(value): + if isinstance(value, list): + battery_v, battery_p = value + if math.isnan(battery_v) or math.isnan(battery_p): + return "NO_INFO" + return value + + +@ModelFormatter.col_format(3, ModelFormatter.VIEW_FORMATTER) +def view_battery(value): + if isinstance(value, list): + battery_v, battery_p = value + return "{:.1f}V {:d}%".format(battery_v, int(battery_p*100)) + return value + +@ModelFormatter.col_format(7, ModelFormatter.VIEW_FORMATTER) +def view_selfcheck(value): + if isinstance(value, list): + return "ERROR" + return value + +@ModelFormatter.col_format(8, ModelFormatter.VIEW_FORMATTER) +def view_selfcheck(value): + if isinstance(value, list): + x, y, z, yaw, frame = value + return "{:.2f} {:.2f} {:.2f} {:d} {}".format(x, y, z, int(yaw), frame) + return value + +@ModelFormatter.col_format(9, ModelFormatter.VIEW_FORMATTER) +def view_selfcheck(value): + if isinstance(value, list): + x, y, z = value + return "{:.2f} {:.2f} {:.2f}".format(x, y, z) + return value + +@ModelFormatter.col_format(10, ModelFormatter.PLACE_FORMATTER) +def place_time_delta(value): + return abs(value - time.time()) + + +@ModelFormatter.col_format(10, ModelFormatter.VIEW_FORMATTER) +def view_time_delta(value): + return "{:.3f}".format(value) class CopterDataModel(QtCore.QAbstractTableModel): selected_ready_signal = QtCore.pyqtSignal(bool) selected_takeoff_ready_signal = QtCore.pyqtSignal(bool) - selected_flip_ready_signal = QtCore.pyqtSignal(bool) + selected_flip_ready_signal = QtCore.pyqtSignal(bool) # TODO fix this signals selected_calibrating_signal = QtCore.pyqtSignal(bool) selected_calibration_ready_signal = QtCore.pyqtSignal(bool) - def __init__(self, parent=None): + def __init__(self, checks=ModelChecks, formatter=ModelFormatter, parent=None): super(CopterDataModel, self).__init__(parent) self.headers = ('copter ID', 'version', ' animation ID ', ' battery ', ' system ', 'sensors', ' mode ', 'checks', 'current x y z yaw frame_id', ' start x y z ', 'dt') self.data_contents = [] - self.on_id_changed = None + self.checks = checks + self.formatter = formatter self.first_col_is_checked = False @@ -144,15 +326,15 @@ class CopterDataModel(QtCore.QAbstractTableModel): def flip_ready(self, contents=()): contents = contents or self.data_contents - return filter(lambda x: flip_checks(x), contents) # possibly change as takeoff checks + return filter(flip_checks, contents) # possibly change as takeoff checks def calibrating(self, contents=()): contents = contents or self.data_contents - return filter(lambda x: calibrating_check(x), contents) + return filter(calibrating_check, contents) def calibration_ready(self, contents=()): contents = contents or self.data_contents - return filter(lambda x: calibration_ready_check(x), contents) + return filter(calibration_ready_check, contents) def get_row_index(self, row_data): try: @@ -186,7 +368,7 @@ class CopterDataModel(QtCore.QAbstractTableModel): col = index.column() if role == Qt.DisplayRole or role == Qt.EditRole: # Separate editRole in case of editing non-text item = self.data_contents[row][col] - return str(item) if item is not None else "" + return str(self.formatter.format_view(col, item)) if item is not None else "" elif role == ModelDataRole: return self.data_contents[row][col] @@ -208,7 +390,7 @@ class CopterDataModel(QtCore.QAbstractTableModel): return self.data_contents[row].states.checked if role == QtCore.Qt.TextAlignmentRole and col != 0: - return QtCore.Qt.AlignHCenter | QtCore.Qt.AlignVCenter + return QtCore.Qt.AlignHCenter | QtCore.Qt.AlignVCenter def update_model(self, index=QtCore.QModelIndex(), role=QtCore.Qt.EditRole): selected = set(self.user_selected()) @@ -232,19 +414,14 @@ class CopterDataModel(QtCore.QAbstractTableModel): if role == Qt.CheckStateRole: self.data_contents[row].states.checked = value - elif role == Qt.EditRole: # For user actions with data - if col == 0: - # check user hostname spelling http://man7.org/linux/man-pages/man7/hostname.7.html - if value[0] != '-' and len(value) <= 63 and re.match("^[A-Za-z0-9-]*$", value): - self.data_contents[row].client.send_message("id", {"new_id": value}) + elif role == Qt.EditRole: # For user/outer actions with data, place modifiers applied + formatted_value = self.formatter.format_place(col, value) + if formatted_value is not None: # todo use new := syntax + self.data_contents[row][col] = formatted_value + + if col == 0: + self.data_contents[row].client.send_message("id", {"new_id": formatted_value}) self.data_contents[row].client.remove() - else: - msg = QtWidgets.QMessageBox() - msg.setIcon(QtWidgets.QMessageBox.Critical) - msg.setText("Wrong input for the copter name!\nPlease use only A-Z, a-z, 0-9, and '-' chars.\nDon't use '-' as first char.") - msg.exec_() - else: - self.data_contents[row][col] = value elif role == ModelDataRole: # For inner setting\editing of data self.data_contents[row][col] = value @@ -256,7 +433,7 @@ class CopterDataModel(QtCore.QAbstractTableModel): self.update_model(index, role) return True - def select_all(self): + def select_all(self): # probably NOT thread-safe! self.first_col_is_checked = not self.first_col_is_checked for row_num, copter in enumerate(self.data_contents): copter.states.checked = int(self.first_col_is_checked)*2 @@ -276,109 +453,23 @@ class CopterDataModel(QtCore.QAbstractTableModel): def add_client(self, client): self.insertRows([client]) - @QtCore.pyqtSlot(int) - def remove_client(self, row): + @QtCore.pyqtSlot(int) # Probably deprecated now + def remove_row(self, row): self.removeRows(row) - -def col_check(col): - def inner(f): - Checks.all_checks[col] = f - - def wrapper(*args, **kwargs): - return f(*args, **kwargs) - - return wrapper - - return inner - - -@col_check(1) -def check_ver(item): - if not item: - return None - return True - -@col_check(2) -def check_anim(item): - if not item: - return None - return str(item) != 'No animation' - -@col_check(3) -def check_bat(item): - if not item: - return None - if item == "NO_INFO": - return False - else: - return float(item.split(' ')[1][:-1]) > Checks.battery_min - -@col_check(4) -def check_sys_status(item): - if not item: - return None - return item == "STANDBY" - -@col_check(5) -def check_cal_status(item): - if not item: - return None - return item == "OK" - -@col_check(6) -def check_mode(item): - if not item: - return None - return (item != "NO_FCU") and not ("CMODE" in item) - -@col_check(7) -def check_selfcheck(item): - if not item: - return None - return item == "OK" - -@col_check(8) -def check_pos_status(item): - if not item: - return None - str_pos = item.split(' ') - return str_pos[0] != 'nan' and str_pos[0] != 'NO_POS' - -@col_check(9) -def check_start_pos_status(item): - if not item: - return None - str_start_pos = item.split(' ') - return str_start_pos[0] != 'nan' and str_start_pos[0] != 'NO_POS' - -@col_check(10) -def check_time_delta(item): - if not item: - return None - return abs(float(item)) < Checks.time_delta_max - - -def all_checks(copter_item): - for col, check in Checks.all_checks.items(): - if not check(copter_item[col]): - return False - return True - -def takeoff_checks(copter_item): - for col in Checks.takeoff_checklist: - if not Checks.all_checks[col](copter_item[col]): - return False - return True + @QtCore.pyqtSlot(object) + def remove_row_data(self, data): + row = self.get_row_index(data) + if row is not None: + self.removeRows(row) def flip_checks(copter_item): - for col in Checks.takeoff_checklist: + for col in ModelChecks.takeoff_checklist: if col != 4 or col != 7: - if not Checks.all_checks[col](copter_item[col]): - return False - else: - if copter_item[4] != "ACTIVE": + if not ModelChecks.checks_dict[col](copter_item[col]): return False + elif copter_item[4] != "ACTIVE": + return False return True @@ -387,7 +478,7 @@ def calibrating_check(copter_item): def calibration_ready_check(copter_item): - if not Checks.all_checks[4](copter_item[4]): + if not ModelChecks.checks_dict[4](copter_item[4]): return False return not calibrating_check(copter_item) @@ -414,7 +505,9 @@ class CopterProxyModel(QtCore.QSortFilterProxyModel): class SignalManager(QtCore.QObject): update_data_signal = QtCore.pyqtSignal(int, int, QtCore.QVariant, QtCore.QVariant) add_client_signal = QtCore.pyqtSignal(object) - remove_client_signal = QtCore.pyqtSignal(int) + remove_row_signal = QtCore.pyqtSignal(int) + remove_client_signal = QtCore.pyqtSignal(object) + if __name__ == '__main__': diff --git a/Server/server.py b/Server/server.py index 82157d9..b3b958e 100644 --- a/Server/server.py +++ b/Server/server.py @@ -146,12 +146,13 @@ class Server(messaging.Singleton): # noinspection PyArgumentList def _client_processor(self): logging.info("Client processor (selector) thread started!") + + messaging.NotifierSock().init(self.sel) + self.server_socket.listen() self.server_socket.setblocking(False) self.sel.register(self.server_socket, selectors.EVENT_READ, data=None) #| selectors.EVENT_WRITE - messaging.NotifierSock().bind((self.ip, self.port)) - while self.client_processor_thread_running.is_set(): events = self.sel.select() #logging.error('tick') @@ -177,15 +178,12 @@ class Server(messaging.Singleton): logging.info("Got connection from: {}".format(str(addr))) conn.setblocking(False) - if addr[0] == self.ip and messaging.NotifierSock().addr is None: - client = messaging.NotifierSock() - logging.info("Notifier sock client") - - elif 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: client = Client.clients[addr[0]] + client.close(True) # to ensure in unregistering logging.info("Reconnected client") self.sel.register(conn, selectors.EVENT_READ, data=client) client.connect(self.sel, conn, addr) @@ -284,7 +282,7 @@ class Client(messaging.ConnectionManager): @staticmethod def get_by_id(copter_id): - for client in Client.clients.values(): + for client in Client.clients.values(): # TODO filter if client.copter_id == copter_id: return client @@ -303,10 +301,12 @@ class Client(messaging.ConnectionManager): if self.on_connect: self.on_connect(self) - def _got_id(self, value): + def _got_id(self, _client, value): logging.info("Got copter id: {} for client {}".format(value, self.addr)) + old_id = self.copter_id self.copter_id = value - if self.on_first_connect: + + if old_id is None and self.on_first_connect: self.on_first_connect(self) def close(self, inner=False): @@ -325,11 +325,12 @@ class Client(messaging.ConnectionManager): def remove(self): if self.connected: self.close() - if self.clients: - try: - self.clients.pop(self.addr[0]) - except Exception as e: - logging.error(e) + + try: + self.clients.pop(self.addr[0]) + except KeyError as e: + logging.error(e) + logging.info("Client {} successfully removed!".format(self.copter_id)) @requires_connect diff --git a/Server/server_qt.py b/Server/server_qt.py index 48aff7f..ef78935 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -37,7 +37,6 @@ def wait(end, interrupter=threading.Event(), maxsleep=0.1): def confirmation_required(text="Are you sure?", label="Confirm operation?"): def inner(f): - @functools.wraps(f) def wrapper(*args, **kwargs): reply = QMessageBox.question( @@ -55,6 +54,7 @@ def confirmation_required(text="Are you sure?", label="Confirm operation?"): return inner + # noinspection PyArgumentList,PyCallByClass class MainWindow(QtWidgets.QMainWindow): def __init__(self): @@ -70,9 +70,9 @@ class MainWindow(QtWidgets.QMainWindow): self.player = QtMultimedia.QMediaPlayer() self.init_model() - + self.show() - + def init_model(self): # self.model.on_id_changed = self.set_copter_id @@ -88,7 +88,8 @@ class MainWindow(QtWidgets.QMainWindow): # 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) - self.signals.remove_client_signal.connect(self.model.remove_client) + self.signals.remove_row_signal.connect(self.model.remove_row) + self.signals.remove_client_signal.connect(self.model.remove_row_data) # Connect model signals to UI self.model.selected_ready_signal.connect(self.ui.start_button.setEnabled) @@ -110,20 +111,26 @@ class MainWindow(QtWidgets.QMainWindow): self.ui.action_select_all_rows.triggered.connect(self.model.select_all) def new_client_connected(self, client: Client): + logging.debug("Added client {}".format(client)) self.signals.add_client_signal.emit(StatedCopterData(copter_id=client.copter_id, client=client)) def client_connection_changed(self, client: Client): - logging.debug("Start remove {}".format(client.copter_id)) + logging.debug("Connection {} changed {}".format(client, client.connected), ) row_data = self.model.get_row_by_attr("client", client) - row_num = self.model.get_row_index(row_data) - logging.debug("Removing {}".format(client.copter_id)) - if row_num is not None: - if Server().remove_disconnected and (not client.connected): - client.remove() - self.signals.remove_client_signal.emit(row_num) - else: + + if row_data is None: + logging.error("No row for client presented") + return + + if Server().remove_disconnected and (not client.connected): + client.remove() + self.signals.remove_client_signal.emit(row_data) + logging.debug("Removing from table") + else: + row_num = self.model.get_row_index(row_data) + if row_num is not None: self.signals.update_data_signal.emit(row_num, 0, client.connected, ModelStateRole) - logging.debug("{} removed".format(client.copter_id)) + logging.debug("DATA: connected") def init_ui(self): # Connecting @@ -139,7 +146,7 @@ class MainWindow(QtWidgets.QMainWindow): self.ui.leds_button.clicked.connect(self.test_leds_selected) self.ui.takeoff_button.clicked.connect(self.takeoff_selected) self.ui.flip_button.clicked.connect(self.flip_selected) - self.ui.land_button.clicked.connect(self.land_selected) + self.ui.land_button.clicked.connect(self.land_selected) self.ui.reboot_fcu.clicked.connect(self.reboot_selected) self.ui.calibrate_gyro.clicked.connect(self.calibrate_gyro_selected) @@ -179,43 +186,36 @@ class MainWindow(QtWidgets.QMainWindow): client = copter_data_row.client client.get_response("telemetry", self.update_table_data) - @pyqtSlot(str) - def update_table_data(self, message): - fields = message.split('`') - # copter_id git_version animation_id battery_v battery_p system_status calibration_status mode selfcheck current_position start_position copter_time - copter_id = fields[0] - git_version = fields[1] - animation_id = fields[2] - battery_v = fields[3] - battery_p = fields[4] - if battery_v == 'nan' or battery_p == 'nan': - battery_info = "NO_INFO" - else: - battery_info = "{}V {}%".format(battery_v, battery_p) - sys_status = fields[5] - cal_status = fields[6] - mode = fields[7] - selfcheck = fields[8] - current_pos = fields[9] - start_pos = fields[10] - copter_time = fields[11] - time_delta = "{}".format(round(float(copter_time) - time.time(), 3)) - row = self.model.get_row_index(self.model.get_row_by_attr('copter_id', copter_id)) - self.signals.update_data_signal.emit(row, 1, git_version, ModelDataRole) - self.signals.update_data_signal.emit(row, 2, animation_id, ModelDataRole) - self.signals.update_data_signal.emit(row, 3, battery_info, ModelDataRole) - self.signals.update_data_signal.emit(row, 4, sys_status, ModelDataRole) - self.signals.update_data_signal.emit(row, 5, cal_status, ModelDataRole) - self.signals.update_data_signal.emit(row, 6, mode, ModelDataRole) - self.signals.update_data_signal.emit(row, 7, selfcheck, ModelDataRole) - self.signals.update_data_signal.emit(row, 8, current_pos, ModelDataRole) - self.signals.update_data_signal.emit(row, 9, start_pos, ModelDataRole) - self.signals.update_data_signal.emit(row, 10, time_delta, ModelDataRole) + @pyqtSlot(object, dict) + def update_table_data(self, client, telems: dict): + cols_dict = { + "git_version": 1, + "animation_id": 2, + "battery": 3, + "system_status": 4, + "calibration_status": 5, + "mode": 6, + "selfcheck": 7, + "current_position": 8, + "start_position": 9, + "time": 10, + } + + for key, value in telems.items(): + col = cols_dict.get(key, None) + if col is None: + logging.error("No column {} present!".format(key)) + continue + + 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: + self.signals.update_data_signal.emit(row_num, col, value, Qt.EditRole) @pyqtSlot(QtCore.QModelIndex) def selfcheck_info_dialog(self, index): col = index.column() - if col == 6: + if col == 7: data = self.proxy_model.data(index, role=ModelDataRole) if data and data != "OK": dialog = QMessageBox() @@ -226,7 +226,7 @@ class MainWindow(QtWidgets.QMainWindow): dialog.setDetailedText("\n".join(data)) dialog.exec() - def _selfcheck_shortener(self, data): + def _selfcheck_shortener(self, data): # TODO!!! shortened = [] for line in data: if len(line) > 89: @@ -236,16 +236,11 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def remove_selected(self): for copter in self.model.user_selected(): - row_num = self.model.get_row_index(copter) - if row_num is not None: - copter.client.remove() + copter.client.remove() - if not Server().remove_disconnected: - self.signals.remove_client_signal.emit(row_num) - - logging.info("Client removed from table!") - else: - logging.error("Client is not in table!") + if not Server().remove_disconnected: + self.signals.remove_client_signal.emit(copter) + logging.info("Client removed from table!") @pyqtSlot() @confirmation_required("This operation will takeoff selected copters with delay and start animation. Proceed?") @@ -255,12 +250,12 @@ class MainWindow(QtWidgets.QMainWindow): logging.info('Wait {} seconds to start animation'.format(dt)) if self.ui.music_checkbox.isChecked(): music_dt = self.ui.music_delay_spin.value() - asyncio.ensure_future(self.play_music_at_time(music_dt+time_now), loop=loop) + asyncio.ensure_future(self.play_music_at_time(music_dt + time_now), loop=loop) logging.info('Wait {} seconds to play music'.format(music_dt)) # self.selfcheck_selected() for copter in self.model.user_selected(): - if all_checks(copter): - server.send_starttime(copter.client, dt+time_now) + if self.model.checks.all_checks(copter): + server.send_starttime(copter.client, dt + time_now) @pyqtSlot() def pause_resume_selected(self): @@ -290,7 +285,7 @@ class MainWindow(QtWidgets.QMainWindow): def test_leds_selected(self): for copter in self.model.user_selected(): copter.client.send_message("led_test") - + @pyqtSlot() def disarm_all(self): Client.broadcast_message("disarm") @@ -299,9 +294,9 @@ class MainWindow(QtWidgets.QMainWindow): @confirmation_required("This operation will takeoff copters immediately. Proceed?") def takeoff_selected(self, **kwargs): for copter in self.model.user_selected(): - if takeoff_checks(copter): + if self.model.checks.takeoff_checks(copter): if self.ui.z_checkbox.isChecked(): - copter.client.send_message("takeoff_z", {"z":str(self.ui.z_spin.value())}) + copter.client.send_message("takeoff_z", {"z": str(self.ui.z_spin.value())}) # todo int else: copter.client.send_message("takeoff") @@ -320,7 +315,7 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def reboot_selected(self): for copter in self.model.user_selected(): - copter.client.send_message("reboot_fcu") + copter.client.send_message("reboot_fcu") @pyqtSlot() def calibrate_gyro_selected(self): @@ -332,7 +327,7 @@ class MainWindow(QtWidgets.QMainWindow): data = 'CALIBRATING' self.signals.update_data_signal.emit(row, col, data, ModelDataRole) # Send request - client.get_response("calibrate_gyro", self._get_calibration_info, callback_args=(copter_data_row, )) + client.get_response("calibrate_gyro", self._get_calibration_info) @pyqtSlot() def calibrate_level_selected(self): @@ -344,13 +339,15 @@ class MainWindow(QtWidgets.QMainWindow): data = 'CALIBRATING' self.signals.update_data_signal.emit(row, col, data, ModelDataRole) # Send request - client.get_response("calibrate_level", self._get_calibration_info, callback_args=(copter_data_row, )) + client.get_response("calibrate_level", self._get_calibration_info) - def _get_calibration_info(self, value, copter_data_row): + def _get_calibration_info(self, client, value): col = 5 - row = self.model.get_row_index(copter_data_row) - data = str(value) - self.signals.update_data_signal.emit(row, col, data, ModelDataRole) + row_data = self.model.get_row_by_attr("client", client) + row = self.model.get_row_index(row_data) + if row is not None: + data = str(value) + self.signals.update_data_signal.emit(row, col, data, ModelDataRole) @pyqtSlot() def send_animations(self): @@ -379,7 +376,8 @@ class MainWindow(QtWidgets.QMainWindow): for file, name in zip(files, names): for copter in self.model.user_selected(): if name == copter.copter_id: - copter.client.send_file(file, "/home/pi/catkin_ws/src/clever/clever/camera_info/calibration.yaml") + copter.client.send_file(file, + "/home/pi/catkin_ws/src/clever/clever/camera_info/calibration.yaml") else: logging.info("Filename has no matches with any drone selected") @@ -402,7 +400,8 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def send_aruco(self): - path = QFileDialog.getOpenFileName(self, "Select aruco map configuration file", filter="Aruco map files (*.txt)")[0] + path = \ + QFileDialog.getOpenFileName(self, "Select aruco map configuration file", filter="Aruco map files (*.txt)")[0] if path: filename = os.path.basename(path) print("Selected file:", path, filename) @@ -455,7 +454,7 @@ class MainWindow(QtWidgets.QMainWindow): def restart_clever(self): for copter in self.model.user_selected(): copter.client.send_message("service_restart", {"name": "clever"}) - + @pyqtSlot() def restart_clever_show(self): for copter in self.model.user_selected(): @@ -464,12 +463,12 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def update_client_repo(self): for copter in self.model.user_selected(): - copter.client.send_message("update_repo") + copter.client.send_message("update_repo") @pyqtSlot() def reboot_all_on_selected(self): for copter in self.model.user_selected(): - copter.client.send_message("reboot_all") + copter.client.send_message("reboot_all") @pyqtSlot() def update_start_to_current_position(self): @@ -501,7 +500,7 @@ class MainWindow(QtWidgets.QMainWindow): path = QFileDialog.getOpenFileName(self, "Select music file", filter="Music files (*.mp3 *.wav)")[0] if path: media = QUrl.fromLocalFile(path) - content = QtMultimedia.QMediaContent(media) + content = QtMultimedia.QMediaContent(media) self.player.setMedia(content) self.ui.action_select_music_file.setText(self.ui.action_select_music_file.text() + " (selected)") @@ -513,9 +512,9 @@ class MainWindow(QtWidgets.QMainWindow): if self.player.mediaStatus() == QtMultimedia.QMediaPlayer.NoMedia: logging.info("No media file") return - + if self.player.state() == QtMultimedia.QMediaPlayer.StoppedState or \ - self.player.state() == QtMultimedia.QMediaPlayer.PausedState: + self.player.state() == QtMultimedia.QMediaPlayer.PausedState: self.ui.action_play_music.setText("Pause music") self.player.play() else: @@ -552,16 +551,16 @@ class MainWindow(QtWidgets.QMainWindow): result = -1 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)) + 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): - self.model.data_contents[row_num].client\ + 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): self.model.data_contents[row_num].client \ .send_message("led_fill", {"red": 255}) - Dialog = QtWidgets.QDialog() + Dialog = QtWidgets.QDialog() ui = Ui_Dialog() ui.setupUi(Dialog) Dialog.show() @@ -574,7 +573,7 @@ class MainWindow(QtWidgets.QMainWindow): self.model.data_contents[row_num].client \ .send_message("led_fill") client_row_max = client_row_mid - 1 - + elif result == 2: for row_num in range(client_row_min, client_row_mid): self.model.data_contents[row_num].client \ @@ -592,18 +591,25 @@ class MainWindow(QtWidgets.QMainWindow): self.model.data_contents[row_num].client \ .send_message("disarm") -@messaging.message_callback("telem") -def get_telem_data(*args, **kwargs): - message = kwargs.get("message", None) - window.update_table_data(message) + +@messaging.message_callback("telemetry") +def get_telem_data(self, **kwargs): + message = kwargs.get("value") + window.update_table_data(self, message) + + +def except_hook(cls, exception, traceback): + sys.__excepthook__(cls, exception, traceback) if __name__ == "__main__": + sys.excepthook = except_hook # for debugging (exceptions traceback) + app = QtWidgets.QApplication(sys.argv) loop = QEventLoop(app) asyncio.set_event_loop(loop) - #app.exec_() + # app.exec_() with loop: window = MainWindow() diff --git a/messaging_lib.py b/messaging_lib.py index 9b1074b..855e1ef 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -17,12 +17,24 @@ try: except ImportError: import selectors2 as selectors + # import logging_lib -PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on", - "callback", "callback_args", "callback_kwargs", - "request_args", "resend", - ]) + +class Namespace: + def __init__(self, **kwargs): + self.__dict__.update(kwargs) + + def __getitem__(self, key): + return self.__dict__[key] + + def __setitem__(self, key, value): + self.__dict__[key] = value + + +class PendingRequest(Namespace): pass + + logger = logging.getLogger(__name__) @@ -239,10 +251,19 @@ class ConnectionManager(object): self.socket = client_socket self.addr = client_addr + self._clear() + self._set_selector_events_mask('r') if self.resend_requests: self._resend_requests() + def _clear(self): + if not self.resume_queue: # maybe needs locks + self._recv_buffer = b'' + self._send_buffer = b'' + self._received_queue.clear() + self._send_queue.clear() + def close(self): with self._close_lock: self._should_close = True @@ -253,11 +274,6 @@ class ConnectionManager(object): def _close(self): logger.info("Closing connection to {}".format(self.addr)) - if not self.resume_queue: - self._recv_buffer = b'' - self._send_buffer = b'' - self._received_queue.clear() # - try: logger.info("Unregistering selector of {}".format(self.addr)) self.selector.unregister(self.socket) @@ -281,6 +297,7 @@ class ConnectionManager(object): with self._close_lock: self._should_close = False + self._clear() logger.info("CLOSED connection to {}".format(self.addr)) def process_events(self, mask): @@ -379,7 +396,7 @@ class ConnectionManager(object): ) f = request.callback - f(value, *request.callback_args, **request.callback_kwargs) + f(self, value, *request.callback_args, **request.callback_kwargs) else: logger.warning("Unexpected response!") @@ -417,9 +434,6 @@ class ConnectionManager(object): logger.warning( "Attempt to send message {} to {} failed due error: {}".format(self._send_buffer, self.addr, error)) - if not self.resume_queue: - self._send_buffer = b'' - raise error else: logger.debug("Sent {} to {}".format(self._send_buffer[:sent], self.addr)) @@ -456,14 +470,12 @@ class ConnectionManager(object): def _resend_requests(self): with self._request_lock: - for request_id, request in self._request_queue.items(): + for request_id, request in self._request_queue.items(): #TODO filter if request.resend: self._send(MessageManager.create_request( request.requested_value, request_id, request.request_args.update(resend=request.resend)) ) - #request.resend = False - - # self._request_queue.clear() + request.resend = False def send_message(self, command, args=None): self._send(MessageManager.create_simple_message(command, args)) @@ -484,41 +496,45 @@ class ConnectionManager(object): )) -class NotifierSock(Singleton): #TODO remake as connecting ONLY to self socket and selector +class NotifierSock(Singleton): def __init__(self): - self.receive_socket = None - self.addr = None + self._server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self._server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1) + self._server_socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) - self._notify_socket = None - self._notify_lock = threading.Lock() + self._sending_sock = socket.socket() + self._send_lock = threading.Lock() - def bind(self, server_addr): - self._notify_socket = socket.socket() - self._notify_socket.connect(server_addr) - logger.info("Notify socket: bind") + self._receiving_sock = None - def connect(self, _, client_socket, client_addr): - self.receive_socket = client_socket - self.addr = client_addr + def init(self, selector, port=26000): + port += random.randint(0, 100) # local testing fix + self._server_socket.bind(('', port)) + self._server_socket.listen(1) + self._sending_sock.connect(('127.0.0.1', port)) + self._receiving_sock, _ = self._server_socket.accept() logger.info("Notify socket: connected") + selector.register(self._receiving_sock, selectors.EVENT_READ, data=self) + logger.info("Notify socket: selector registered") + + def get_sock(self): + return self._receiving_sock + def notify(self): - with self._notify_lock: - if self.addr is not None: - self._notify_socket.sendall(bytes(1)) + with self._send_lock: + if self._receiving_sock is not None: + self._sending_sock.sendall(bytes(1)) logger.debug("Notify socket: notified") def process_events(self, mask): - if mask & selectors.EVENT_READ: + if mask & selectors.EVENT_READ and self._receiving_sock is not None: try: - data = self.receive_socket.recv(1024) - except Exception: # TODO remove + self._receiving_sock.recv(1024) + logger.debug("Notify socket: received") + except io.BlockingIOError: pass - else: - if data: - logger.debug("Notifier received {} from {}".format(data, self.addr)) - else: - self.addr = None - logger.warning("Notifier: connection to {} lost!".format(self.addr)) - + except Exception as e: + print(e) \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index 728753f..a196866 100644 --- a/requirements.txt +++ b/requirements.txt @@ -3,3 +3,4 @@ numpy==1.16.4 PyQt5==5.13.0 PyQt5-sip==4.19.18 selectors2==2.0.1 +Quamash==0.6.1 \ No newline at end of file