From 79d59f0b28e28e1f9cc06e6ff1d0a9379bff3811 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 25 Oct 2019 07:06:17 +0100 Subject: [PATCH 01/47] 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 02/47] 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 03/47] 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 04/47] 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 05/47] 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 06/47] 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 07/47] 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 08/47] 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 09/47] 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 10/47] 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 11/47] 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 12/47] 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 13/47] 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 14/47] 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 15/47] 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 16/47] 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 From 5e56a82b35c7a4ffe6e2b08b8b4fa58e0ec621a6 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 31 Oct 2019 15:12:34 +0300 Subject: [PATCH 17/47] Server: simplify table update --- Server/server_qt.py | 24 +----------------------- 1 file changed, 1 insertion(+), 23 deletions(-) diff --git a/Server/server_qt.py b/Server/server_qt.py index 6f697e0..b6f819b 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -55,25 +55,6 @@ 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): def __init__(self): @@ -89,7 +70,6 @@ class MainWindow(QtWidgets.QMainWindow): self.player = QtMultimedia.QMediaPlayer() self.init_model() - self.signal = Signal() self.show() @@ -622,7 +602,7 @@ class MainWindow(QtWidgets.QMainWindow): @messaging.message_callback("telem") def get_telem_data(*args, **kwargs): message = kwargs.get("message", None) - sender.signal.send_message(message) + window.update_table_data(message) if __name__ == "__main__": @@ -632,9 +612,7 @@ 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 b17607e4c31402d75bd9e29ece767da0427d2858 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 31 Oct 2019 16:33:28 +0000 Subject: [PATCH 18/47] FlightLib: Fix check ros services function --- Drone/FlightLib/FlightLib.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index 40d019e..6d6e28d 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -22,7 +22,8 @@ get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) landing = rospy.ServiceProxy('/land', Trigger) -services_list = [navigate, set_position, set_rates, set_mode, get_telemetry, arming,landing] +services_list = ['/navigate', '/set_position', '/set_rates', '/mavros/set_mode', + '/get_telemetry', '/mavros/cmd/arming', '/land', '/mavros/param/get'] logger.info("Proxy services inited") @@ -88,15 +89,22 @@ def check(check_name): def _check_nans(*values): return any(math.isnan(x) for x in values) +def check_ros_services_unavailable(): + unavailable_services = [] + for service in services_list: + try: + rospy.wait_for_service(service, timeout=0.1) + except (rospy.ServiceException, rospy.ROSException): + unavailable_services.append(service) + return unavailable_services @check("Ros services") def check_ros_services(): - timeout = 0.1 for service in services_list: try: - service.wait_for_service(timeout=timeout) - except (rospy.ServiceException, rospy.ROSException) as e: - yield ("ROS service {} is not available!".format(service.name)) + rospy.wait_for_service(service, timeout=0.1) + except (rospy.ServiceException, rospy.ROSException): + yield ("ROS service {} is not available!".format(service)) @check("FCU connection") From 370bee5c890b84e3f252b0350cc799d79efd4369 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 31 Oct 2019 16:39:12 +0000 Subject: [PATCH 19/47] mavros_mavlink: Modify get_calibration_status --- Drone/mavros_mavlink.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/Drone/mavros_mavlink.py b/Drone/mavros_mavlink.py index 7f58180..e5dede5 100644 --- a/Drone/mavros_mavlink.py +++ b/Drone/mavros_mavlink.py @@ -78,17 +78,11 @@ def get_calibration_status(): mag_status = get_param('CAL_MAG0_ID') acc_status = get_param('CAL_ACC0_ID') status_text = "" - if gyro_status.success == False: - status_text += "gyro: wrong_param; " - elif gyro_status.value.integer == 0: + if gyro_status.value.integer == 0 and gyro_status.success: status_text += "gyro: uncalibrated; " - if mag_status.success == False: - status_text += "mag: wrong_param; " - elif mag_status.value.integer == 0: + if mag_status.value.integer == 0 and mag_status.success: status_text += "mag: uncalibrated; " - if acc_status.success == False: - status_text += "acc: wrong_param; " - elif acc_status.value.integer == 0: + if acc_status.value.integer == 0 and acc_status.success: status_text += "acc: uncalibrated; " if status_text == "": status_text = "OK" From e5c25eac4fc0c37f9deac57849cbeeaaf9650b0f Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 31 Oct 2019 16:39:59 +0000 Subject: [PATCH 20/47] Client: Fix telemetry thread --- Drone/copter_client.py | 94 +++++++++++++++++++++--------------------- 1 file changed, 48 insertions(+), 46 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 666fa36..617d27b 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -611,55 +611,57 @@ 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)) - try: - ros_telemetry = FlightLib.get_telemetry(client.active_client.FRAME_ID) - except: - pass + 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)) 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: - 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(start_position = 'NO_POS') + services_unavailable = FlightLib.check_ros_services_unavailable() + if not services_unavailable: + try: + ros_telemetry = FlightLib.get_telemetry(client.active_client.FRAME_ID) + if ros_telemetry.connected: + 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 + telemetry = telemetry._replace(battery_p = '{:.2f}'.format(min((ros_telemetry.voltage/batt_cells - batt_empty)/(batt_charged - batt_empty)*100., 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) + 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 = 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)) - 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)) - else: - telemetry = telemetry._replace(start_position = 'NO_POS') + 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: + print "Some service is unavailable" + 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)}) From 87a45ad1571f218e94e3476bbff2619fd5798146 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 17:16:25 +0300 Subject: [PATCH 21/47] copter_table_models: Add three new rows with git version, mode and start x y z --- Server/copter_table_models.py | 58 +++++++++++++++++++---------------- 1 file changed, 32 insertions(+), 26 deletions(-) diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index 62bb977..d011b76 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -13,11 +13,10 @@ ModelStateRole = 999 class CopterData: - 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), ]) + 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), + ('start_pos', None), ('time_delta', None), ('client', None)]) def __init__(self, **kwargs): self.attrs_dict = self.class_basic_attrs.copy() @@ -63,7 +62,7 @@ class StatedCopterData(CopterData): class Checks: all_checks = {} - takeoff_checklist = (2, 3, 4, 5, 6) + takeoff_checklist = (3, 4, 6, 7, 8) class CopterDataModel(QtCore.QAbstractTableModel): @@ -75,8 +74,8 @@ 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', 'version', ' animation ID ', ' battery ', ' system ', 'calibration', + ' mode ', 'selfcheck', 'current x y z yaw frame_id', ' start x y z ', 'dt') self.data_contents = [] self.on_id_changed = None @@ -86,7 +85,6 @@ class CopterDataModel(QtCore.QAbstractTableModel): def insertRows(self, contents, position='last', parent=QtCore.QModelIndex()): rows = len(contents) position = len(self.data_contents) if position == 'last' else position - self.beginInsertRows(parent, position, position + rows - 1) self.data_contents[position:position] = contents @@ -263,25 +261,22 @@ def col_check(col): @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(2) -def check_bat_v(item): - if not item: - return None - return float(item) > 3.2 - - @col_check(3) -def check_bat_p(item): +def check_bat(item): if not item: return None - return float(item) > 30 - + return int(item.split(' ')[1][:-1]) > 30 @col_check(4) def check_sys_status(item): @@ -289,29 +284,40 @@ def check_sys_status(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 True + + +@col_check(7) def check_selfcheck(item): if not item: return None return item == "OK" -@col_check(7) +@col_check(8) def check_pos_status(item): if not item: return None - return str(item).split(' ')[0] != 'nan' + return item.split(' ')[0] != 'nan' or item.split(' ')[0] != 'NO_POS' + +@col_check(9) +def check_start_pos_status(item): + if not item: + return None + return str(item).split(' ')[0] != 'NO_POS' -@col_check(8) +@col_check(10) def check_time_delta(item): if not item: return None @@ -334,7 +340,7 @@ def takeoff_checks(copter_item): def flip_checks(copter_item): for col in Checks.takeoff_checklist: - if col != 4: + if col != 4 or col != 7: if not Checks.all_checks[col](copter_item[col]): return False else: From 0beb6ca69ae8f45120053d0a9df8ea13cc0926d5 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 17:18:01 +0300 Subject: [PATCH 22/47] Server: Handle new data, fix getting copter client data, remove old requests --- Server/server_qt.py | 81 ++++++--------------------------------------- 1 file changed, 11 insertions(+), 70 deletions(-) diff --git a/Server/server_qt.py b/Server/server_qt.py index b6f819b..370fea7 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -166,52 +166,6 @@ class MainWindow(QtWidgets.QMainWindow): self.ui.takeoff_button.setEnabled(False) self.ui.flip_button.setEnabled(False) - @pyqtSlot() - def selfcheck_selected_old(self): - for copter_data_row in self.model.user_selected(): - client = copter_data_row.client - - client.get_response("anim_id", self.set_copter_data, callback_args=(1, copter_data_row)) - client.get_response("batt_voltage", self.set_copter_data, callback_args=(2, copter_data_row)) - client.get_response("cell_voltage", self.set_copter_data, callback_args=(3, copter_data_row)) - client.get_response("sys_status", self.set_copter_data, callback_args=(4, copter_data_row)) - client.get_response("cal_status", self.set_copter_data, callback_args=(5, copter_data_row)) - client.get_response("selfcheck", self.set_copter_data, callback_args=(6, copter_data_row)) - client.get_response("position", self.set_copter_data, callback_args=(7, copter_data_row)) - client.get_response("time", self.set_copter_data, callback_args=(8, copter_data_row)) - - - def set_copter_data(self, value, col, copter_data_row): - row = self.model.get_row_index(copter_data_row) - if row is None: - logging.error("No such client!") - return - - if col == 1: - data = value - elif col == 2: - data = "{}".format(round(float(value), 3)) - elif col == 3: - batt_percent = ((float(value) - 3.2) / (4.2 - 3.2)) * 100 # TODO config - data = "{}".format(round(batt_percent, 3)) - elif col == 4: - data = str(value) - elif col == 5: - data = str(value) - elif col == 6: - data = value - elif col == 7: - data = str(value) - elif col == 8: - data = "{}".format(round(float(value) - time.time(), 3)) - if abs(float(data)) > 1: - copter_data_row.client.send_message("repair_chrony") - else: - logging.error("No column matched for response") - return - - self.signals.update_data_signal.emit(row, col, data, ModelDataRole) - @pyqtSlot() def selfcheck_selected(self): for copter_data_row in self.model.user_selected(): @@ -221,7 +175,6 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot(str) def update_table_data(self, message): 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] @@ -236,27 +189,16 @@ class MainWindow(QtWidgets.QMainWindow): 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, 1, git_version, ModelDataRole) + self.signals.update_data_signal.emit(row, 2, animation_id, ModelDataRole) + self.signals.update_data_signal.emit(row, 3, "{}V {}%".format(battery_v, 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 - # row = self.model.get_row_index(copter_data_row) - # if row is None: - # logging.error("No such client!") - # return - # logging.info("SET COPTER ID TO {}".format(value)) - # - # copter_data_row.client.copter_id = value - # self.signals.update_data_signal.emit(row, col, value, ModelDataRole) - # self.signals.update_data_signal.emit(row, col, True, ModelStateRole) + 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, "{}".format(round(float(copter_time) - time.time(), 3)), ModelDataRole) @pyqtSlot(QtCore.QModelIndex) def selfcheck_info_dialog(self, index): @@ -400,13 +342,12 @@ class MainWindow(QtWidgets.QMainWindow): print("Selected directory:", path) files = [file for file in glob.glob(path + '/*.csv')] names = [os.path.basename(file).split(".")[0] for file in files] - # print(files) for file, name in zip(files, names): for copter in self.model.user_selected(): if name == copter.copter_id: copter.client.send_file(file, "animation.csv") # TODO config else: - print("Filename has no matches with any drone selected") + logging.info("Filename has no matches with any drone selected") @pyqtSlot() def send_calibrations(self): @@ -422,7 +363,7 @@ class MainWindow(QtWidgets.QMainWindow): if name == copter.copter_id: copter.client.send_file(file, "/home/pi/catkin_ws/src/clever/clever/camera_info/calibration.yaml") else: - print("Filename has no matches with any drone selected") + logging.info("Filename has no matches with any drone selected") @pyqtSlot() def send_configurations(self): @@ -435,7 +376,7 @@ class MainWindow(QtWidgets.QMainWindow): for section in sendable_config.sections(): for option in dict(sendable_config.items(section)): value = sendable_config[section][option] - logging.debug("Got item from config:".format(section, option, value)) + logging.debug("Got item from config: {} {} {}".format(section, option, value)) options.append(ConfigOption(section, option, value)) for copter in self.model.user_selected(): From 1053ffe5bb558590d1f54c878929736980c16e07 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 17:18:29 +0300 Subject: [PATCH 23/47] Server: Update GUI --- Server/server_gui.py | 7 ++++--- Server/server_gui.ui | 9 ++++++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/Server/server_gui.py b/Server/server_gui.py index 4743936..e577ef7 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, 761) + MainWindow.resize(1360, 761) self.centralwidget = QtWidgets.QWidget(MainWindow) self.centralwidget.setEnabled(True) self.centralwidget.setObjectName("centralwidget") @@ -35,9 +35,10 @@ class Ui_MainWindow(object): self.tableView.setWordWrap(True) self.tableView.setObjectName("tableView") self.tableView.horizontalHeader().setCascadingSectionResizes(False) - self.tableView.horizontalHeader().setDefaultSectionSize(125) + self.tableView.horizontalHeader().setDefaultSectionSize(50) self.tableView.horizontalHeader().setMinimumSectionSize(50) self.tableView.horizontalHeader().setStretchLastSection(True) + self.tableView.verticalHeader().setVisible(False) self.horizontalLayout.addWidget(self.tableView) self.verticalLayout = QtWidgets.QVBoxLayout() self.verticalLayout.setSizeConstraint(QtWidgets.QLayout.SetMaximumSize) @@ -186,7 +187,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, 1360, 25)) self.menubar.setObjectName("menubar") self.menuOptions = QtWidgets.QMenu(self.menubar) self.menuOptions.setObjectName("menuOptions") diff --git a/Server/server_gui.ui b/Server/server_gui.ui index bb58ced..e391a1b 100644 --- a/Server/server_gui.ui +++ b/Server/server_gui.ui @@ -6,7 +6,7 @@ 0 0 - 1220 + 1360 761 @@ -50,7 +50,7 @@ false - 125 + 50 50 @@ -58,6 +58,9 @@ true + + false + @@ -375,7 +378,7 @@ 0 0 - 1220 + 1360 25 From e50b33611bf1bf918c563f2ad9785247cf1701af Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 14:21:16 +0000 Subject: [PATCH 24/47] Client: Update battery status format --- Drone/copter_client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 617d27b..2c54d9f 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -635,7 +635,7 @@ def telemetry_loop(): batt_empty = batt_empty_param.value.real batt_charged = batt_charged_param.value.real batt_cells = batt_cells_param.value.integer - telemetry = telemetry._replace(battery_p = '{:.2f}'.format(min((ros_telemetry.voltage/batt_cells - batt_empty)/(batt_charged - batt_empty)*100., 100))) + telemetry = telemetry._replace(battery_p = '{}'.format(int(min((ros_telemetry.voltage/batt_cells - batt_empty)/(batt_charged - batt_empty)*100., 100)))) else: telemetry = telemetry._replace(battery_p = 'nan') telemetry = telemetry._replace(calibration_status = get_calibration_status()) From 0b23fd6e532caaffd1dd72975a628a89edbbaf97 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 18:53:24 +0300 Subject: [PATCH 25/47] Server: Modify updating table data --- Server/copter_table_models.py | 7 +++++-- Server/server_qt.py | 9 +++++++-- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index d011b76..1b48f36 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -276,7 +276,10 @@ def check_anim(item): def check_bat(item): if not item: return None - return int(item.split(' ')[1][:-1]) > 30 + if item == "NO_INFO": + return False + else: + return float(item.split(' ')[1][:-1]) > 30 @col_check(4) def check_sys_status(item): @@ -294,7 +297,7 @@ def check_cal_status(item): def check_mode(item): if not item: return None - return True + return (item != "NO_FCU") and not ("CMODE" in item) @col_check(7) diff --git a/Server/server_qt.py b/Server/server_qt.py index 370fea7..cbae2b5 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -181,6 +181,10 @@ class MainWindow(QtWidgets.QMainWindow): 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] @@ -188,17 +192,18 @@ class MainWindow(QtWidgets.QMainWindow): 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, "{}V {}%".format(battery_v, battery_p), 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, "{}".format(round(float(copter_time) - time.time(), 3)), ModelDataRole) + self.signals.update_data_signal.emit(row, 10, time_delta, ModelDataRole) @pyqtSlot(QtCore.QModelIndex) def selfcheck_info_dialog(self, index): From e26f7a861a5938617077a15627ef5ad048f8e84f Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 19:12:14 +0300 Subject: [PATCH 26/47] copter_table_models: Update position column check --- Server/copter_table_models.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index 1b48f36..dd669bf 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -311,7 +311,7 @@ def check_selfcheck(item): def check_pos_status(item): if not item: return None - return item.split(' ')[0] != 'nan' or item.split(' ')[0] != 'NO_POS' + return item.split(' ')[0] != 'nan' and item.split(' ')[0] != 'NO_POS' @col_check(9) def check_start_pos_status(item): From 4f59f397fe885031f26361b6218871c1b5d0ca76 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 15:56:05 +0000 Subject: [PATCH 27/47] mavros_mavlink: Update get_calibration_status --- Drone/mavros_mavlink.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Drone/mavros_mavlink.py b/Drone/mavros_mavlink.py index e5dede5..feded7c 100644 --- a/Drone/mavros_mavlink.py +++ b/Drone/mavros_mavlink.py @@ -85,7 +85,10 @@ def get_calibration_status(): if acc_status.value.integer == 0 and acc_status.success: status_text += "acc: uncalibrated; " if status_text == "": - status_text = "OK" + if not gyro_status.success or not mag_status.success or not acc_status.success: + status_text = "NO_INFO" + else: + status_text = "OK" else: status_text = status_text[:-2] return status_text From a72c447209b33edac65373427fb63c6748f78abd Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 1 Nov 2019 16:13:35 +0000 Subject: [PATCH 28/47] Client: Modify floor frame broadcast --- Drone/copter_client.py | 54 ++++++++++++++++++++++++------------------ 1 file changed, 31 insertions(+), 23 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 2c54d9f..dded96a 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -6,6 +6,7 @@ import datetime import logging import threading import subprocess +import ConfigParser from collections import namedtuple from FlightLib import FlightLib @@ -41,6 +42,7 @@ logger = logging.getLogger(__name__) class CopterClient(client.Client): def load_config(self): + self.FLOOR_FRAME_EXISTS = False super(CopterClient, self).load_config() self.TELEM_FREQ = self.config.getfloat('TELEMETRY', 'frequency') self.TELEM_TRANSMIT = self.config.getboolean('TELEMETRY', 'transmit') @@ -65,7 +67,18 @@ class CopterClient(client.Client): self.Z0 = self.config.getfloat('PRIVATE', 'z0') self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds') self.LED_PIN = self.config.getint('PRIVATE', 'led_pin') - + try: + self.FLOOR_DX = self.config.getfloat('FLOOR FRAME', 'x') + self.FLOOR_DY = self.config.getfloat('FLOOR FRAME', 'y') + self.FLOOR_DZ = self.config.getfloat('FLOOR FRAME', 'z') + self.FLOOR_ROLL = self.config.getfloat('FLOOR FRAME', 'roll') + self.FLOOR_PITCH = self.config.getfloat('FLOOR FRAME', 'pitch') + self.FLOOR_YAW = self.config.getfloat('FLOOR FRAME', 'yaw') + self.FLOOR_PARENT = self.config.get('FLOOR FRAME', 'parent') + self.FLOOR_FRAME_EXISTS = True + except ConfigParser.Error: + logger.error("No floor frame!") + self.FLOOR_FRAME_EXISTS = False self.RESTART_DHCPCD = self.config.getboolean('PRIVATE', 'restart_dhcpcd') def on_broadcast_bind(self): @@ -79,32 +92,25 @@ class CopterClient(client.Client): LedLib.init_led(self.LED_PIN) task_manager_instance.start() if self.FRAME_ID == "floor": - try: - self.FLOOR_DX = self.config.getfloat('FLOOR FRAME', 'x') - self.FLOOR_DY = self.config.getfloat('FLOOR FRAME', 'y') - self.FLOOR_DZ = self.config.getfloat('FLOOR FRAME', 'z') - self.FLOOR_ROLL = self.config.getfloat('FLOOR FRAME', 'roll') - self.FLOOR_PITCH = self.config.getfloat('FLOOR FRAME', 'pitch') - self.FLOOR_YAW = self.config.getfloat('FLOOR FRAME', 'yaw') - self.FLOOR_PARENT = self.config.get('FLOOR FRAME', 'parent') - except Exception as e: - raise Exception("Can't make floor frame!") - quit() + if self.FLOOR_FRAME_EXISTS: + self.start_floor_frame_broadcast() else: - trans = TransformStamped() - trans.transform.translation.x = self.FLOOR_DX - trans.transform.translation.y = self.FLOOR_DY - trans.transform.translation.z = self.FLOOR_DZ - trans.transform.rotation = Quaternion(*quaternion_from_euler(math.radians(self.FLOOR_ROLL), - math.radians(self.FLOOR_PITCH), - math.radians(self.FLOOR_YAW))) - trans.header.frame_id = self.FLOOR_PARENT - trans.child_frame_id = self.FRAME_ID - static_bloadcaster.sendTransform(trans) + logger.error("Can't make floor frame!") start_subscriber() telemetry_thread.start() super(CopterClient, self).start() + def start_floor_frame_broadcast(self): + trans = TransformStamped() + trans.transform.translation.x = self.FLOOR_DX + trans.transform.translation.y = self.FLOOR_DY + trans.transform.translation.z = self.FLOOR_DZ + trans.transform.rotation = Quaternion(*quaternion_from_euler(math.radians(self.FLOOR_ROLL), + math.radians(self.FLOOR_PITCH), + math.radians(self.FLOOR_YAW))) + trans.header.frame_id = self.FLOOR_PARENT + trans.child_frame_id = self.FRAME_ID + static_bloadcaster.sendTransform(trans) def restart_service(name): os.system("systemctl restart {}".format(name)) @@ -407,7 +413,9 @@ def _command_reboot(*args, **kwargs): @messaging.message_callback("service_restart") def _command_service_restart(*args, **kwargs): - restart_service(kwargs["name"]) + service = kwargs["name"] + restart_service(service) + @messaging.message_callback("repair_chrony") def _command_chrony_repair(*args, **kwargs): From 576e1a52fad6a510d2f250772828bbfce51e85cc Mon Sep 17 00:00:00 2001 From: artem30801 <38689676+artem30801@users.noreply.github.com> Date: Sat, 2 Nov 2019 00:29:06 +0300 Subject: [PATCH 29/47] Feature branch - fix for removing clients from qt table (#54) Fixed removing clients from table. --- Server/server_config.ini | 2 +- Server/server_gui.py | 1 + Server/server_gui.ui | 1 + Server/server_qt.py | 17 +++++++--- builder/assets/clever-show.service | 4 ++- messaging_lib.py | 54 ++++++++++++++++++++---------- 6 files changed, 56 insertions(+), 23 deletions(-) diff --git a/Server/server_config.ini b/Server/server_config.ini index 8fdfddc..96a835e 100644 --- a/Server/server_config.ini +++ b/Server/server_config.ini @@ -1,7 +1,7 @@ [SERVER] port = 25000 buffer_size = 1024 -remove_disconnected = False +remove_disconnected = True [BROADCAST] use_broadcast = True diff --git a/Server/server_gui.py b/Server/server_gui.py index e577ef7..09f2a17 100644 --- a/Server/server_gui.py +++ b/Server/server_gui.py @@ -270,6 +270,7 @@ class Ui_MainWindow(object): self.menuDrone.addAction(self.action_remove_row) 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) diff --git a/Server/server_gui.ui b/Server/server_gui.ui index e391a1b..e1b5831 100644 --- a/Server/server_gui.ui +++ b/Server/server_gui.ui @@ -428,6 +428,7 @@ + diff --git a/Server/server_qt.py b/Server/server_qt.py index cbae2b5..298bce8 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -113,14 +113,17 @@ class MainWindow(QtWidgets.QMainWindow): self.signals.add_client_signal.emit(StatedCopterData(copter_id=client.copter_id, client=client)) def client_connection_changed(self, client: Client): + print("removeee") row_data = self.model.get_row_by_attr("client", client) row_num = self.model.get_row_index(row_data) + print("removing") 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: self.signals.update_data_signal.emit(row_num, 0, client.connected, ModelStateRole) + print("removed") def init_ui(self): # Connecting @@ -229,10 +232,16 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def remove_selected(self): for copter in self.model.user_selected(): - row_num = self.model.data_contents.index(copter) - copter.client.remove() - self.signals.remove_client_signal.emit(row_num) - logging.info("Client removed from table!") + row_num = self.model.get_row_index(copter) + if row_num is not None: + 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!") @pyqtSlot() @confirmation_required("This operation will takeoff selected copters with delay and start animation. Proceed?") diff --git a/builder/assets/clever-show.service b/builder/assets/clever-show.service index 33d65a1..e6b96aa 100644 --- a/builder/assets/clever-show.service +++ b/builder/assets/clever-show.service @@ -1,6 +1,8 @@ [Unit] Description=Clever Show Client Service -After=clever.service +Requires=clever.service +Requires=network.target +After=network.target [Service] WorkingDirectory=/home/pi/clever-show/Drone diff --git a/messaging_lib.py b/messaging_lib.py index 878c76f..d8d02fc 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -20,6 +20,7 @@ except ImportError: PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on", "callback", "callback_args", "callback_kwargs", + "request_args", "resend", ]) logger = logging.getLogger(__name__) @@ -213,6 +214,7 @@ class ConnectionManager(object): self.BUFFER_SIZE = 1024 self.resume_queue = False + self.resend_requests = True def _set_selector_events_mask(self, mode): """Set selector to listen for events: mode is 'r', 'w', 'rw'.""" @@ -235,6 +237,8 @@ class ConnectionManager(object): self.addr = client_addr self._set_selector_events_mask('r') + if self.resend_requests: + self._resend_requests() def close(self): with self._close_lock: @@ -245,6 +249,12 @@ 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) @@ -298,7 +308,7 @@ class ConnectionManager(object): self._received_queue[0].income_raw = b'' if self._received_queue: - if self._received_queue[-1].content: + if self._received_queue[0].content: self.process_received(self._received_queue.popleft()) def _read(self): @@ -313,8 +323,6 @@ class ConnectionManager(object): logger.debug("Received {} from {}".format(data, self.addr)) else: logger.warning("Connection to {} lost!".format(self.addr)) - if not self.resume_queue: - self._recv_buffer = b'' raise RuntimeError("Peer closed.") @@ -357,21 +365,20 @@ class ConnectionManager(object): def _process_response(self, message): request_id, requested_value = message.content["request_id"], message.content["requested_value"] + with self._request_lock: - for key, value in self._request_queue.items(): # TODO as try [] - if (key == request_id) and (value.requested_value == requested_value): - request = self._request_queue.pop(key) - value = message.content["value"] - logger.debug( - "Request {} successfully closed with value {}".format(request, message.content["value"]) - ) + request = self._request_queue.pop(request_id, None) - f = request.callback - f(value, *request.callback_args, **request.callback_kwargs) + if (request is not None) and (request.requested_value == requested_value): + value = message.content["value"] + logger.debug( + "Request {} successfully closed with value {}".format(request, message.content["value"]) + ) - break - else: - logger.warning("Unexpected response!") + f = request.callback + f(value, *request.callback_args, **request.callback_kwargs) + else: + logger.warning("Unexpected response!") def _process_filetransfer(self, message): # TODO path? if message.jsonheader["content-type"] == "binary": @@ -418,7 +425,7 @@ class ConnectionManager(object): self._send_queue.append(data) if self.selector.get_key(self.socket).events != selectors.EVENT_WRITE: - self._set_selector_events_mask('w') + self._set_selector_events_mask('rw') NotifierSock().notify() def get_response(self, requested_value, callback, request_args=None, # timeout=30, @@ -437,9 +444,22 @@ class ConnectionManager(object): callback=callback, callback_args=callback_args, callback_kwargs=callback_kwargs, + request_args=request_args, + resend=True, ) self._send(MessageManager.create_request(requested_value, request_id, request_args)) + def _resend_requests(self): + with self._request_lock: + for request_id, request in self._request_queue.items(): + 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() + def send_message(self, command, args=None): self._send(MessageManager.create_simple_message(command, args)) @@ -459,7 +479,7 @@ class ConnectionManager(object): )) -class NotifierSock(Singleton): +class NotifierSock(Singleton): #TODO remake as connecting ONLY to self socket and selector def __init__(self): self.receive_socket = None self.addr = None From e656498478424c18e73872ec0d273a1de1e86c2f Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Sat, 2 Nov 2019 01:16:25 +0000 Subject: [PATCH 30/47] Client: Fix move_start function --- Drone/copter_client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index dded96a..4ec9b9c 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -356,7 +356,7 @@ def _command_move_start_to_current_position(*args, **kwargs): 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): + if not math.isnan(telem.x): 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 b7b7a1ba1b49d7d67f2d91b5df290db129f07fbb Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Sat, 2 Nov 2019 06:46:45 +0000 Subject: [PATCH 31/47] Client: Setup animation logger --- Drone/animation_lib.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index c9913c7..f758207 100644 --- a/Drone/animation_lib.py +++ b/Drone/animation_lib.py @@ -24,7 +24,7 @@ def get_id(filepath="animation.csv"): try: animation_file = open(filepath) except IOError: - logging.error("File {} can't be opened".format(filepath)) + logger.error("File {} can't be opened".format(filepath)) anim_id = "No animation" return anim_id else: @@ -35,17 +35,17 @@ def get_id(filepath="animation.csv"): row_0 = csv_reader.next() if len(row_0) == 1: anim_id = row_0[0] - print("Got animation_id: {}".format(anim_id)) + logger.debug("Got animation_id: {}".format(anim_id)) else: anim_id = "Empty id" - print("No animation id in file") + logger.debug("No animation id in file") return anim_id def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1): try: animation_file = open(filepath) except IOError: - logging.error("File {} can't be opened".format(filepath)) + logger.error("File {} can't be opened".format(filepath)) anim_id = "No animation" return float('nan'), float('nan') else: @@ -59,14 +59,14 @@ def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1): return float('nan'), float('nan') if len(row_0) == 1: anim_id = row_0[0] - print("Got animation_id: {}".format(anim_id)) + logger.debug("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") + logger.debug("No animation id in file") try: frame_number, x, y, z, yaw, red, green, blue = row_0 except: @@ -90,9 +90,9 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_rati row_0 = csv_reader.next() if len(row_0) == 1: anim_id = row_0[0] - print("Got animation_id: {}".format(anim_id)) + logger.debug("Got animation_id: {}".format(anim_id)) else: - print("No animation id in file") + logger.debug("No animation id in file") frame_number, x, y, z, yaw, red, green, blue = row_0 imported_frames.append({ 'number': int(frame_number), From 7181091c16a619e30bf682f5db2e3969e1172a4f Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Sat, 2 Nov 2019 06:51:02 +0000 Subject: [PATCH 32/47] client.py: Remove logging settings --- Drone/client.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index f557081..a52e704 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -17,7 +17,6 @@ current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentfra parent_dir = os.path.dirname(current_dir) sys.path.insert(0, parent_dir) -logging.basicConfig(level=logging.DEBUG) logger = logging.getLogger(__name__) import messaging_lib as messaging @@ -31,7 +30,7 @@ class Client(object): self.selector = selectors.DefaultSelector() self.client_socket = None - self.server_connection = messaging.ConnectionManager() + self.server_connection = messaging.ConnectionManager("pi") self.server_host = None self.server_port = None From 96cb768039bdeffa1b3a46f0310f4ff4d6e664f1 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Sat, 2 Nov 2019 06:51:59 +0000 Subject: [PATCH 33/47] FlightLib: Change message type to logger.debug --- Drone/FlightLib/FlightLib.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index 6d6e28d..1ce4fe3 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -77,7 +77,7 @@ def check(check_name): if msgs: return msgs else: - logger.info("[{}]: OK".format(check_name)) + logger.debug("[{}]: OK".format(check_name)) return None checklist.append(wrapper) From a18429362164428dbe2493ba9c3004dfe57b426f Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Sat, 2 Nov 2019 06:54:25 +0000 Subject: [PATCH 34/47] Client: Setup loggers for animation, messaging, client and tasking lib, fixed telemetry exceptions handle --- Drone/copter_client.py | 115 +++++++++++++++++++++++++---------------- 1 file changed, 71 insertions(+), 44 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 4ec9b9c..94284c0 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -1,4 +1,5 @@ import os +import sys import time import math import rospy @@ -28,17 +29,42 @@ 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") 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 -# format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s", -# handlers=[ -# logging.StreamHandler(), -# ]) +logging.basicConfig( # TODO all prints as logs + level=logging.DEBUG, # INFO + stream=sys.stdout, + format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s", + handlers=[ + logging.StreamHandler(sys.stdout), + ]) + +handler = logging.StreamHandler(sys.stdout) +handler.setLevel(logging.DEBUG) +formatter = logging.Formatter("%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s") +handler.setFormatter(formatter) logger = logging.getLogger(__name__) +logger.setLevel(logging.DEBUG) +logger.addHandler(handler) +animation_logger = logging.getLogger('animation_lib') +animation_logger.setLevel(logging.INFO) +animation_logger.addHandler(handler) -# import ros_logging +client_logger = logging.getLogger('client') +client_logger.setLevel(logging.DEBUG) +client_logger.addHandler(handler) + +messaging_logger = logging.getLogger('messaging_lib') +messaging_logger.setLevel(logging.INFO) +messaging_logger.addHandler(handler) + +tasking_logger = logging.getLogger('tasking_lib') +tasking_logger.setLevel(logging.INFO) +tasking_logger.addHandler(handler) + +flightlib_logger = logging.getLogger('FlightLib') +flightlib_logger.setLevel(logging.INFO) +flightlib_logger.addHandler(handler) class CopterClient(client.Client): def load_config(self): @@ -77,7 +103,7 @@ class CopterClient(client.Client): self.FLOOR_PARENT = self.config.get('FLOOR FRAME', 'parent') self.FLOOR_FRAME_EXISTS = True except ConfigParser.Error: - logger.error("No floor frame!") + rospy.logerror("No floor frame!") self.FLOOR_FRAME_EXISTS = False self.RESTART_DHCPCD = self.config.getboolean('PRIVATE', 'restart_dhcpcd') @@ -86,7 +112,7 @@ class CopterClient(client.Client): restart_service("chrony") def start(self, task_manager_instance): - client.logger.info("Init ROS node") + rospy.loginfo("Init ROS node") rospy.init_node('clever_show_client') if self.USE_LEDS: LedLib.init_led(self.LED_PIN) @@ -95,7 +121,7 @@ class CopterClient(client.Client): if self.FLOOR_FRAME_EXISTS: self.start_floor_frame_broadcast() else: - logger.error("Can't make floor frame!") + rospy.logerror("Can't make floor frame!") start_subscriber() telemetry_thread.start() super(CopterClient, self).start() @@ -123,7 +149,7 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1): with open(path, 'r') as f: raw_content = f.read() except IOError as e: - print("Reading error {}".format(e)) + logger.error("Reading error {}".format(e)) return False content = raw_content.split(" ") @@ -131,11 +157,11 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1): try: current_ip = content[ip_index] except IndexError: - print("Something wrong with config") + logger.error("Something wrong with config") return False if "." not in current_ip: - print("That's not ip!") + logger.debug("That's not ip!") return False if current_ip != ip: @@ -145,7 +171,7 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1): with open(path, 'w') as f: f.write(" ".join(content)) except IOError: - print("Error writing") + logger.error("Error writing") return False return True @@ -157,7 +183,7 @@ def configure_hostname(hostname): with open(path, 'r') as f: raw_content = f.read() except IOError as e: - print("Reading error {}".format(e)) + logger.error("Reading error {}".format(e)) return False current_hostname = str(raw_content) @@ -168,7 +194,7 @@ def configure_hostname(hostname): with open(path, 'w') as f: f.write(content) except IOError: - print("Error writing") + logger.error("Error writing") return False return True @@ -180,7 +206,7 @@ def configure_hosts(hostname): with open(path, 'r') as f: raw_content = f.read() except IOError as e: - print("Reading error {}".format(e)) + logger.error("Reading error {}".format(e)) return False index_start = raw_content.find("127.0.1.1", ) @@ -193,7 +219,7 @@ def configure_hosts(hostname): with open(path, 'w') as f: f.write(content) except IOError: - print("Error writing") + logger.error("Error writing") return False return True @@ -208,7 +234,7 @@ def configure_bashrc(hostname): with open(path, 'r') as f: raw_content = f.read() except IOError as e: - print("Reading error {}".format(e)) + logger.error("Reading error {}".format(e)) return False index_start = raw_content.find("ROS_HOSTNAME='", ) + 14 @@ -221,7 +247,7 @@ def configure_bashrc(hostname): with open(path, 'w') as f: f.write(content) except IOError: - print("Error writing") + logger.error("Error writing") return False return True @@ -276,7 +302,7 @@ def _response_animation_id(*args, **kwargs): # Load animation result = animation.get_id() if result != 'No animation': - print ("Saving corrected animation") + logger.debug ("Saving corrected 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, @@ -290,7 +316,7 @@ def _response_animation_id(*args, **kwargs): check_takeoff=client.active_client.TAKEOFF_CHECK, check_land=client.active_client.LAND_CHECK, ) - print("Start action: {}".format(start_action)) + logger.debug("Start action: {}".format(start_action)) # Save corrected animation animation.save_corrected_animation(corrected_frames) return result @@ -344,6 +370,7 @@ def _calibrate_level(*args, **kwargs): @messaging.message_callback("test") def _command_test(*args, **kwargs): logger.info("logging info test") + rospy.logdebug("ros logdebug test") print("stdout test") @messaging.message_callback("move_start") @@ -352,20 +379,20 @@ def _command_move_start_to_current_position(*args, **kwargs): x_ratio=client.active_client.X_RATIO, y_ratio=client.active_client.Y_RATIO, ) - print("x_start = {}, y_start = {}".format(x_start, y_start)) + logger.debug("x_start = {}, y_start = {}".format(x_start, y_start)) 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)) + logger.debug("x_telem = {}, y_telem = {}".format(telem.x, telem.y)) if not math.isnan(telem.x): 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)) + logger.info ("Set start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0)) else: - print ("Wrong telemetry") + logger.debug ("Wrong telemetry") else: - print("Wrong animation file") + logger.debug("Wrong animation file") @messaging.message_callback("reset_start") def _command_reset_start(*args, **kwargs): @@ -373,7 +400,7 @@ def _command_reset_start(*args, **kwargs): client.active_client.config.set('PRIVATE', 'y0', 0) client.active_client.rewrite_config() client.active_client.load_config() - print ("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0)) + logger.info ("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(*args, **kwargs): @@ -381,14 +408,14 @@ def _command_set_z(*args, **kwargs): client.active_client.config.set('PRIVATE', 'z0', telem.z) client.active_client.rewrite_config() client.active_client.load_config() - print ("Set z offset to {:.2f}".format(client.active_client.Z0)) + logger.info ("Set z offset to {:.2f}".format(client.active_client.Z0)) @messaging.message_callback("reset_z_offset") def _command_reset_z(*args, **kwargs): client.active_client.config.set('PRIVATE', 'z0', 0) client.active_client.rewrite_config() client.active_client.load_config() - print ("Reset z offset to {:.2f}".format(client.active_client.Z0)) + logger.info ("Reset z offset to {:.2f}".format(client.active_client.Z0)) @messaging.message_callback("update_repo") @@ -445,7 +472,7 @@ def _copter_flip(*args, **kwargs): @messaging.message_callback("takeoff") def _command_takeoff(*args, **kwargs): - print("Takeoff at {}".format(datetime.datetime.now())) + logger.info("Takeoff at {}".format(datetime.datetime.now())) task_manager.add_task(0, 0, animation.takeoff, task_kwargs={ "z": client.active_client.TAKEOFF_HEIGHT, @@ -460,7 +487,7 @@ def _command_takeoff_z(*args, **kwargs): z_str = kwargs.get("z", None) if z_str is not None: telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) - print("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now())) + logger.info("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now())) task_manager.add_task(0, 0, FlightLib.reach_point, task_kwargs={ "x": telem.x, @@ -516,12 +543,12 @@ def _play_animation(*args, **kwargs): start_time = float(kwargs["time"]) # Check if animation file is available if animation.get_id() == 'No animation': - print("Can't start animation without animation file!") + logger.error("Can't start animation without animation file!") return task_manager.reset(interrupt_next_task=False) - print("Start time = {}, wait for {} seconds".format(start_time, start_time-time.time())) + logger.info("Start time = {}, wait for {} seconds".format(start_time, start_time-time.time())) # Load animation frames = animation.load_animation(os.path.abspath("animation.csv"), x0=client.active_client.X0 + client.active_client.X0_COMMON, @@ -564,7 +591,6 @@ def _play_animation(*args, **kwargs): frame_time = rfp_time + client.active_client.RFP_TIME elif start_action == 'arm': - print ("Start_time") # Calculate start time start_time += start_delay # Arm @@ -587,6 +613,7 @@ def _play_animation(*args, **kwargs): ) # Calculate first frame start time frame_time += client.active_client.FRAME_DELAY # TODO Think about arming time + logger.debug(task_manager.task_queue) # Play animation file for frame in corrected_frames: point, color, yaw = animation.convert_frame(frame) @@ -611,7 +638,6 @@ def _play_animation(*args, **kwargs): "use_leds": client.active_client.USE_LEDS, }, ) - #print(task_manager.task_queue) def telemetry_loop(): global telemetry @@ -667,14 +693,19 @@ def telemetry_loop(): telemetry = telemetry._replace(selfcheck = 'NO_FCU') telemetry = telemetry._replace(current_position = 'NO_POS') except rospy.ServiceException: - print "Some service is unavailable" + rospy.logdebug("Some service is unavailable") + except AttributeError as e: + rospy.logdebug(e) + except rospy.TransportException as e: + rospy.logdebug(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 Exception as e: - print e + except AttributeError as e: + rospy.logdebug(e) + rate.sleep() def create_telemetry_message(telemetry): @@ -687,11 +718,7 @@ def create_telemetry_message(telemetry): telemetry_thread = threading.Thread(target=telemetry_loop, name="Telemetry getting thread") if __name__ == "__main__": + copter_client = CopterClient() task_manager = tasking.TaskManager() copter_client.start(task_manager) - - # ros_logging.route_logger_to_ros() - # ros_logging.route_logger_to_ros("__main__") - # ros_logging.route_logger_to_ros("client") - # ros_logging.route_logger_to_ros("messaging") From 43390f9eb4101fffbb34528d3def4acb761ed89f Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Sat, 2 Nov 2019 06:56:32 +0000 Subject: [PATCH 35/47] tasking_lib: Setup loggers, fix task waiting bug, modify handling exceptions --- Drone/tasking_lib.py | 33 +++++++++++++++++++++++---------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/Drone/tasking_lib.py b/Drone/tasking_lib.py index 76a5234..6f7edca 100644 --- a/Drone/tasking_lib.py +++ b/Drone/tasking_lib.py @@ -87,7 +87,7 @@ class TaskManager(object): def start(self): #print("Task manager is started") - #logger.info("Task manager is started") + logger.info("Task manager is started") self._processor_thread.start() self.resume() @@ -107,7 +107,7 @@ class TaskManager(object): self._wait_interrupt_event.set() self._task_interrupt_event.set() self._running_event.clear() - #logger.info("Task queue paused") + logger.info("Task queue paused") #print("Task queue paused") def resume(self, time_to_start_next_task=0.0): @@ -118,7 +118,7 @@ class TaskManager(object): self._wait_interrupt_event.clear() self._task_interrupt_event.clear() self._running_event.set() - #logger.info("Task queue resumed with timeshift {}".format(self._timeshift)) + logger.info("Task queue resumed with timeshift {}".format(self._timeshift)) #print("Task queue resumed with timeshift {}".format(self._timeshift)) def reset(self, interrupt_next_task=True): @@ -128,11 +128,13 @@ class TaskManager(object): self._reset_event.set() def execute_task(self): + delta = 0.1 + with self._task_queue_lock: try: start_time, priority, count, task = self.task_queue[0] except Exception as e: - #print("Task queue checking exception: {}".format(e)) + logger.debug("Task queue checking exception: {}".format(e)) self._timeshift = 0.0 self._wait_interrupt_event.clear() self._task_interrupt_event.clear() @@ -140,24 +142,32 @@ class TaskManager(object): return task_start_time = start_time + self._timeshift - #logger.info("Waiting util task execution time:{}".format(task_start_time)) + logger.info("Executing task {}".format(task.func.__name__)) + logger.debug("Waiting util task execution time:{}".format(task_start_time)) #print("Waiting until task execution time:{}".format(task_start_time)) wait(task_start_time, self._wait_interrupt_event) + if task_start_time - time.time() > 0.01: + logger.error("Need for waiting more") + self._wait_interrupt_event.clear() + return + if not self._wait_interrupt_event.is_set(): #logger.info("Executing task {}".format(task)) #print("{} Executing task {}".format(time.time(),task)) #print("Interrupter is set: {}".format(self._task_interrupt_event.is_set())) try: task.func(*task.args, interrupter=self._task_interrupt_event, **task.kwargs) + except Exception as e: logger.error("Error '{}' occurred in task {}".format(e, task)) #print("Error '{}' occurred in task {}".format(e, task)) if str(e) == 'STOP': self.reset() + logger.error("Return after STOP exception, can't arm!") return else: - #logger.warning("Task interrupted before execution") + logger.error("Task interrupted before execution") #print("Task interrupted before execution") self._wait_interrupt_event.clear() return @@ -166,14 +176,17 @@ class TaskManager(object): try: start_time_n, priority_n, count_n, task_n = self.task_queue[0] except Exception as e: - #print("Timeout checking exception: {}".format(e)) + logger.warning("Timeout checking exception: {}".format(e)) self._timeshift = 0.0 self._wait_interrupt_event.clear() self._task_interrupt_event.clear() self._running_event.clear() return if (task_n == task) and (start_time_n == start_time): - self.pop_task() + try: + self.pop_task() + except KeyError as e: + logger.error(str(e)) #try: #print("Pop {} function!".format(task.func.__name__)) #except Exception as e: @@ -182,11 +195,11 @@ class TaskManager(object): if self._task_interrupt_event.is_set(): self._task_interrupt_event.clear() - #logger.info("Execution done") + logger.info("Execution done") #print("Execution done") def _task_processor(self): - #logger.info("Tasking thread started") + logger.info("Tasking thread started") # self._update_queue() # Initial tick if tasks added before start while not self._shutdown_event.is_set(): self._running_event.wait() From 9f7fda0b161b007cd74c8b0684387131722cd3dc Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Sat, 2 Nov 2019 06:59:32 +0000 Subject: [PATCH 36/47] messaging: add whoami option --- messaging_lib.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/messaging_lib.py b/messaging_lib.py index d8d02fc..9b1074b 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -5,6 +5,7 @@ import json import socket import struct import random +import inspect import logging import threading import collections @@ -34,7 +35,7 @@ def get_ip_address(): ip_socket.connect(("8.8.8.8", 80)) return ip_socket.getsockname()[0] except OSError: - logging.warning("No network connection detected, using localhost") + logger.warning("No network connection detected, using localhost") return "localhost" @@ -194,7 +195,7 @@ class ConnectionManager(object): messages_callbacks = {} requests_callbacks = {} - def __init__(self): + def __init__(self, whoami = "computer"): self.selector = None self.socket = None self.addr = None @@ -204,6 +205,8 @@ class ConnectionManager(object): self._recv_buffer = b"" self._send_buffer = b"" + self.whoami = whoami + self._send_queue = collections.deque() self._received_queue = collections.deque() self._request_queue = collections.OrderedDict() @@ -228,7 +231,7 @@ class ConnectionManager(object): raise ValueError("Invalid events mask mode {}.".format(mode)) key = self.selector.modify(self.socket, events, data=self) - logging.debug("Switched selector of {} to mode {}".format(self.addr, key.events)) + logger.debug("Switched selector of {} to mode {}".format(self.addr, key.events)) return key def connect(self, client_selector, client_socket, client_addr): @@ -390,7 +393,9 @@ 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 pi:pi {}".format(filepath)) + if self.whoami == "pi": + logger.info("Return rights to pi:pi after file transfer") + os.system("chown pi:pi {}".format(filepath)) def write(self): with self._send_lock: From d491a454686cd329570a73784aa01bdd8f52c4bf Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Tue, 5 Nov 2019 21:14:17 +0300 Subject: [PATCH 37/47] Server: Update table headers --- 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 dd669bf..f5b83da 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -74,8 +74,8 @@ class CopterDataModel(QtCore.QAbstractTableModel): def __init__(self, parent=None): super(CopterDataModel, self).__init__(parent) - self.headers = ('copter ID', 'version', ' animation ID ', ' battery ', ' system ', 'calibration', - ' mode ', 'selfcheck', 'current x y z yaw frame_id', ' start x y z ', 'dt') + 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 From c1f6219af31f6f896b662d799da54d5c05b87a1e Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Tue, 5 Nov 2019 21:30:46 +0300 Subject: [PATCH 38/47] Server: Fix logging messages for copter removing --- Server/server_qt.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Server/server_qt.py b/Server/server_qt.py index 298bce8..2de6411 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -113,17 +113,17 @@ class MainWindow(QtWidgets.QMainWindow): self.signals.add_client_signal.emit(StatedCopterData(copter_id=client.copter_id, client=client)) def client_connection_changed(self, client: Client): - print("removeee") + logging.debug("Start remove {}".format(client.copter_id)) row_data = self.model.get_row_by_attr("client", client) row_num = self.model.get_row_index(row_data) - print("removing") + 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: self.signals.update_data_signal.emit(row_num, 0, client.connected, ModelStateRole) - print("removed") + logging.debug("{} removed".format(client.copter_id)) def init_ui(self): # Connecting From bc4c4644e0b281d5d326d3a224da725c5928220d Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Tue, 5 Nov 2019 18:55:03 +0000 Subject: [PATCH 39/47] Client: replace FlightLib.get_telemetry to direct get_telemetry service proxy to resolve proxy service from imported module error --- Drone/copter_client.py | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 94284c0..ba8738c 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -3,6 +3,7 @@ import sys import time import math import rospy +from clever import srv import datetime import logging import threading @@ -29,6 +30,8 @@ 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") telemetry = Telemetry('nan', 'No animation', 'nan', 'nan', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_POS', 'NO_POS') +get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) + logging.basicConfig( # TODO all prints as logs level=logging.DEBUG, # INFO stream=sys.stdout, @@ -324,7 +327,7 @@ def _response_animation_id(*args, **kwargs): @messaging.request_callback("batt_voltage") def _response_batt(*args, **kwargs): if check_state_topic(wait_new_status=True): - return FlightLib.get_telemetry('body').voltage + return get_telemetry('body').voltage else: stop_subscriber() return float('nan') @@ -333,7 +336,7 @@ def _response_batt(*args, **kwargs): @messaging.request_callback("cell_voltage") def _response_cell(*args, **kwargs): if check_state_topic(wait_new_status=True): - return FlightLib.get_telemetry('body').cell_voltage + return get_telemetry('body').cell_voltage else: stop_subscriber() return float('nan') @@ -352,7 +355,7 @@ def _response_cal_status(*args, **kwargs): @messaging.request_callback("position") def _response_position(*args, **kwargs): - telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) + telem = 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) @@ -381,7 +384,7 @@ def _command_move_start_to_current_position(*args, **kwargs): ) logger.debug("x_start = {}, y_start = {}".format(x_start, y_start)) if not math.isnan(x_start): - telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) + telem = get_telemetry(client.active_client.FRAME_ID) logger.debug("x_telem = {}, y_telem = {}".format(telem.x, telem.y)) if not math.isnan(telem.x): client.active_client.config.set('PRIVATE', 'x0', telem.x - x_start) @@ -404,7 +407,7 @@ def _command_reset_start(*args, **kwargs): @messaging.message_callback("set_z_to_ground") def _command_set_z(*args, **kwargs): - telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) + telem = get_telemetry(client.active_client.FRAME_ID) client.active_client.config.set('PRIVATE', 'z0', telem.z) client.active_client.rewrite_config() client.active_client.load_config() @@ -486,7 +489,7 @@ def _command_takeoff(*args, **kwargs): def _command_takeoff_z(*args, **kwargs): z_str = kwargs.get("z", None) if z_str is not None: - telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) + telem = get_telemetry(client.active_client.FRAME_ID) logger.info("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now())) task_manager.add_task(0, 0, FlightLib.reach_point, task_kwargs={ @@ -659,7 +662,7 @@ def telemetry_loop(): services_unavailable = FlightLib.check_ros_services_unavailable() if not services_unavailable: try: - ros_telemetry = FlightLib.get_telemetry(client.active_client.FRAME_ID) + ros_telemetry = get_telemetry(client.active_client.FRAME_ID) if ros_telemetry.connected: telemetry = telemetry._replace(battery_v = '{:.2f}'.format(ros_telemetry.voltage)) batt_empty_param = get_param('BAT_V_EMPTY') @@ -693,18 +696,18 @@ def telemetry_loop(): telemetry = telemetry._replace(selfcheck = 'NO_FCU') telemetry = telemetry._replace(current_position = 'NO_POS') except rospy.ServiceException: - rospy.logdebug("Some service is unavailable") + logger.debug("Some service is unavailable") except AttributeError as e: - rospy.logdebug(e) + logger.debug(e) except rospy.TransportException as e: - rospy.logdebug(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: - rospy.logdebug(e) + logger.debug(e) rate.sleep() From 34ad8bd5ef0fed8e32c64508f68dded119dbd723 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 5 Nov 2019 22:58:59 +0300 Subject: [PATCH 40/47] Update README.md --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index cbde450..7ec58c2 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,5 @@ # clever-show + [Русская версия](README_RU.md) Software for making the drone show controlled by Raspberry Pi and COEX [Clever](https://github.com/CopterExpress/clever) package. @@ -6,16 +7,16 @@ Software for making the drone show controlled by Raspberry Pi and COEX [Clever]( [![Build Status](https://travis-ci.org/CopterExpress/clever-show.svg?branch=master)](https://travis-ci.org/CopterExpress/clever-show) ## This software includes + * [Drone side](https://github.com/CopterExpress/clever-show/tree/master/Drone) with autonomous flight module, animation player module and client application for remote synchronized control of drones * [Server side](https://github.com/CopterExpress/clever-show/tree/master/Server) for making the drone show with ability of tuning drones, animation and music * [Blender 2.8 addon](https://github.com/CopterExpress/clever-show/tree/master/blender-addon) for animation export to drone paths * [Raspberry Pi image](https://github.com/CopterExpress/clever-show/releases/latest) for quick launch software on the drones ## Documentation + > Documentation is available only in Russian for now. Start tutorial is located [here](docs/ru/start-tutorial.md). Detailed documentation is located in the [docs](https://github.com/CopterExpress/clever-show/tree/master/docs) folder. - - From 4db18395eb20b0c90b56b4302995f06fea696e13 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Tue, 5 Nov 2019 22:49:40 +0000 Subject: [PATCH 41/47] Client: Catch ValueError when count battery percentage --- Drone/copter_client.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index ba8738c..f2ac110 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -672,7 +672,10 @@ def telemetry_loop(): batt_empty = batt_empty_param.value.real batt_charged = batt_charged_param.value.real batt_cells = batt_cells_param.value.integer - telemetry = telemetry._replace(battery_p = '{}'.format(int(min((ros_telemetry.voltage/batt_cells - batt_empty)/(batt_charged - batt_empty)*100., 100)))) + 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()) From 5328d7c20b575562fe84f12f33e8482eabc8b1df Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 7 Nov 2019 05:12:17 +0000 Subject: [PATCH 42/47] Client: Add threading lock for get_telemetry service --- Drone/FlightLib/FlightLib.py | 36 ++++++++++++++++++++---------------- Drone/copter_client.py | 16 ++++++++-------- 2 files changed, 28 insertions(+), 24 deletions(-) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index 1ce4fe3..abf8e59 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -43,7 +43,11 @@ INTERRUPTER = threading.Event() FLIP_MIN_Z = 2.0 checklist = [] +get_telemetry_lock = threading.Lock() +def get_telemetry_locked(*args, **kwargs): + with get_telemetry_lock: + return get_telemetry(*args, **kwargs) def arming_wrapper(state=False, *args, **kwargs): arming(state) @@ -109,14 +113,14 @@ def check_ros_services(): @check("FCU connection") def check_connection(): - telemetry = get_telemetry() + telemetry = get_telemetry_locked() if not telemetry.connected: yield ("Flight controller is not connected!") @check("Linear velocity estimation") def check_linear_speeds(speed_limit=0.15): - telemetry = get_telemetry(frame_id='body') + telemetry = get_telemetry_locked(frame_id='body') if _check_nans(telemetry.vx, telemetry.vy, telemetry.vz): yield ("Velocity estimation is not available") @@ -131,7 +135,7 @@ def check_linear_speeds(speed_limit=0.15): @check("Angular velocity estimation") def check_angular_speeds(rate_limit=0.05): - telemetry = get_telemetry(frame_id='body') + telemetry = get_telemetry_locked(frame_id='body') if _check_nans(telemetry.pitch_rate, telemetry.roll_rate, telemetry.yaw_rate): yield ("Angular velocities estimation is not available") @@ -146,7 +150,7 @@ def check_angular_speeds(rate_limit=0.05): @check("Angles estimation") def check_angles(angle_limit=math.radians(5)): - telemetry = get_telemetry(frame_id='body') + telemetry = get_telemetry_locked(frame_id='body') if _check_nans(telemetry.pitch, telemetry.roll, telemetry.yaw): yield ("Angular velocities estimation is not available") @@ -173,7 +177,7 @@ def selfcheck(): def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, auto_arm=False, **kwargs): set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, auto_arm=auto_arm) - #telemetry = get_telemetry(frame_id=frame_id) + #telemetry = get_telemetry_locked(frame_id=frame_id) logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) #print('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) @@ -190,7 +194,7 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed, auto_arm=auto_arm) # waiting for completion - telemetry = get_telemetry(frame_id=frame_id) + telemetry = get_telemetry_locked(frame_id=frame_id) rate = rospy.Rate(freq) time_start = time.time() @@ -201,7 +205,7 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO interrupter.clear() return False - telemetry = get_telemetry(frame_id=frame_id) + telemetry = get_telemetry_locked(frame_id=frame_id) rospy.logdebug('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) #print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( @@ -229,10 +233,10 @@ def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, fr freq=FREQUENCY, timeout=TIMEOUT, interrupter=INTERRUPTER, wait=False): logger.info('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw)) #print('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw)) - current_telem = get_telemetry(frame_id=frame_id) + current_telem = get_telemetry_locked(frame_id=frame_id) navigate(frame_id=frame_id, x=current_telem.x, y=current_telem.y, z=z, yaw=yaw, speed=speed) - telemetry = get_telemetry(frame_id=frame_id) + telemetry = get_telemetry_locked(frame_id=frame_id) rate = rospy.Rate(freq) time_start = time.time() @@ -243,7 +247,7 @@ def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, fr interrupter.clear() return - telemetry = get_telemetry(frame_id=frame_id) + telemetry = get_telemetry_locked(frame_id=frame_id) logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) #print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( @@ -276,7 +280,7 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA landing() #print("Land is started") - telemetry = get_telemetry(frame_id=frame_id_land) + telemetry = get_telemetry_locked(frame_id=frame_id_land) rate = rospy.Rate(freq) time_start = time.time() @@ -287,7 +291,7 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA interrupter.clear() return False - telemetry = get_telemetry(frame_id=frame_id_land) + telemetry = get_telemetry_locked(frame_id=frame_id_land) logger.info("Landing... | z: {}".format(telemetry.z)) #print("Landing... | z: {}".format(telemetry.z)) time_passed = time.time() - time_start @@ -308,7 +312,7 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, timeout_takeoff=TIMEOUT, interrupter=INTERRUPTER, emergency_land=False): rospy.loginfo("Takeoff started...") rate = rospy.Rate(FREQUENCY) - start = get_telemetry(frame_id=frame_id) + start = get_telemetry_locked(frame_id=frame_id) climb = 0. time_start = time.time() result = navigate(z=height, speed=speed, yaw=float('nan'), frame_id="body", auto_arm=True) @@ -322,7 +326,7 @@ def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, fra interrupter.clear() return 'interrupted' - climb = abs(get_telemetry(frame_id=frame_id).z - start.z) + climb = abs(get_telemetry_locked(frame_id=frame_id).z - start.z) rospy.logdebug("Takeoff to {:.2f} of {:.2f} meters".format(climb, height)) time_passed = time.time() - time_start @@ -341,7 +345,7 @@ def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, fra def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different directions logger.info("Flip started!") - start_telemetry = get_telemetry(frame_id=frame_id) # memorize starting position + start_telemetry = get_telemetry_locked(frame_id=frame_id) # memorize starting position if start_telemetry.z < min_z - TOLERANCE: logger.warning("Can't do flip! Flip failed!") @@ -355,7 +359,7 @@ def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different direc set_rates(roll_rate=30, thrust=0.2) # maximum roll rate while True: - telem = get_telemetry() + telem = get_telemetry_locked() if abs(telem.roll) > math.pi/2: break diff --git a/Drone/copter_client.py b/Drone/copter_client.py index f2ac110..bdf6371 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -30,7 +30,7 @@ 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") telemetry = Telemetry('nan', 'No animation', 'nan', 'nan', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_POS', 'NO_POS') -get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) +# get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) logging.basicConfig( # TODO all prints as logs level=logging.DEBUG, # INFO @@ -327,7 +327,7 @@ def _response_animation_id(*args, **kwargs): @messaging.request_callback("batt_voltage") def _response_batt(*args, **kwargs): if check_state_topic(wait_new_status=True): - return get_telemetry('body').voltage + return FlightLib.get_telemetry_locked('body').voltage else: stop_subscriber() return float('nan') @@ -336,7 +336,7 @@ def _response_batt(*args, **kwargs): @messaging.request_callback("cell_voltage") def _response_cell(*args, **kwargs): if check_state_topic(wait_new_status=True): - return get_telemetry('body').cell_voltage + return FlightLib.get_telemetry_locked('body').cell_voltage else: stop_subscriber() return float('nan') @@ -355,7 +355,7 @@ def _response_cal_status(*args, **kwargs): @messaging.request_callback("position") def _response_position(*args, **kwargs): - telem = get_telemetry(client.active_client.FRAME_ID) + telem = FlightLib.get_telemetry_locked(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) @@ -384,7 +384,7 @@ def _command_move_start_to_current_position(*args, **kwargs): ) logger.debug("x_start = {}, y_start = {}".format(x_start, y_start)) if not math.isnan(x_start): - telem = get_telemetry(client.active_client.FRAME_ID) + telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID) logger.debug("x_telem = {}, y_telem = {}".format(telem.x, telem.y)) if not math.isnan(telem.x): client.active_client.config.set('PRIVATE', 'x0', telem.x - x_start) @@ -407,7 +407,7 @@ def _command_reset_start(*args, **kwargs): @messaging.message_callback("set_z_to_ground") def _command_set_z(*args, **kwargs): - telem = get_telemetry(client.active_client.FRAME_ID) + telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID) client.active_client.config.set('PRIVATE', 'z0', telem.z) client.active_client.rewrite_config() client.active_client.load_config() @@ -489,7 +489,7 @@ def _command_takeoff(*args, **kwargs): def _command_takeoff_z(*args, **kwargs): z_str = kwargs.get("z", None) if z_str is not None: - telem = get_telemetry(client.active_client.FRAME_ID) + telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID) logger.info("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now())) task_manager.add_task(0, 0, FlightLib.reach_point, task_kwargs={ @@ -662,7 +662,7 @@ def telemetry_loop(): services_unavailable = FlightLib.check_ros_services_unavailable() if not services_unavailable: try: - ros_telemetry = get_telemetry(client.active_client.FRAME_ID) + ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID) if ros_telemetry.connected: telemetry = telemetry._replace(battery_v = '{:.2f}'.format(ros_telemetry.voltage)) batt_empty_param = get_param('BAT_V_EMPTY') From fc2bfda50871a295c5565a0123c6424cdb2e8420 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 8 Nov 2019 10:34:49 +0300 Subject: [PATCH 43/47] Add tools folder --- tools/cut.py | 100 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 100 insertions(+) create mode 100644 tools/cut.py diff --git a/tools/cut.py b/tools/cut.py new file mode 100644 index 0000000..c81025d --- /dev/null +++ b/tools/cut.py @@ -0,0 +1,100 @@ +import argparse +import os +import csv +import glob +import copy +import logging + +def cut_file(filename, _from, _to): + imported_frames = [] + anim_id = "" + + try: + animation_file = open(filename) + except IOError: + logging.error("File {} can't be opened".format(filepath)) + else: + with animation_file: + csv_reader = csv.reader( + animation_file, delimiter=',', quotechar='|' + ) + row_0 = csv_reader.next() + if len(row_0) == 1: + anim_id = row_0[0] + logging.debug("Got animation_id: {}".format(anim_id)) + else: + logging.debug("No animation id in file") + frame_number, x, y, z, yaw, red, green, blue = row_0 + imported_frames.append({ + 'number': int(frame_number), + 'x': float(x), + 'y': float(y), + 'z': float(z), + 'yaw': float(yaw), + 'red': int(red), + 'green': int(green), + 'blue': int(blue), + }) + for row in csv_reader: + frame_number, x, y, z, yaw, red, green, blue = row + imported_frames.append({ + 'number': int(frame_number), + 'x': float(x), + 'y': float(y), + 'z': float(z), + 'yaw': float(yaw), + 'red': int(red), + 'green': int(green), + 'blue': int(blue), + }) + + if _to == 0 or _to >= len(imported_frames): + _to = len(imported_frames)-1 + + path = '{}/cut_{}_{}'.format(os.path.dirname(filename),_from,_to) + print('Path is {}'.format(path)) + + csv_file = open(path+'/'+os.path.basename(filename), mode='w+') + with csv_file: + csv_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL) + if anim_id != "": + csv_writer.writerow([anim_id]) + for i in range(_from, _to+1): + csv_writer.writerow([imported_frames[i]['number'], imported_frames[i]['x'], imported_frames[i]['y'], imported_frames[i]['z'], + imported_frames[i]['yaw'], imported_frames[i]['red'], imported_frames[i]['green'], imported_frames[i]['blue']]) + + print("Successfully created file {}".format(path+'/'+filename)) + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description="cut animation") + parser.add_argument('directory', nargs='?', default='.', + help="Directory with animation csv files. Default is '.'") + parser.add_argument('-f','--frm', type=int, default=0, + help="Cut from this frame, default is 0 (from the beginning)") + parser.add_argument('-t','--to', type=int, default=0, + help="Cut to this frame (including this one), default is 0 (to the end)") + args = parser.parse_args() + + _from = args.frm + _to = args.to + + path = '{}/cut_{}_{}'.format(args.directory,_from,_to) + + try: + os.mkdir(path) + except OSError: + print("Creation of the directory %s failed" % path) + files = [f for f in glob.glob(args.directory + '/*.csv')] + for f in files: + cut_file(f, _from, _to) + else: + print("Successfully created the directory %s " % path) + + files = [f for f in glob.glob(args.directory + '/*.csv')] + for f in files: + cut_file(f, _from, _to) + + + + From 2a09b6362dfc127376b6e954d55df2ef35289481 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 8 Nov 2019 10:35:34 +0300 Subject: [PATCH 44/47] Server: return remove_disconnected to False again --- Server/server_config.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Server/server_config.ini b/Server/server_config.ini index 96a835e..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 From c2b1e4d299a019d9d99254873cc8808870de25b1 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 8 Nov 2019 10:45:37 +0300 Subject: [PATCH 45/47] Add failsafe and power parameters for clever --- Drone/FCU/clever_failsafe_and_power.params | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 Drone/FCU/clever_failsafe_and_power.params diff --git a/Drone/FCU/clever_failsafe_and_power.params b/Drone/FCU/clever_failsafe_and_power.params new file mode 100644 index 0000000..5f95317 --- /dev/null +++ b/Drone/FCU/clever_failsafe_and_power.params @@ -0,0 +1,5 @@ +1 1 COM_LOW_BAT_ACT 2 6 +1 1 COM_OBL_ACT 0 6 +1 1 COM_OBL_RC_ACT 4 6 +1 1 BAT_V_CHARGED 4.050000190734863281 9 +1 1 BAT_V_EMPTY 3.500000000000000000 9 From f7f227caacd1e5d25119805cbaca786aede5d566 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Sat, 9 Nov 2019 22:10:27 +0300 Subject: [PATCH 46/47] Server: Increase minimum battery percentage to start copters --- Server/copter_table_models.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index f5b83da..c8c21ce 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -279,7 +279,7 @@ def check_bat(item): if item == "NO_INFO": return False else: - return float(item.split(' ')[1][:-1]) > 30 + return float(item.split(' ')[1][:-1]) > 50 @col_check(4) def check_sys_status(item): From f7b5c7f3ba8454a37308411c586f4c1bf85cdd76 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Sat, 9 Nov 2019 22:54:13 +0300 Subject: [PATCH 47/47] Server: Add start position delta check --- Server/copter_table_models.py | 32 ++++++++++++++++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index c8c21ce..245e4a8 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -1,5 +1,6 @@ import sys import re +import math import collections import indexed from server import ConfigOption @@ -63,7 +64,17 @@ class StatedCopterData(CopterData): class Checks: all_checks = {} takeoff_checklist = (3, 4, 6, 7, 8) + current_position = 'NO_POS' + start_position = 'NO_POS' + @classmethod + def get_pos_delta(self): + if self.current_position != 'NO_POS' and self.start_position != 'NO_POS': + delta_squared = 0 + for i in range(3): + delta_squared += (self.current_position[i]-self.start_position[i])**2 + return math.sqrt(delta_squared) + return 'NO_POS' class CopterDataModel(QtCore.QAbstractTableModel): selected_ready_signal = QtCore.pyqtSignal(bool) @@ -311,13 +322,30 @@ def check_selfcheck(item): def check_pos_status(item): if not item: return None - return item.split(' ')[0] != 'nan' and item.split(' ')[0] != 'NO_POS' + str_pos = item.split(' ') + if str_pos[0] != 'nan' and str_pos[0] != 'NO_POS': + Checks.current_position = [] + for i in range(3): + Checks.current_position.append(float(str_pos[i])) + return True + Checks.current_position = 'NO_POS' + return False @col_check(9) def check_start_pos_status(item): if not item: return None - return str(item).split(' ')[0] != 'NO_POS' + str_start_pos = item.split(' ') + if str_start_pos[0] != 'nan' and str_start_pos[0] != 'NO_POS': + Checks.start_position = [] + for i in range(3): + Checks.start_position.append(float(str_start_pos[i])) + delta = Checks.get_pos_delta() + if delta == 'NO_POS': + return False + else: + return delta < 1. + return False @col_check(10)