From eb61fd0dd821bff1d138703a17bcec9678c49aa1 Mon Sep 17 00:00:00 2001
From: artem30801 <38689676+artem30801@users.noreply.github.com>
Date: Fri, 18 Oct 2019 22:52:25 +0300
Subject: [PATCH 01/31] Test and merge feature branch (#52)
* Added indication of connected/disconnected copters
* Added client removing functionality
* Option for automatically remove disconnected copters from table
* Renaming copters from QT server table on the go + some improvements
---
Drone/FlightLib/FlightLib.py | 2 +-
Drone/client.py | 46 ++++--
Drone/client_config.ini | 1 +
Drone/copter_client.py | 167 +++++++++++++++++----
Server/copter_table_models.py | 226 ++++++++++++++++++++++-------
Server/server.py | 53 ++++---
Server/server_config.ini | 1 +
Server/server_gui.py | 6 +-
Server/server_gui.ui | 8 +-
Server/server_qt.py | 165 ++++++++++++++-------
builder/assets/clever-show.service | 4 +-
messaging_lib.py | 41 +++++-
12 files changed, 533 insertions(+), 187 deletions(-)
diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py
index b7dae37..deae299 100644
--- a/Drone/FlightLib/FlightLib.py
+++ b/Drone/FlightLib/FlightLib.py
@@ -69,7 +69,7 @@ def check(check_name):
failures = f(*args, **kwargs)
msgs = []
for failure in failures:
- msg = "[{}]: Failure: {}".format(check_name, failure)
+ msg = "[{}]: Err: {}".format(check_name, failure)
msgs.append(msg)
logger.warning(msg)
diff --git a/Drone/client.py b/Drone/client.py
index 2b6664b..aed8dc4 100644
--- a/Drone/client.py
+++ b/Drone/client.py
@@ -47,6 +47,8 @@ class Client(object):
global active_client
active_client = self
+ # self._last_ping_time = 0
+
def load_config(self):
self.config.read(self.config_path)
@@ -58,14 +60,16 @@ class Client(object):
self.NTP_HOST = self.config.get('NTP', 'host')
self.NTP_PORT = self.config.getint('NTP', 'port')
- self.files_directory = self.config.get('FILETRANSFER', 'files_directory')
+ self.files_directory = self.config.get('FILETRANSFER', 'files_directory') # not used?!
self.client_id = self.config.get('PRIVATE', 'id')
- if self.client_id == 'default':
+ if self.client_id == '/default':
self.client_id = 'copter' + str(random.randrange(9999)).zfill(4)
- self.write_config(False, 'PRIVATE', 'id', self.client_id)
+ self.write_config(False, ConfigOption('PRIVATE', 'id', self.client_id))
elif self.client_id == '/hostname':
self.client_id = socket.gethostname()
+ elif self.client_id == '/ip':
+ self.client_id = messaging.get_ip_address()
def rewrite_config(self):
with open(self.config_path, 'w') as file:
@@ -103,7 +107,7 @@ class Client(object):
try:
while True:
self._reconnect()
- #self._process_connections()
+ self._process_connections()
except (KeyboardInterrupt, ):
logger.critical("Caught interrupt, exiting!")
@@ -117,6 +121,8 @@ class Client(object):
try:
self.client_socket = socket.socket()
self.client_socket.settimeout(timeout)
+ self.client_socket.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
+ self.client_socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
self.client_socket.connect((self.server_host, self.server_port))
except socket.error as error:
if isinstance(error, OSError):
@@ -138,15 +144,12 @@ class Client(object):
self.broadcast_bind(timeout*2, attempt_limit)
attempt_count = 0
-
def _connect(self):
self.connected = True
self.client_socket.setblocking(False)
events = selectors.EVENT_READ # | selectors.EVENT_WRITE
self.selector.register(self.client_socket, events, data=self.server_connection)
self.server_connection.connect(self.selector, self.client_socket, (self.server_host, self.server_port))
- self._process_connections()
-
def broadcast_bind(self, timeout=3.0, attempt_limit=5):
broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
@@ -187,6 +190,9 @@ class Client(object):
def _process_connections(self):
while True:
events = self.selector.select(timeout=1)
+ # if time.time() - self._last_ping_time > 5:
+ # 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
@@ -200,7 +206,7 @@ class Client(object):
logger.error(
"Exception {} occurred for {}! Resetting connection!".format(error, connection.addr)
)
- self.server_connection.close()
+ self.server_connection._close()
self.connected = False
if isinstance(error, OSError):
@@ -213,20 +219,28 @@ class Client(object):
return
-@messaging.request_callback("id")
-def _response_id():
- return active_client.client_id
-
-@messaging.request_callback("time")
-def _response_time():
- return active_client.time_now()
-
@messaging.message_callback("config_write")
def _command_config_write(*args, **kwargs):
options = [ConfigOption(**raw_option) for raw_option in kwargs["options"]]
logger.info("Writing config options: {}".format(options))
active_client.write_config(kwargs["reload"], *options)
+
+@messaging.request_callback("id")
+def _response_id(*args, **kwargs):
+ new_id = kwargs.get("new_id", None)
+ if new_id is not None:
+ cfg = ConfigOption("PRIVATE", "id", new_id)
+ active_client.write_config(True, cfg)
+
+ return active_client.client_id
+
+
+@messaging.request_callback("time")
+def _response_time(*args, **kwargs):
+ return active_client.time_now()
+
+
if __name__ == "__main__":
client = Client()
client.start()
diff --git a/Drone/client_config.ini b/Drone/client_config.ini
index 697fbfe..4fa87e1 100644
--- a/Drone/client_config.ini
+++ b/Drone/client_config.ini
@@ -41,6 +41,7 @@ yaw = -90
[PRIVATE]
id = /hostname
+restart_dhcpcd = True
use_leds = True
led_pin = 21
x0 = 0
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index 92cd73b..62d4a73 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -56,6 +56,8 @@ class CopterClient(client.Client):
self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds')
self.LED_PIN = self.config.getint('PRIVATE', 'led_pin')
+ self.RESTART_DHCPCD = self.config.getboolean('PRIVATE', 'restart_dhcpcd')
+
def on_broadcast_bind(self):
configure_chrony_ip(self.server_host)
restart_service("chrony")
@@ -97,6 +99,9 @@ class CopterClient(client.Client):
def restart_service(name):
os.system("systemctl restart {}".format(name))
+def execute_command(command):
+ os.system(command)
+
def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):
try:
with open(path, 'r') as f:
@@ -130,8 +135,114 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):
return True
+def configure_hostname(hostname):
+ path = "/etc/hostname"
+ try:
+ with open(path, 'r') as f:
+ raw_content = f.read()
+ except IOError as e:
+ print("Reading error {}".format(e))
+ return False
+
+ current_hostname = str(raw_content)
+
+ if current_hostname != hostname:
+ content = hostname + '\n'
+ try:
+ with open(path, 'w') as f:
+ f.write(content)
+ except IOError:
+ print("Error writing")
+ return False
+
+ return True
+
+
+def configure_hosts(hostname):
+ path = "/etc/hosts"
+ try:
+ with open(path, 'r') as f:
+ raw_content = f.read()
+ except IOError as e:
+ print("Reading error {}".format(e))
+ return False
+
+ index_start = raw_content.find("127.0.1.1", )
+ index_stop = raw_content.find("\n", index_start)
+
+ _ip, current_hostname = raw_content[index_start:index_stop].split()
+ if current_hostname != hostname:
+ content = raw_content[:index_start] + "{} {}".format(_ip, hostname) + raw_content[index_stop:]
+ try:
+ with open(path, 'w') as f:
+ f.write(content)
+ except IOError:
+ print("Error writing")
+ return False
+
+ return True
+
+def configure_motd(hostname):
+ with open("/etc/motd", "w") as f:
+ f.write("\r\n{}\r\n\r\n".format(hostname))
+
+def configure_bashrc(hostname):
+ path = "/home/pi/.bashrc"
+ try:
+ with open(path, 'r') as f:
+ raw_content = f.read()
+ except IOError as e:
+ print("Reading error {}".format(e))
+ return False
+
+ index_start = raw_content.find("ROS_HOSTNAME='", ) + 14
+ index_stop = raw_content.find("'", index_start)
+
+ current_hostname = raw_content[index_start:index_stop]
+ if current_hostname != hostname:
+ content = raw_content[:index_start] + hostname + raw_content[index_stop:]
+ try:
+ with open(path, 'w') as f:
+ f.write(content)
+ except IOError:
+ print("Error writing")
+ return False
+
+ return True
+
+@messaging.message_callback("execute")
+def _execute(*args, **kwargs):
+ command = kwargs.get("command", None)
+ if command:
+ execute_command(command)
+
+@messaging.message_callback("id")
+def _response_id(*args, **kwargs):
+ new_id = kwargs.get("new_id", None)
+ if new_id is not None:
+ old_id = client.active_client.client_id
+ if new_id != old_id:
+ cfg = client.ConfigOption("PRIVATE", "id", new_id)
+ client.active_client.write_config(True, cfg)
+ if new_id != '/hostname':
+ if client.active_client.RESTART_DHCPCD:
+ hostname = client.active_client.client_id
+ configure_hostname(hostname)
+ configure_hosts(hostname)
+ configure_bashrc(hostname)
+ configure_motd(hostname)
+ execute_command("reboot")
+ #execute_command("hostname {}".format(hostname))
+ #restart_service("dhcpcd")
+ #restart_service("avahi-daemon")
+ #restart_service("smbd")
+ #restart_service("roscore")
+ #restart_service("clever")
+ restart_service("clever-show")
+
+
@messaging.request_callback("selfcheck")
-def _response_selfcheck():
+def _response_selfcheck(*args, **kwargs):
if check_state_topic(wait_new_status=True):
check = FlightLib.selfcheck()
return check if check else "OK"
@@ -141,7 +252,7 @@ def _response_selfcheck():
@messaging.request_callback("anim_id")
-def _response_animation_id():
+def _response_animation_id(*args, **kwargs):
# Load animation
result = animation.get_id()
if result != 'No animation':
@@ -163,7 +274,7 @@ def _response_animation_id():
return result
@messaging.request_callback("batt_voltage")
-def _response_batt():
+def _response_batt(*args, **kwargs):
if check_state_topic(wait_new_status=True):
return FlightLib.get_telemetry('body').voltage
else:
@@ -172,7 +283,7 @@ def _response_batt():
@messaging.request_callback("cell_voltage")
-def _response_cell():
+def _response_cell(*args, **kwargs):
if check_state_topic(wait_new_status=True):
return FlightLib.get_telemetry('body').cell_voltage
else:
@@ -180,37 +291,37 @@ def _response_cell():
return float('nan')
@messaging.request_callback("sys_status")
-def _response_sys_status():
+def _response_sys_status(*args, **kwargs):
return get_sys_status()
@messaging.request_callback("cal_status")
-def _response_cal_status():
+def _response_cal_status(*args, **kwargs):
return get_calibration_status()
@messaging.request_callback("position")
-def _response_position():
+def _response_position(*args, **kwargs):
telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
return "{:.2f} {:.2f} {:.2f} {:.1f} {}".format(
telem.x, telem.y, telem.z, math.degrees(telem.yaw), client.active_client.FRAME_ID)
@messaging.request_callback("calibrate_gyro")
-def _calibrate_gyro():
+def _calibrate_gyro(*args, **kwargs):
calibrate('gyro')
return get_calibration_status()
@messaging.request_callback("calibrate_level")
-def _calibrate_level():
+def _calibrate_level(*args, **kwargs):
calibrate('level')
return get_calibration_status()
@messaging.message_callback("test")
-def _command_test(**kwargs):
+def _command_test(*args, **kwargs):
logger.info("logging info test")
print("stdout test")
@messaging.message_callback("move_start")
-def _command_move_start_to_current_position(**kwargs):
+def _command_move_start_to_current_position(*args, **kwargs):
# Load animation
frames = animation.load_animation(os.path.abspath("animation.csv"),
x0=client.active_client.X0_COMMON,
@@ -232,7 +343,7 @@ def _command_move_start_to_current_position(**kwargs):
print ("Start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
@messaging.message_callback("reset_start")
-def _command_reset_start(**kwargs):
+def _command_reset_start(*args, **kwargs):
client.active_client.config.set('PRIVATE', 'x0', 0)
client.active_client.config.set('PRIVATE', 'y0', 0)
client.active_client.rewrite_config()
@@ -240,7 +351,7 @@ def _command_reset_start(**kwargs):
print ("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
@messaging.message_callback("set_z_to_ground")
-def _command_set_z(**kwargs):
+def _command_set_z(*args, **kwargs):
telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
client.active_client.config.set('PRIVATE', 'z0', telem.z)
client.active_client.rewrite_config()
@@ -248,7 +359,7 @@ def _command_set_z(**kwargs):
print ("Set z offset to {:.2f}".format(client.active_client.Z0))
@messaging.message_callback("reset_z_offset")
-def _command_reset_z(**kwargs):
+def _command_reset_z(*args, **kwargs):
client.active_client.config.set('PRIVATE', 'z0', 0)
client.active_client.rewrite_config()
client.active_client.load_config()
@@ -256,36 +367,36 @@ def _command_reset_z(**kwargs):
@messaging.message_callback("update_repo")
-def _command_update_repo(**kwargs):
+def _command_update_repo(*args, **kwargs):
os.system("git reset --hard origin/master")
os.system("git fetch")
os.system("git pull")
os.system("chown -R pi:pi ~/CleverSwarm")
@messaging.message_callback("reboot_fcu")
-def _command_reboot():
+def _command_reboot(*args, **kwargs):
reboot_fcu()
@messaging.message_callback("service_restart")
-def _command_service_restart(**kwargs):
+def _command_service_restart(*args, **kwargs):
restart_service(kwargs["name"])
@messaging.message_callback("repair_chrony")
-def _command_chrony_repair():
+def _command_chrony_repair(*args, **kwargs):
configure_chrony_ip(client.active_client.server_host)
restart_service("chrony")
@messaging.message_callback("led_test")
-def _command_led_test(**kwargs):
+def _command_led_test(*args, **kwargs):
LedLib.chase(255, 255, 255)
time.sleep(2)
LedLib.off()
@messaging.message_callback("led_fill")
-def _command_led_fill(**kwargs):
+def _command_led_fill(*args, **kwargs):
r = kwargs.get("red", 0)
g = kwargs.get("green", 0)
b = kwargs.get("blue", 0)
@@ -294,11 +405,11 @@ def _command_led_fill(**kwargs):
@messaging.message_callback("flip")
-def _copter_flip():
+def _copter_flip(*args, **kwargs):
FlightLib.flip(frame_id=client.active_client.FRAME_ID)
@messaging.message_callback("takeoff")
-def _command_takeoff(**kwargs):
+def _command_takeoff(*args, **kwargs):
task_manager.add_task(time.time(), 0, animation.takeoff,
task_kwargs={
"z": client.active_client.TAKEOFF_HEIGHT,
@@ -310,7 +421,7 @@ def _command_takeoff(**kwargs):
@messaging.message_callback("land")
-def _command_land(**kwargs):
+def _command_land(*args, **kwargs):
task_manager.reset()
task_manager.add_task(0, 0, animation.land,
task_kwargs={
@@ -323,7 +434,7 @@ def _command_land(**kwargs):
@messaging.message_callback("disarm")
-def _command_disarm(**kwargs):
+def _command_disarm(*args, **kwargs):
task_manager.reset()
task_manager.add_task(-5, 0, FlightLib.arming_wrapper,
task_kwargs={
@@ -333,22 +444,22 @@ def _command_disarm(**kwargs):
@messaging.message_callback("stop")
-def _command_stop(**kwargs):
+def _command_stop(*args, **kwargs):
task_manager.reset()
@messaging.message_callback("pause")
-def _command_pause(**kwargs):
+def _command_pause(*args, **kwargs):
task_manager.pause()
@messaging.message_callback("resume")
-def _command_resume(**kwargs):
+def _command_resume(*args, **kwargs):
task_manager.resume(time_to_start_next_task=kwargs.get("time", 0))
@messaging.message_callback("start")
-def _play_animation(**kwargs):
+def _play_animation(*args, **kwargs):
start_time = float(kwargs["time"])
# Check if animation file is available
if animation.get_id() == 'No animation':
diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py
index b5f9e53..9b85d6b 100644
--- a/Server/copter_table_models.py
+++ b/Server/copter_table_models.py
@@ -1,32 +1,72 @@
import sys
import re
import collections
+import indexed
+from server import ConfigOption
from PyQt5 import QtCore, QtGui, QtWidgets
from PyQt5.QtCore import Qt as Qt
+ModelDataRole = 998
+ModelStateRole = 999
+
+
class CopterData:
- class_attrs = collections.OrderedDict([('copter_id', None), ('anim_id', None), ('batt_v', None), ('batt_p', None),
- ('sys_status', None), ('cal_status', None), ('selfcheck', None), ('position', None), ("time_delta", None),
- ("client", None), ("checked", 0)], )
+ class_basic_attrs = indexed.IndexedOrderedDict([('copter_id', None), ('anim_id', None),
+ ('batt_v', None), ('batt_p', None),
+ ('sys_status', None), ('cal_status', None), ('selfcheck', None),
+ ('position', None), ("time_delta", None),
+ ("client", None), ])
def __init__(self, **kwargs):
- self.attrs = self.class_attrs.copy()
- self.attrs.update(kwargs)
+ self.attrs_dict = self.class_basic_attrs.copy()
+ self.attrs_dict.update(kwargs)
- for attr, value in self.attrs.items():
+ for attr, value in self.attrs_dict.items():
setattr(self, attr, value)
def __getitem__(self, key):
- return getattr(self, list(self.attrs.keys())[key])
+ return getattr(self, self.attrs_dict.keys()[key])
def __setitem__(self, key, value):
- setattr(self, list(self.attrs.keys())[key], value)
+ setattr(self, self.attrs_dict.keys()[key], value)
+
+
+class StatedCopterData(CopterData):
+ class_basic_states = indexed.IndexedOrderedDict([("checked", 0), ("selfchecked", None), ("takeoff_ready", None),
+ ("copter_id", True), ])
+
+ def __init__(self, **kwargs):
+ self.states = CopterData(**self.class_basic_states)
+
+ super(StatedCopterData, self).__init__(**kwargs)
+
+ def __setattr__(self, key, value):
+ self.__dict__[key] = value
+
+ if key in self.class_basic_attrs.keys():
+ try:
+ self.states.__dict__[key] = \
+ Checks.all_checks[self.attrs_dict.keys().index(key)](value)
+ 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.__dict__["takeoff_ready"] = all(
+ [self.states[i] for i in Checks.takeoff_checklist]
+ )
+
+
+class Checks:
+ all_checks = {}
+ takeoff_checklist = (2, 3, 4, 5, 6)
class CopterDataModel(QtCore.QAbstractTableModel):
- checks = {}
selected_ready_signal = QtCore.pyqtSignal(bool)
selected_takeoff_ready_signal = QtCore.pyqtSignal(bool)
selected_flip_ready_signal = QtCore.pyqtSignal(bool)
@@ -35,8 +75,12 @@ class CopterDataModel(QtCore.QAbstractTableModel):
def __init__(self, parent=None):
super(CopterDataModel, self).__init__(parent)
- self.headers = ('copter ID', ' animation ID ', 'batt V', 'batt %', ' system ', 'calibration', 'selfcheck', 'current x y z yaw frame_id', 'time delta')
+ self.headers = ('copter ID', ' animation ID ', 'batt V', 'batt %', ' system ',
+ 'calibration', 'selfcheck', 'current x y z yaw frame_id', 'time delta')
self.data_contents = []
+
+ self.on_id_changed = None
+
self.first_col_is_checked = False
def insertRows(self, contents, position='last', parent=QtCore.QModelIndex()):
@@ -48,20 +92,28 @@ class CopterDataModel(QtCore.QAbstractTableModel):
self.endInsertRows()
- def user_selected(self):
- return filter(lambda x: x.checked == Qt.Checked, self.data_contents)
+ def removeRows(self, position, rows=1, index=QtCore.QModelIndex()):
+ self.beginRemoveRows(QtCore.QModelIndex(), position, position + rows - 1)
+ self.data_contents = self.data_contents[:position] + self.data_contents[position + rows:]
+ self.endRemoveRows()
+
+ return True
+
+ def user_selected(self, contents=()):
+ contents = contents or self.data_contents
+ return filter(lambda x: x.states.checked == Qt.Checked, contents)
def selfchecked_ready(self, contents=()):
contents = contents or self.data_contents
- return filter(lambda x: all_checks(x), contents)
+ return filter(lambda x: x.states.selfchecked, contents)
def takeoff_ready(self, contents=()):
contents = contents or self.data_contents
- return filter(lambda x: takeoff_checks(x), contents)
+ return filter(lambda x: x.states.takeoff_ready, contents)
def flip_ready(self, contents=()):
contents = contents or self.data_contents
- return filter(lambda x: flip_checks(x), contents)
+ return filter(lambda x: flip_checks(x), contents) # possibly change as takeoff checks
def calibrating(self, contents=()):
contents = contents or self.data_contents
@@ -71,6 +123,22 @@ class CopterDataModel(QtCore.QAbstractTableModel):
contents = contents or self.data_contents
return filter(lambda x: calibration_ready_check(x), contents)
+ def get_row_index(self, row_data):
+ try:
+ index = self.data_contents.index(row_data)
+ except ValueError:
+ return None
+ else:
+ return index
+
+ def get_row_by_attr(self, attr, value):
+ try:
+ row_data = next(filter(lambda x: getattr(x, attr, None) == value, self.data_contents))
+ except StopIteration:
+ return None
+ else:
+ return row_data
+
def rowCount(self, n=None):
return len(self.data_contents)
@@ -85,15 +153,19 @@ class CopterDataModel(QtCore.QAbstractTableModel):
def data(self, index, role=Qt.DisplayRole):
row = index.row()
col = index.column()
- #print('row {}, col {}, role {}'.format(row, col, role))
- if role == Qt.DisplayRole:
- #print(self.data_contents[row][col])
- return self.data_contents[row][col] or ""
+ 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 ""
+ elif role == ModelDataRole:
+ return self.data_contents[row][col]
elif role == Qt.BackgroundRole:
- if col in self.checks.keys():
- item = self.data_contents[row][col]
- result = self.checks[col](item)
+ try:
+ item = self.data_contents[row]
+ result = item.states[col]
+ except KeyError:
+ return QtGui.QBrush(Qt.white)
+ else:
if result is None:
return QtGui.QBrush(Qt.yellow)
if result:
@@ -102,61 +174,81 @@ class CopterDataModel(QtCore.QAbstractTableModel):
return QtGui.QBrush(Qt.red)
elif role == Qt.CheckStateRole and col == 0:
- return self.data_contents[row].checked
+ return self.data_contents[row].states.checked
if role == QtCore.Qt.TextAlignmentRole and col != 0:
return QtCore.Qt.AlignHCenter | QtCore.Qt.AlignVCenter
- def update_model(self, index=QtCore.QModelIndex()):
- #self.modelReset.emit()
- self.selected_ready_signal.emit(set(self.user_selected()).issubset(self.selfchecked_ready()))
- self.selected_takeoff_ready_signal.emit(set(self.user_selected()).issubset(self.takeoff_ready()))
- self.selected_flip_ready_signal.emit(set(self.user_selected()).issubset(self.flip_ready()))
- self.selected_calibrating_signal.emit(set(self.user_selected()).issubset(self.calibrating()))
- self.selected_calibration_ready_signal.emit(set(self.user_selected()).issubset(self.calibration_ready()))
- self.dataChanged.emit(index, index, (QtCore.Qt.EditRole,))
+ def update_model(self, index=QtCore.QModelIndex(), role=QtCore.Qt.EditRole):
+ selected = set(self.user_selected())
+
+ self.selected_ready_signal.emit(selected.issubset(self.selfchecked_ready()))
+ self.selected_takeoff_ready_signal.emit(selected.issubset(self.takeoff_ready()))
+
+ self.selected_flip_ready_signal.emit(selected.issubset(self.flip_ready()))
+ self.selected_calibrating_signal.emit(selected.issubset(self.calibrating()))
+ self.selected_calibration_ready_signal.emit(selected.issubset(self.calibration_ready()))
+
+ self.dataChanged.emit(index, index, (role,))
@QtCore.pyqtSlot()
def setData(self, index, value, role=Qt.EditRole):
if not index.isValid():
return False
- if role == Qt.CheckStateRole:
- self.data_contents[index.row()].checked = value
+ col = index.column()
+ row = index.row()
- elif role == Qt.EditRole:
- self.data_contents[index.row()][index.column()] = value
- self.update_model(index)
+ if role == Qt.CheckStateRole:
+ self.data_contents[row].states.checked = value
+ elif role == Qt.EditRole: # For user actions with data
+ if col == 0: # and self.on_id_changed:
+ #self.data_contents[row][col] = "Awaiting for response"
+ #self.data_contents[row].states.copter_id = None
+
+ self.data_contents[row].client.send_message("id", {"new_id": value})
+ self.data_contents[row].client.remove()
+ else:
+ self.data_contents[row][col] = value
+
+ elif role == ModelDataRole: # For inner setting\editing of data
+ self.data_contents[row][col] = value
+ elif role == ModelStateRole:
+ self.data_contents[row].states[col] = value
else:
return False
+ self.update_model(index, role)
return True
def select_all(self):
self.first_col_is_checked = not self.first_col_is_checked
- for copter in self.data_contents:
- copter.checked = int(self.first_col_is_checked)*2
- for row in range(len(self.data_contents)):
- self.update_model(self.index(row, 0))
+ for row_num, copter in enumerate(self.data_contents):
+ copter.states.checked = int(self.first_col_is_checked)*2
+ self.update_model(self.index(row_num, 0), Qt.CheckStateRole)
def flags(self, index):
roles = Qt.ItemIsSelectable | Qt.ItemIsEnabled
if index.column() == 0:
- roles |= Qt.ItemIsUserCheckable
+ roles |= Qt.ItemIsUserCheckable | Qt.ItemIsEditable
return roles
- @QtCore.pyqtSlot(int, int, QtCore.QVariant)
- def update_item(self, row, col, value):
- self.setData(self.index(row, col), value)
+ @QtCore.pyqtSlot(int, int, QtCore.QVariant, QtCore.QVariant)
+ def update_item(self, row, col, value, role=Qt.EditRole):
+ self.setData(self.index(row, col), value, role)
@QtCore.pyqtSlot(object)
def add_client(self, client):
self.insertRows([client])
+ @QtCore.pyqtSlot(int)
+ def remove_client(self, row):
+ self.removeRows(row)
+
def col_check(col):
def inner(f):
- CopterDataModel.checks[col] = f
+ Checks.all_checks[col] = f
def wrapper(*args, **kwargs):
return f(*args, **kwargs)
@@ -172,42 +264,49 @@ def check_anim(item):
return None
return str(item) != 'No animation'
+
@col_check(2)
def check_bat_v(item):
if not item:
return None
return float(item) > 3.2
+
@col_check(3)
def check_bat_p(item):
if not item:
return None
return float(item) > 30
+
@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_selfcheck(item):
if not item:
return None
return item == "OK"
+
@col_check(7)
def check_cal_status(item):
if not item:
return None
return True
+
@col_check(8)
def check_time_delta(item):
if not item:
@@ -216,35 +315,40 @@ def check_time_delta(item):
def all_checks(copter_item):
- for col, check in CopterDataModel.checks.items():
+ for col, check in Checks.all_checks.items():
if not check(copter_item[col]):
return False
return True
+
def takeoff_checks(copter_item):
- for i in range(5):
- if not CopterDataModel.checks[2+i](copter_item[2+i]):
+ for col in Checks.takeoff_checklist:
+ if not Checks.all_checks[col](copter_item[col]):
return False
return True
+
def flip_checks(copter_item):
- for i in range(5):
- if 2+i != 4:
- if not CopterDataModel.checks[2+i](copter_item[2+i]):
+ for col in Checks.takeoff_checklist:
+ if col != 4:
+ if not Checks.all_checks[col](copter_item[col]):
return False
else:
if copter_item[4] != "ACTIVE":
return False
return True
+
def calibrating_check(copter_item):
return copter_item[5] == "CALIBRATING"
+
def calibration_ready_check(copter_item):
- if not CopterDataModel.checks[4](copter_item[4]):
+ if not Checks.all_checks[4](copter_item[4]):
return False
return not calibrating_check(copter_item)
+
class CopterProxyModel(QtCore.QSortFilterProxyModel):
def __init__(self, parent=None):
super(CopterProxyModel, self).__init__(parent)
@@ -265,8 +369,9 @@ class CopterProxyModel(QtCore.QSortFilterProxyModel):
class SignalManager(QtCore.QObject):
- update_data_signal = QtCore.pyqtSignal(int, int, QtCore.QVariant)
+ update_data_signal = QtCore.pyqtSignal(int, int, QtCore.QVariant, QtCore.QVariant)
add_client_signal = QtCore.pyqtSignal(object)
+ remove_client_signal = QtCore.pyqtSignal(int)
if __name__ == '__main__':
@@ -296,9 +401,18 @@ if __name__ == '__main__':
tableView.setSortingEnabled(True)
tableView.show()
- myModel.add_client(CopterData(copter_id=1000, checked=0, time_utc=1))
- myModel.add_client(CopterData(checked=2, selfcheck="OK", time_utc=2))
- myModel.add_client(CopterData(checked=2, selfcheck="not ok", time_utc="no"))
+
+ msgs = []
+ msg = "[{}]: Failure: {}".format("FCU connection", "Angular velocities estimation is not available")
+ msgs.append(msg)
+ msg = "[{}]: Failure: {}".format("FCU connection1", "Angular velocities estimation is not available")
+ msgs.append(msg)
+ msg = "[{}]: Failure: {}".format("FCU connection2", "Angular velocities estimation is not available")
+ msgs.append(msg)
+
+ myModel.add_client(StatedCopterData(copter_id=1000, checked=0, selfcheck=msgs, time_utc=1))
+ myModel.add_client(StatedCopterData(checked=2, selfcheck="OK", time_utc=2))
+ myModel.add_client(StatedCopterData(checked=2, selfcheck="not ok", time_utc="no"))
myModel.setData(myModel.index(0, 1), "test")
diff --git a/Server/server.py b/Server/server.py
index e19ba2d..311b924 100644
--- a/Server/server.py
+++ b/Server/server.py
@@ -28,9 +28,7 @@ logging.basicConfig( # TODO all prints as logs
ConfigOption = collections.namedtuple("ConfigOption", ["section", "option", "value"])
-class Server:
- BUFFER_SIZE = 1024
-
+class Server(messaging.Singleton):
def __init__(self, server_id=None, config_path="server_config.ini", on_stop=None):
self.id = server_id if server_id else str(random.randint(0, 9999)).zfill(4)
self.time_started = 0
@@ -41,8 +39,11 @@ class Server:
self.sel = selectors.DefaultSelector()
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.host = socket.gethostname()
- self.ip = Server.get_ip_address()
+ self.ip = messaging.get_ip_address()
# Init configs
self.config_path = config_path
@@ -65,7 +66,9 @@ class Server:
def load_config(self):
self.config.read(self.config_path)
self.port = int(self.config['SERVER']['port']) # TODO try, init def
- Server.BUFFER_SIZE = int(self.config['SERVER']['buffer_size'])
+ self.BUFFER_SIZE = int(self.config['SERVER']['buffer_size']) # TODO connect to connection manager
+
+ self.remove_disconnected = self.config.getboolean('SERVER', 'remove_disconnected')
self.use_broadcast = self.config.getboolean('BROADCAST', 'use_broadcast')
self.broadcast_port = int(self.config['BROADCAST']['broadcast_port'])
@@ -112,16 +115,6 @@ class Server:
sys.exit("Stopped")
- @staticmethod
- def get_ip_address():
- try:
- with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as ip_socket:
- ip_socket.connect(("8.8.8.8", 80))
- return ip_socket.getsockname()[0]
- except OSError:
- logging.warning("No network connection detected, starting on localhost")
- return "localhost"
-
@staticmethod
def get_ntp_time(ntp_host, ntp_port):
NTP_DELTA = 2208988800 # 1970-01-01 00:00:00
@@ -149,7 +142,7 @@ class Server:
while self.client_processor_thread_running.is_set():
events = self.sel.select()
- logging.error('tick')
+ #logging.error('tick')
for key, mask in events:
# logging.error(mask)
# logging.error(str(key.data))
@@ -161,7 +154,7 @@ class Server:
client.process_events(mask)
except Exception as error:
logging.error("Exception {} occurred for {}! Resetting connection!".format(error, client.addr))
- client.close()
+ client.close(True)
else: # Notifier
client.process_events(mask)
@@ -218,7 +211,7 @@ class Server:
try:
while self.listener_thread_running.is_set():
- data, addr = broadcast_client.recvfrom(1024) # TODO nonblock
+ data, addr = broadcast_client.recvfrom(1024) # TODO nonblock
message = messaging.MessageManager()
message.income_raw = data
message.process_message()
@@ -301,16 +294,28 @@ class Client(messaging.ConnectionManager):
def _got_id(self, value):
logging.info("Got copter id: {} for client {}".format(value, self.addr))
self.copter_id = value
- if Client.on_first_connect:
- Client.on_first_connect(self)
+ if self.on_first_connect:
+ self.on_first_connect(self)
- def close(self):
+ def close(self, inner=False):
self.connected = False
- if Client.on_disconnect:
- Client.on_disconnect(self)
+ if self.on_disconnect:
+ self.on_disconnect(self)
- super(Client, self).close()
+ if inner:
+ super(Client, self)._close()
+ else:
+ super(Client, self).close()
+
+ logging.info("Connection to {} closed!".format(self.copter_id))
+
+ def remove(self):
+ if self.connected:
+ self.close()
+ if self.clients:
+ self.clients.pop(self.addr[0])
+ logging.info("Client {} successfully removed!".format(self.copter_id))
@requires_connect
def _send(self, data):
diff --git a/Server/server_config.ini b/Server/server_config.ini
index 0614208..5160e2d 100644
--- a/Server/server_config.ini
+++ b/Server/server_config.ini
@@ -1,6 +1,7 @@
[SERVER]
port = 25000
buffer_size = 1024
+remove_disconnected = True
[BROADCAST]
use_broadcast = True
diff --git a/Server/server_gui.py b/Server/server_gui.py
index a26363c..794d524 100644
--- a/Server/server_gui.py
+++ b/Server/server_gui.py
@@ -167,7 +167,7 @@ class Ui_MainWindow(object):
self.gridLayout.addLayout(self.horizontalLayout, 0, 0, 1, 1)
MainWindow.setCentralWidget(self.centralwidget)
self.menubar = QtWidgets.QMenuBar(MainWindow)
- self.menubar.setGeometry(QtCore.QRect(0, 0, 1220, 25))
+ self.menubar.setGeometry(QtCore.QRect(0, 0, 1220, 26))
self.menubar.setObjectName("menubar")
self.menuOptions = QtWidgets.QMenu(self.menubar)
self.menuOptions.setObjectName("menuOptions")
@@ -222,6 +222,8 @@ class Ui_MainWindow(object):
self.actionSend_any_command.setObjectName("actionSend_any_command")
self.action_stop_music = QtWidgets.QAction(MainWindow)
self.action_stop_music.setObjectName("action_stop_music")
+ self.action_remove_row = QtWidgets.QAction(MainWindow)
+ self.action_remove_row.setObjectName("action_remove_row")
self.action_send_calibrations = QtWidgets.QAction(MainWindow)
self.action_send_calibrations.setObjectName("action_send_calibrations")
self.menuDeveloper_mode.addAction(self.action_send_any_file)
@@ -244,6 +246,7 @@ class Ui_MainWindow(object):
self.menuDrone.addAction(self.action_reset_z_offset)
self.menuDrone.addSeparator()
self.menuDrone.addAction(self.menuDeveloper_mode_2.menuAction())
+ self.menuDrone.addAction(self.action_remove_row)
self.menuMusic.addAction(self.action_select_music_file)
self.menuMusic.addAction(self.action_play_music)
self.menuMusic.addAction(self.action_stop_music)
@@ -305,4 +308,5 @@ class Ui_MainWindow(object):
self.action_send_any_file.setText(_translate("MainWindow", "Send any file"))
self.actionSend_any_command.setText(_translate("MainWindow", "Send any command"))
self.action_stop_music.setText(_translate("MainWindow", "Stop music"))
+ self.action_remove_row.setText(_translate("MainWindow", "Remove from table"))
self.action_send_calibrations.setText(_translate("MainWindow", "Send camera calibrations"))
diff --git a/Server/server_gui.ui b/Server/server_gui.ui
index c9e1fc6..7141df1 100644
--- a/Server/server_gui.ui
+++ b/Server/server_gui.ui
@@ -328,7 +328,7 @@
0
0
1220
- 25
+ 26
-
@@ -101,11 +104,17 @@
0
+
+ Qt::NoFocus
+
+
+ Qt::DefaultContextMenu
+
Qt::RightToLeft
- true
+ false
@@ -248,14 +257,14 @@
Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter
-
-
+
-
Land
- -
+
-
Flip
@@ -279,6 +288,45 @@
+ -
+
+
+ 0
+
+
-
+
+
+ ArrowCursor
+
+
+ Qt::NoFocus
+
+
+ Qt::LeftToRight
+
+
+ Z =
+
+
+
+ -
+
+
+ Qt::LeftToRight
+
+
+ m
+
+
+ 1
+
+
+ 1.000000000000000
+
+
+
+
+
-
@@ -328,7 +376,7 @@
0
0
1220
- 26
+ 25
+
-
start_delay_spin
diff --git a/Server/server_qt.py b/Server/server_qt.py
index d08f822..d690d82 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -152,6 +152,7 @@ class MainWindow(QtWidgets.QMainWindow):
self.ui.action_restart_clever.triggered.connect(self.restart_clever)
self.ui.action_restart_clever_show.triggered.connect(self.restart_clever_show)
self.ui.action_update_client_repo.triggered.connect(self.update_client_repo)
+ self.ui.action_reboot_all.triggered.connect(self.reboot_all_on_selected)
self.ui.action_set_start_to_current_position.triggered.connect(self.update_start_to_current_position)
self.ui.action_reset_start.triggered.connect(self.reset_start)
self.ui.action_set_z_offset_to_ground.triggered.connect(self.set_z_offset_to_ground)
@@ -439,7 +440,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")
@pyqtSlot()
def update_start_to_current_position(self):
From 5d97ca26a8a6b84ad1fa9ed569d66efde0e79d68 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Wed, 23 Oct 2019 10:23:38 +0100
Subject: [PATCH 15/31] Client: restore default client_config file
---
Drone/client_config.ini | 16 ++++++++--------
1 file changed, 8 insertions(+), 8 deletions(-)
diff --git a/Drone/client_config.ini b/Drone/client_config.ini
index 45585ce..3053175 100644
--- a/Drone/client_config.ini
+++ b/Drone/client_config.ini
@@ -1,3 +1,4 @@
+
[SERVER]
port = 25000
broadcast_port = 8181
@@ -22,11 +23,11 @@ y_ratio = 1.0
z_ratio = 1.0
[COPTERS]
-frame_id = floor
-takeoff_height = 1.0
-takeoff_time = 5.0
+frame_id = map
+takeoff_height = 2.0
+takeoff_time = 8.0
safe_takeoff = False
-reach_first_point_time = 5.0
+reach_first_point_time = 8.0
land_time = 3.0
x0_common = 0
y0_common = 0
@@ -36,7 +37,7 @@ z0_common = 0
parent = aruco_map
x = 0.0
y = 0.0
-z = 3.55
+z = 6.5
roll = 180
pitch = 0
yaw = -90
@@ -44,9 +45,8 @@ yaw = -90
[PRIVATE]
id = /hostname
restart_dhcpcd = True
-use_leds = False
+use_leds = True
led_pin = 21
x0 = 0
y0 = 0
-z0 = 0
-
+z0 = 0
\ No newline at end of file
From 79d59f0b28e28e1f9cc06e6ff1d0a9379bff3811 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Fri, 25 Oct 2019 07:06:17 +0100
Subject: [PATCH 16/31] Client: Fix bug in move start callback
---
Drone/client_config.ini | 16 ++++++++--------
Drone/copter_client.py | 17 ++++++++---------
2 files changed, 16 insertions(+), 17 deletions(-)
diff --git a/Drone/client_config.ini b/Drone/client_config.ini
index 3053175..45585ce 100644
--- a/Drone/client_config.ini
+++ b/Drone/client_config.ini
@@ -1,4 +1,3 @@
-
[SERVER]
port = 25000
broadcast_port = 8181
@@ -23,11 +22,11 @@ y_ratio = 1.0
z_ratio = 1.0
[COPTERS]
-frame_id = map
-takeoff_height = 2.0
-takeoff_time = 8.0
+frame_id = floor
+takeoff_height = 1.0
+takeoff_time = 5.0
safe_takeoff = False
-reach_first_point_time = 8.0
+reach_first_point_time = 5.0
land_time = 3.0
x0_common = 0
y0_common = 0
@@ -37,7 +36,7 @@ z0_common = 0
parent = aruco_map
x = 0.0
y = 0.0
-z = 6.5
+z = 3.55
roll = 180
pitch = 0
yaw = -90
@@ -45,8 +44,9 @@ yaw = -90
[PRIVATE]
id = /hostname
restart_dhcpcd = True
-use_leds = True
+use_leds = False
led_pin = 21
x0 = 0
y0 = 0
-z0 = 0
\ No newline at end of file
+z0 = 0
+
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index cd2f113..e60a136 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -329,21 +329,20 @@ def _command_test(*args, **kwargs):
def _command_move_start_to_current_position(*args, **kwargs):
# Load animation
frames = animation.load_animation(os.path.abspath("animation.csv"),
- x0=client.active_client.X0 + client.active_client.X0_COMMON,
- y0=client.active_client.Y0 + client.active_client.Y0_COMMON,
- z0=client.active_client.Z0 + client.active_client.Z0_COMMON,
x_ratio=client.active_client.X_RATIO,
y_ratio=client.active_client.Y_RATIO,
z_ratio=client.active_client.Z_RATIO,
)
# Correct start and land frames in animation
- corrected_frames, start_action, start_delay = animation.correct_animation(frames,
- check_takeoff=client.active_client.TAKEOFF_CHECK,
- check_land=client.active_client.LAND_CHECK,
- )
- x_start = corrected_frames[0]['x']
- y_start = corrected_frames[0]['y']
+ # corrected_frames, start_action, start_delay = animation.correct_animation(frames,
+ # check_takeoff=client.active_client.TAKEOFF_CHECK,
+ # check_land=client.active_client.LAND_CHECK,
+ # )
+ x_start = frames[0]['x']
+ y_start = frames[0]['y']
+ print("x_start = {}, y_start = {}".format(x_start, y_start))
telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
+ print("x_telem = {}, y_telem = {}".format(telem.x, telem.y))
client.active_client.config.set('PRIVATE', 'x0', telem.x - x_start)
client.active_client.config.set('PRIVATE', 'y0', telem.y - y_start)
client.active_client.rewrite_config()
From eb96ee4a4b42336f18ee2dbd20daa10d8a8105ba Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Tue, 29 Oct 2019 14:30:19 +0300
Subject: [PATCH 17/31] Server: don't do selfcheck before takeoff until the
connection problem will be solved
---
Server/server_qt.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/Server/server_qt.py b/Server/server_qt.py
index d690d82..846e012 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -263,7 +263,7 @@ class MainWindow(QtWidgets.QMainWindow):
music_dt = self.ui.music_delay_spin.value()
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()
+ # self.selfcheck_selected()
for copter in self.model.user_selected():
if all_checks(copter):
server.send_starttime(copter.client, dt+time_now)
From 585cb616732c40644970809c9882d39b1ebaa254 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Tue, 29 Oct 2019 15:06:55 +0300
Subject: [PATCH 18/31] Server: Change remove_disconnected param to False in
server_config
---
Server/server_config.ini | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/Server/server_config.ini b/Server/server_config.ini
index 5160e2d..8fdfddc 100644
--- a/Server/server_config.ini
+++ b/Server/server_config.ini
@@ -1,7 +1,7 @@
[SERVER]
port = 25000
buffer_size = 1024
-remove_disconnected = True
+remove_disconnected = False
[BROADCAST]
use_broadcast = True
@@ -11,4 +11,4 @@ broadcast_delay = 5
[NTP]
use_ntp = False
host = ntp1.stratum2.ru
-port = 123
\ No newline at end of file
+port = 123
From 82a434b4caa75f8871609891f78543165d0bb688 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Tue, 29 Oct 2019 15:54:08 +0300
Subject: [PATCH 19/31] Add ability to send folder with launch files
---
Server/server_gui.ui | 4 ++--
Server/server_qt.py | 10 ++++++----
messaging_lib.py | 2 +-
3 files changed, 9 insertions(+), 7 deletions(-)
diff --git a/Server/server_gui.ui b/Server/server_gui.ui
index 36569a1..bb58ced 100644
--- a/Server/server_gui.ui
+++ b/Server/server_gui.ui
@@ -7,7 +7,7 @@
0
0
1220
- 750
+ 761
@@ -466,7 +466,7 @@
- Send launch file to clever
+ Send launch files
diff --git a/Server/server_qt.py b/Server/server_qt.py
index 846e012..eed0560 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -419,12 +419,14 @@ class MainWindow(QtWidgets.QMainWindow):
@pyqtSlot()
def send_launch(self):
- path = QFileDialog.getOpenFileName(self, "Select launch file for clever", filter="Launch files (*.launch)")[0]
+ path = str(QFileDialog.getExistingDirectory(self, "Select directory with launch files"))
if path:
- filename = os.path.basename(path)
- print("Selected file:", path, filename)
+ print("Selected directory:", path)
+ files = [file for file in glob.glob(path + '/*.launch')]
for copter in self.model.user_selected():
- copter.client.send_file(path, "/home/pi/catkin_ws/src/clever/clever/launch/{}".format(filename))
+ for file in files:
+ filename = os.path.basename(file)
+ copter.client.send_file(file, "/home/pi/catkin_ws/src/clever/clever/launch/{}".format(filename))
# copter.client.send_message("service_restart", {"name": "clever"})
@pyqtSlot()
diff --git a/messaging_lib.py b/messaging_lib.py
index 357e7f1..1cb0383 100644
--- a/messaging_lib.py
+++ b/messaging_lib.py
@@ -383,7 +383,7 @@ class ConnectionManager(object):
logger.error("File {} can not be written due error: {}".format(filepath, error))
else:
logger.info("File {} successfully received ".format(filepath))
- os.system("chown -R pi:pi /home/pi/clever-show")
+ os.system("chown -R pi:pi /home/pi/clever-show/")
def write(self):
with self._send_lock:
From 34999302f7bfd5069a35f1e2f02be708ac8e11a1 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Tue, 29 Oct 2019 15:58:27 +0300
Subject: [PATCH 20/31] Server: Update GUI
---
Server/server_gui.py | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/Server/server_gui.py b/Server/server_gui.py
index dcc652c..4743936 100644
--- a/Server/server_gui.py
+++ b/Server/server_gui.py
@@ -13,7 +13,7 @@ from PyQt5 import QtCore, QtGui, QtWidgets
class Ui_MainWindow(object):
def setupUi(self, MainWindow):
MainWindow.setObjectName("MainWindow")
- MainWindow.resize(1220, 750)
+ MainWindow.resize(1220, 761)
self.centralwidget = QtWidgets.QWidget(MainWindow)
self.centralwidget.setEnabled(True)
self.centralwidget.setObjectName("centralwidget")
@@ -316,7 +316,7 @@ class Ui_MainWindow(object):
self.action_send_Aruco_map.setText(_translate("MainWindow", "Send aruco map"))
self.action_update_client_repo.setText(_translate("MainWindow", "Update clever-show git"))
self.actionSend_launch_file_for_clever.setText(_translate("MainWindow", "Send launch file for clever"))
- self.action_send_launch_file.setText(_translate("MainWindow", "Send launch file to clever"))
+ self.action_send_launch_file.setText(_translate("MainWindow", "Send launch files"))
self.action_restart_clever.setText(_translate("MainWindow", "Restart clever service"))
self.action_restart_clever_show.setText(_translate("MainWindow", "Restart clever-show service"))
self.action_select_all_rows.setText(_translate("MainWindow", "Select all drones"))
From 167fafbbb0d5ccda365e4e525381f64f23508697 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Tue, 29 Oct 2019 16:03:31 +0300
Subject: [PATCH 21/31] file transfer: return rights to pi:pi after file
receive
---
messaging_lib.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/messaging_lib.py b/messaging_lib.py
index 1cb0383..878c76f 100644
--- a/messaging_lib.py
+++ b/messaging_lib.py
@@ -383,7 +383,7 @@ class ConnectionManager(object):
logger.error("File {} can not be written due error: {}".format(filepath, error))
else:
logger.info("File {} successfully received ".format(filepath))
- os.system("chown -R pi:pi /home/pi/clever-show/")
+ os.system("chown pi:pi {}".format(filepath))
def write(self):
with self._send_lock:
From 1adade0bdabada884363ab7e5e56dd1c641bf95b Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Tue, 29 Oct 2019 16:38:13 +0300
Subject: [PATCH 22/31] Server: Add check for position col in table
---
Server/copter_table_models.py | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py
index 0905635..62bb977 100644
--- a/Server/copter_table_models.py
+++ b/Server/copter_table_models.py
@@ -305,10 +305,10 @@ def check_selfcheck(item):
@col_check(7)
-def check_cal_status(item):
+def check_pos_status(item):
if not item:
return None
- return True
+ return str(item).split(' ')[0] != 'nan'
@col_check(8)
From d33cc0c32dfa3316d9cb6d4d191189ab0096cf8c Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Fri, 25 Oct 2019 12:28:55 +0100
Subject: [PATCH 23/31] Client: modify calibration response and reduce timeout
in service check
---
Drone/FlightLib/FlightLib.py | 2 +-
Drone/copter_client.py | 6 +++++-
2 files changed, 6 insertions(+), 2 deletions(-)
diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py
index d0c304e..40d019e 100644
--- a/Drone/FlightLib/FlightLib.py
+++ b/Drone/FlightLib/FlightLib.py
@@ -91,7 +91,7 @@ def _check_nans(*values):
@check("Ros services")
def check_ros_services():
- timeout = 5.0
+ timeout = 0.1
for service in services_list:
try:
service.wait_for_service(timeout=timeout)
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index e60a136..6566a66 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -301,7 +301,11 @@ def _response_sys_status(*args, **kwargs):
@messaging.request_callback("cal_status")
def _response_cal_status(*args, **kwargs):
- return get_calibration_status()
+ if check_state_topic(wait_new_status=True):
+ return get_calibration_status()
+ else:
+ stop_subscriber()
+ return "NOT_CONNECTED_TO_FCU"
@messaging.request_callback("position")
def _response_position(*args, **kwargs):
From dc85d9bd3f65a04f751e0e694034c7cfaaae5762 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Fri, 25 Oct 2019 15:18:56 +0100
Subject: [PATCH 24/31] animation_lib: add get_start_xy function
---
Drone/animation_lib.py | 34 ++++++++++++++++++++++++++++++++++
1 file changed, 34 insertions(+)
diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py
index 4c12b92..857f7b9 100644
--- a/Drone/animation_lib.py
+++ b/Drone/animation_lib.py
@@ -37,9 +37,43 @@ def get_id(filepath="animation.csv"):
anim_id = row_0[0]
print("Got animation_id: {}".format(anim_id))
else:
+ anim_id = "Empty id"
print("No animation id in file")
return anim_id
+def get_start_xy(filepath="animation.csv"):
+ try:
+ animation_file = open(filepath)
+ except IOError:
+ logging.error("File {} can't be opened".format(filepath))
+ anim_id = "No animation"
+ return float('nan'), float('nan')
+ else:
+ with animation_file:
+ csv_reader = csv.reader(
+ animation_file, delimiter=',', quotechar='|'
+ )
+ try:
+ row_0 = csv_reader.next()
+ except:
+ return float('nan'), float('nan')
+ if len(row_0) == 1:
+ anim_id = row_0[0]
+ print("Got animation_id: {}".format(anim_id))
+ try:
+ frame_number, x, y, z, yaw, red, green, blue = csv_reader.next()
+ except:
+ return float('nan'), float('nan')
+ else:
+ anim_id = "Empty id"
+ print("No animation id in file")
+ try:
+ frame_number, x, y, z, yaw, red, green, blue = row_0
+ except:
+ return float('nan'), float('nan')
+ return float(x), float(y)
+
+
def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_ratio=1, z_ratio=1):
imported_frames = []
global anim_id
From 46802667bde0ef26c1dae9db767df9e273ed3dea Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Fri, 25 Oct 2019 15:32:49 +0100
Subject: [PATCH 25/31] Client: modify move_start function
---
Drone/animation_lib.py | 4 ++--
Drone/copter_client.py | 16 ++--------------
2 files changed, 4 insertions(+), 16 deletions(-)
diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py
index 857f7b9..c9913c7 100644
--- a/Drone/animation_lib.py
+++ b/Drone/animation_lib.py
@@ -41,7 +41,7 @@ def get_id(filepath="animation.csv"):
print("No animation id in file")
return anim_id
-def get_start_xy(filepath="animation.csv"):
+def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1):
try:
animation_file = open(filepath)
except IOError:
@@ -71,7 +71,7 @@ def get_start_xy(filepath="animation.csv"):
frame_number, x, y, z, yaw, red, green, blue = row_0
except:
return float('nan'), float('nan')
- return float(x), float(y)
+ return float(x)*x_ratio, float(y)*y_ratio
def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_ratio=1, z_ratio=1):
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index 6566a66..61ee8f3 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -21,7 +21,6 @@ from tf.transformations import quaternion_from_euler, euler_from_quaternion, qua
import tf2_ros
static_bloadcaster = tf2_ros.StaticTransformBroadcaster()
-
# logging.basicConfig( # TODO all prints as logs
# level=logging.DEBUG, # INFO
# format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s",
@@ -95,7 +94,7 @@ class CopterClient(client.Client):
trans.child_frame_id = self.FRAME_ID
static_bloadcaster.sendTransform(trans)
start_subscriber()
- # print(check_state_topic())
+
super(CopterClient, self).start()
@@ -331,19 +330,10 @@ def _command_test(*args, **kwargs):
@messaging.message_callback("move_start")
def _command_move_start_to_current_position(*args, **kwargs):
- # Load animation
- frames = animation.load_animation(os.path.abspath("animation.csv"),
+ 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,
- z_ratio=client.active_client.Z_RATIO,
)
- # Correct start and land frames in animation
- # corrected_frames, start_action, start_delay = animation.correct_animation(frames,
- # check_takeoff=client.active_client.TAKEOFF_CHECK,
- # check_land=client.active_client.LAND_CHECK,
- # )
- x_start = frames[0]['x']
- y_start = frames[0]['y']
print("x_start = {}, y_start = {}".format(x_start, y_start))
telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
print("x_telem = {}, y_telem = {}".format(telem.x, telem.y))
@@ -596,8 +586,6 @@ def _play_animation(*args, **kwargs):
},
)
#print(task_manager.task_queue)
-
-
if __name__ == "__main__":
copter_client = CopterClient()
From 0f5b5f0346029de2410bae2e81574862e2e16e23 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Fri, 25 Oct 2019 17:22:49 +0100
Subject: [PATCH 26/31] Client: Add telemetry get and send thread
---
Drone/copter_client.py | 90 ++++++++++++++++++++++++++++++++++++++----
1 file changed, 82 insertions(+), 8 deletions(-)
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index 61ee8f3..44123dc 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -4,6 +4,9 @@ import math
import rospy
import datetime
import logging
+import threading
+import subprocess
+from collections import namedtuple
from FlightLib import FlightLib
from FlightLib import LedLib
@@ -21,6 +24,9 @@ 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 current_position start_position")
+telemetry = Telemetry('nan', 'No animation', 'nan', 'nan', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_POS', 'NO_POS')
+
# logging.basicConfig( # TODO all prints as logs
# level=logging.DEBUG, # INFO
# format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s",
@@ -36,6 +42,8 @@ logger = logging.getLogger(__name__)
class CopterClient(client.Client):
def load_config(self):
super(CopterClient, self).load_config()
+ self.TELEM_FREQ = self.config.getfloat('TELEMETRY', 'frequency')
+ self.TELEM_TRANSMIT = self.config.getboolean('TELEMETRY', 'transmit')
self.FRAME_ID = self.config.get('COPTERS', 'frame_id')
self.FRAME_FLIPPED_HEIGHT = 0.
self.TAKEOFF_HEIGHT = self.config.getfloat('COPTERS', 'takeoff_height')
@@ -94,7 +102,7 @@ class CopterClient(client.Client):
trans.child_frame_id = self.FRAME_ID
static_bloadcaster.sendTransform(trans)
start_subscriber()
-
+ telemetry_thread.start()
super(CopterClient, self).start()
@@ -335,13 +343,19 @@ def _command_move_start_to_current_position(*args, **kwargs):
y_ratio=client.active_client.Y_RATIO,
)
print("x_start = {}, y_start = {}".format(x_start, y_start))
- telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
- print("x_telem = {}, y_telem = {}".format(telem.x, telem.y))
- client.active_client.config.set('PRIVATE', 'x0', telem.x - x_start)
- client.active_client.config.set('PRIVATE', 'y0', telem.y - y_start)
- client.active_client.rewrite_config()
- client.active_client.load_config()
- print ("Start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
+ if not math.isnan(x_start):
+ telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
+ print("x_telem = {}, y_telem = {}".format(telem.x, telem.y))
+ if not math.isnan(x_telem):
+ client.active_client.config.set('PRIVATE', 'x0', telem.x - x_start)
+ client.active_client.config.set('PRIVATE', 'y0', telem.y - y_start)
+ client.active_client.rewrite_config()
+ client.active_client.load_config()
+ print ("Start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
+ else:
+ print ("Wrong telemetry")
+ else:
+ print("Wrong animation file")
@messaging.message_callback("reset_start")
def _command_reset_start(*args, **kwargs):
@@ -587,6 +601,66 @@ def _play_animation(*args, **kwargs):
)
#print(task_manager.task_queue)
+def telemetry_loop():
+ global telemetry
+ 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))
+ 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(current_position = 'NO_POS in {}'.format(client.active_client.FRAME_ID))
+ telemetry = telemetry._replace(start_position = 'NO_POS')
+ try:
+ ros_telemetry = FlightLib.get_telemetry(client.active_client.FRAME_ID)
+ except:
+ pass
+ else:
+ if ros_telemetry.connected == False:
+ stop_subscriber()
+ start_subscriber()
+ else:
+ 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')
+ if batt_empty_param.success and batt_charged_param.success:
+ batt_empty = batt_empty_param.value.real
+ batt_charged = batt_charged_param.value.real
+ telemetry = telemetry._replace(battery_p = '{:.2f}'.format((ros_telemetry.cell_voltage - batt_empty)/(batt_charged - batt_empty)*100.))
+ telemetry = telemetry._replace(calibration_status = get_calibration_status())
+ telemetry = telemetry._replace(system_status = get_sys_status())
+ telemetry = telemetry._replace(mode = ros_telemetry.mode)
+ 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))
+ 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_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))
+ if client.active_client.TELEM_TRANSMIT:
+ try:
+ client.active_client.server_connection.send_message('telem', args={'message':create_telemetry_message(telemetry)})
+ except Exception as e:
+ print e
+ rate.sleep()
+
+def create_telemetry_message(telemetry):
+ msg = ""
+ for key in telemetry.__dict__:
+ msg += telemetry.__dict__[key] + ';'
+ msg += repr(time.time())
+ return msg
+
+telemetry_thread = threading.Thread(target=telemetry_loop, name="Telemetry getting thread")
+
if __name__ == "__main__":
copter_client = CopterClient()
task_manager = tasking.TaskManager()
From f8ae6c626349d806546e44931d04d48ccda27304 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Fri, 25 Oct 2019 17:23:22 +0100
Subject: [PATCH 27/31] Client: Modify default config file
---
Drone/client_config.ini | 8 ++++++--
1 file changed, 6 insertions(+), 2 deletions(-)
diff --git a/Drone/client_config.ini b/Drone/client_config.ini
index 45585ce..487f309 100644
--- a/Drone/client_config.ini
+++ b/Drone/client_config.ini
@@ -13,6 +13,10 @@ use_ntp = False
host = ntp1.stratum2.ru
port = 123
+[TELEMETRY]
+frequency = 1
+transmit = True
+
[ANIMATION]
takeoff_animation_check = True
land_animation_check = True
@@ -22,7 +26,7 @@ y_ratio = 1.0
z_ratio = 1.0
[COPTERS]
-frame_id = floor
+frame_id = map
takeoff_height = 1.0
takeoff_time = 5.0
safe_takeoff = False
@@ -44,7 +48,7 @@ yaw = -90
[PRIVATE]
id = /hostname
restart_dhcpcd = True
-use_leds = False
+use_leds = True
led_pin = 21
x0 = 0
y0 = 0
From fbff10cc25c9bc529a6648e901a3ef7a274902ec Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Fri, 25 Oct 2019 18:05:59 +0100
Subject: [PATCH 28/31] Client: add selfcheck to telemetry
---
Drone/copter_client.py | 11 ++++++++---
1 file changed, 8 insertions(+), 3 deletions(-)
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index 44123dc..3ff17c2 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -24,8 +24,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 current_position start_position")
-telemetry = Telemetry('nan', 'No animation', 'nan', 'nan', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_POS', 'NO_POS')
+Telemetry = namedtuple("Telemetry", "git_version animation_id battery_v battery_p system_status calibration_status mode selfcheck current_position start_position")
+telemetry = Telemetry('nan', 'No animation', 'nan', 'nan', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_POS', 'NO_POS')
# logging.basicConfig( # TODO all prints as logs
# level=logging.DEBUG, # INFO
@@ -612,6 +612,7 @@ def telemetry_loop():
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 in {}'.format(client.active_client.FRAME_ID))
telemetry = telemetry._replace(start_position = 'NO_POS')
try:
@@ -633,6 +634,10 @@ def telemetry_loop():
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))
@@ -653,7 +658,7 @@ def telemetry_loop():
rate.sleep()
def create_telemetry_message(telemetry):
- msg = ""
+ msg = client.active_client.client_id + ';'
for key in telemetry.__dict__:
msg += telemetry.__dict__[key] + ';'
msg += repr(time.time())
From 4619a340e37c8de7af90403fb48b8cc4785da51c Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Wed, 30 Oct 2019 06:41:20 +0300
Subject: [PATCH 29/31] Server: Add support for telemetry streaming
---
Server/server_qt.py | 54 +++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 54 insertions(+)
diff --git a/Server/server_qt.py b/Server/server_qt.py
index eed0560..1db2009 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -16,6 +16,7 @@ from quamash import QEventLoop, QThreadExecutor
from server_gui import Ui_MainWindow
from server import *
+import messaging_lib as messaging
from copter_table_models import *
from emergency import *
@@ -54,6 +55,24 @@ def confirmation_required(text="Are you sure?", label="Confirm operation?"):
return inner
+class Signal(object):
+ class Emitter(QObject):
+ send = pyqtSignal(str)
+ def __init__(self):
+ super(Signal.Emitter, self).__init__()
+
+ def __init__(self):
+ self.emitter = Signal.Emitter()
+
+ def send_message(self, message):
+ self.emitter.send.emit(message)
+
+ def connect(self, signal, slot):
+ signal.emitter.send.connect(slot)
+
+class Sender(object):
+ def __init__(self):
+ self.signal = Signal()
# noinspection PyArgumentList,PyCallByClass
class MainWindow(QtWidgets.QMainWindow):
@@ -70,6 +89,7 @@ class MainWindow(QtWidgets.QMainWindow):
self.player = QtMultimedia.QMediaPlayer()
self.init_model()
+ self.signal = Signal()
self.show()
@@ -211,6 +231,33 @@ class MainWindow(QtWidgets.QMainWindow):
self.signals.update_data_signal.emit(row, col, data, ModelDataRole)
+ @pyqtSlot(str)
+ def update_table_data(self, message):
+ fields = message.split(';')
+ logging.info(fields)
+ # 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]
+ 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]
+ row = self.model.get_row_index(self.model.get_row_by_attr('copter_id', copter_id))
+ logging.info("Row = {}".format(row))
+ self.signals.update_data_signal.emit(row, 1, animation_id, ModelDataRole)
+ self.signals.update_data_signal.emit(row, 2, battery_v, ModelDataRole)
+ self.signals.update_data_signal.emit(row, 3, battery_p, 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, selfcheck, ModelDataRole)
+ self.signals.update_data_signal.emit(row, 7, current_pos, ModelDataRole)
+ self.signals.update_data_signal.emit(row, 8, "{}".format(round(float(copter_time) - time.time(), 3)), ModelDataRole)
#def set_copter_id(self, value, copter_data_row):
# col = 0
@@ -565,6 +612,11 @@ 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)
+ sender.signal.send_message(message)
+
if __name__ == "__main__":
app = QtWidgets.QApplication(sys.argv)
@@ -573,7 +625,9 @@ if __name__ == "__main__":
#app.exec_()
with loop:
+ sender = Sender()
window = MainWindow()
+ sender.signal.connect(sender.signal, window.update_table_data)
Client.on_first_connect = window.new_client_connected
Client.on_connect = window.client_connection_changed
From 9f536356abc75db7056816886740437e168867b9 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Wed, 30 Oct 2019 07:05:48 +0300
Subject: [PATCH 30/31] Server: Update request for telemetry
---
Server/server_qt.py | 13 ++++++++++---
1 file changed, 10 insertions(+), 3 deletions(-)
diff --git a/Server/server_qt.py b/Server/server_qt.py
index 1db2009..6f697e0 100644
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -187,7 +187,7 @@ class MainWindow(QtWidgets.QMainWindow):
self.ui.flip_button.setEnabled(False)
@pyqtSlot()
- def selfcheck_selected(self):
+ def selfcheck_selected_old(self):
for copter_data_row in self.model.user_selected():
client = copter_data_row.client
@@ -200,6 +200,7 @@ class MainWindow(QtWidgets.QMainWindow):
client.get_response("position", self.set_copter_data, callback_args=(7, copter_data_row))
client.get_response("time", self.set_copter_data, callback_args=(8, copter_data_row))
+
def set_copter_data(self, value, col, copter_data_row):
row = self.model.get_row_index(copter_data_row)
if row is None:
@@ -231,10 +232,16 @@ class MainWindow(QtWidgets.QMainWindow):
self.signals.update_data_signal.emit(row, col, data, ModelDataRole)
+ @pyqtSlot()
+ def selfcheck_selected(self):
+ for copter_data_row in self.model.user_selected():
+ client = copter_data_row.client
+ client.get_response("telemetry", self.update_table_data)
+
@pyqtSlot(str)
def update_table_data(self, message):
- fields = message.split(';')
- logging.info(fields)
+ fields = message.split('`')
+ logging.info(fields[8])
# 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]
From 05303cc4b7bd13ae24555342b39ecf360e652bc3 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov
Date: Wed, 30 Oct 2019 04:06:59 +0000
Subject: [PATCH 31/31] Client: Add response for telemetry
---
Drone/copter_client.py | 30 ++++++++++++++++++++----------
1 file changed, 20 insertions(+), 10 deletions(-)
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index 3ff17c2..666fa36 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -260,6 +260,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)
+
@messaging.request_callback("anim_id")
def _response_animation_id(*args, **kwargs):
@@ -607,20 +611,20 @@ def telemetry_loop():
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))
- 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 in {}'.format(client.active_client.FRAME_ID))
- telemetry = telemetry._replace(start_position = 'NO_POS')
try:
ros_telemetry = FlightLib.get_telemetry(client.active_client.FRAME_ID)
except:
pass
else:
if ros_telemetry.connected == False:
+ 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 in {}'.format(client.active_client.FRAME_ID))
+ telemetry = telemetry._replace(start_position = 'NO_POS')
stop_subscriber()
start_subscriber()
else:
@@ -631,6 +635,8 @@ def telemetry_loop():
batt_empty = batt_empty_param.value.real
batt_charged = batt_charged_param.value.real
telemetry = telemetry._replace(battery_p = '{:.2f}'.format((ros_telemetry.cell_voltage - batt_empty)/(batt_charged - batt_empty)*100.))
+ 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)
@@ -641,6 +647,8 @@ def telemetry_loop():
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))
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,
@@ -650,6 +658,8 @@ def telemetry_loop():
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')
if client.active_client.TELEM_TRANSMIT:
try:
client.active_client.server_connection.send_message('telem', args={'message':create_telemetry_message(telemetry)})
@@ -658,9 +668,9 @@ def telemetry_loop():
rate.sleep()
def create_telemetry_message(telemetry):
- msg = client.active_client.client_id + ';'
+ msg = client.active_client.client_id + '`'
for key in telemetry.__dict__:
- msg += telemetry.__dict__[key] + ';'
+ msg += telemetry.__dict__[key] + '`'
msg += repr(time.time())
return msg