CopterExpress/clever-show
1
0
Fork 0
mirror of https://github.com/CopterExpress/clever-show.git synced 2026-05-26 07:07:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Feature branch: IMPORTANT connection+telemetry+table fixes and improvements (#55)

Browse Source 
* .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 6122352380.

* 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
This commit is contained in:
artem30801
2019-12-05 15:10:21 +03:00
committed by Arthur Golubtsov
parent 53dad0e3fd
commit ce36c6f1e3
9 changed files with 635 additions and 425 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
Drone/client.py
View File

@@ -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

1
Drone/client_config.ini
View File

@@ -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

363
Drone/copter_client.py
View File

@@ -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)

4
Drone/mavros_mavlink.py
View File

@@ -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

371
Server/copter_table_models.py
View File

@@ -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__':

31
Server/server.py
View File

@@ -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

180
Server/server_qt.py
View File

@@ -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()

100
messaging_lib.py
View File

@@ -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)

1
requirements.txt
View File

@@ -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