diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index deae299..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) @@ -175,11 +175,11 @@ def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, auto_arm=False, **kwargs return True -def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, +def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, auto_arm=False, freq=FREQUENCY, timeout=TIMEOUT, interrupter=INTERRUPTER, wait=False): - logger.info('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) + rospy.loginfo('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) #print('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) - navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed) + 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) @@ -188,17 +188,17 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO while (get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z) > tolerance) or wait: if interrupter.is_set(): - logger.warning("Reach point function interrupted!") + rospy.logwarn("Reach point function interrupted!") #print("Reach point function interrupted!") interrupter.clear() return False telemetry = get_telemetry(frame_id=frame_id) - logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( + 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( # telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) - logger.info('Current delta: | {:.3f}'.format( + rospy.logdebug('Current delta: | {:.3f}'.format( get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z))) #print('Current delta: | {:.3f}'.format( # get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z))) @@ -207,12 +207,12 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO if timeout is not None: if time_passed >= timeout: - logger.warning('Reaching point timed out! | time: {:3f} seconds'.format(time_passed)) - print('Reaching point timed out! | time: {:3f} seconds'.format(time_passed)) + rospy.logwarn('Reaching point timed out! | time: {:3f} seconds'.format(time_passed)) + # print('Reaching point timed out! | time: {:3f} seconds'.format(time_passed)) return wait rate.sleep() - logger.info("Point reached!") + rospy.loginfo("Point reached!") #print("Point reached!") return True diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index 28b7374..c9913c7 100644 --- a/Drone/animation_lib.py +++ b/Drone/animation_lib.py @@ -37,10 +37,44 @@ 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 load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, ratio=1): +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)) + 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)*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): imported_frames = [] global anim_id try: @@ -62,9 +96,9 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, ratio=1): frame_number, x, y, z, yaw, red, green, blue = row_0 imported_frames.append({ 'number': int(frame_number), - 'x': ratio*float(x) + x0, - 'y': ratio*float(y) + y0, - 'z': ratio*float(z) + z0, + 'x': x_ratio*float(x) + x0, + 'y': y_ratio*float(y) + y0, + 'z': z_ratio*float(z) + z0, 'yaw': float(yaw), 'red': int(red), 'green': int(green), @@ -74,9 +108,9 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, ratio=1): frame_number, x, y, z, yaw, red, green, blue = row imported_frames.append({ 'number': int(frame_number), - 'x': ratio*float(x) + x0, - 'y': ratio*float(y) + y0, - 'z': ratio*float(z) + z0, + 'x': x_ratio*float(x) + x0, + 'y': y_ratio*float(y) + y0, + 'z': z_ratio*float(z) + z0, 'yaw': float(yaw), 'red': int(red), 'green': int(green), diff --git a/Drone/client.py b/Drone/client.py index aed8dc4..f557081 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -1,3 +1,4 @@ +import os import time import errno import random @@ -74,6 +75,7 @@ class Client(object): def rewrite_config(self): with open(self.config_path, 'w') as file: self.config.write(file) + os.system("chown -R pi:pi /home/pi/clever-show") def write_config(self, reload_config=True, *config_options): for config_option in config_options: diff --git a/Drone/client_config.ini b/Drone/client_config.ini index 4fa87e1..487f309 100644 --- a/Drone/client_config.ini +++ b/Drone/client_config.ini @@ -13,18 +13,24 @@ use_ntp = False host = ntp1.stratum2.ru port = 123 +[TELEMETRY] +frequency = 1 +transmit = True + [ANIMATION] takeoff_animation_check = True land_animation_check = True frame_delay = 0.1 -ratio = 1.0 +x_ratio = 1.0 +y_ratio = 1.0 +z_ratio = 1.0 [COPTERS] frame_id = map -takeoff_height = 2.0 -takeoff_time = 8.0 +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 @@ -34,7 +40,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 diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 62d4a73..666fa36 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -2,7 +2,11 @@ import os import time import math import rospy +import datetime import logging +import threading +import subprocess +from collections import namedtuple from FlightLib import FlightLib from FlightLib import LedLib @@ -20,6 +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 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 @@ -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') @@ -49,7 +57,9 @@ class CopterClient(client.Client): self.TAKEOFF_CHECK = self.config.getboolean('ANIMATION', 'takeoff_animation_check') self.LAND_CHECK = self.config.getboolean('ANIMATION', 'land_animation_check') self.FRAME_DELAY = self.config.getfloat('ANIMATION', 'frame_delay') - self.RATIO = self.config.getfloat('ANIMATION', 'ratio') + self.X_RATIO = self.config.getfloat('ANIMATION', 'x_ratio') + self.Y_RATIO = self.config.getfloat('ANIMATION', 'y_ratio') + self.Z_RATIO = self.config.getfloat('ANIMATION', 'z_ratio') self.X0 = self.config.getfloat('PRIVATE', 'x0') self.Y0 = self.config.getfloat('PRIVATE', 'y0') self.Z0 = self.config.getfloat('PRIVATE', 'z0') @@ -64,7 +74,7 @@ class CopterClient(client.Client): def start(self, task_manager_instance): client.logger.info("Init ROS node") - rospy.init_node('Swarm_client') + rospy.init_node('clever_show_client') if self.USE_LEDS: LedLib.init_led(self.LED_PIN) task_manager_instance.start() @@ -92,7 +102,7 @@ class CopterClient(client.Client): trans.child_frame_id = self.FRAME_ID static_bloadcaster.sendTransform(trans) start_subscriber() - # print(check_state_topic()) + telemetry_thread.start() super(CopterClient, self).start() @@ -250,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): @@ -261,7 +275,9 @@ def _response_animation_id(*args, **kwargs): 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, - ratio=client.active_client.RATIO, + 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, @@ -296,7 +312,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): @@ -322,25 +342,24 @@ 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"), - x0=client.active_client.X0_COMMON, - y0=client.active_client.Y0_COMMON, - ratio=client.active_client.RATIO, + 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, ) - # 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'] - telem = FlightLib.get_telemetry(client.active_client.FRAME_ID) - 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)) + print("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)) + 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): @@ -368,10 +387,18 @@ def _command_reset_z(*args, **kwargs): @messaging.message_callback("update_repo") def _command_update_repo(*args, **kwargs): - os.system("git reset --hard origin/master") + os.system("mv /home/pi/clever-show/Drone/client_config.ini /home/pi/clever-show/Drone/client_config_tmp.ini") + os.system("git reset --hard HEAD") + os.system("git checkout master") os.system("git fetch") - os.system("git pull") - os.system("chown -R pi:pi ~/CleverSwarm") + os.system("git pull --rebase") + os.system("mv /home/pi/clever-show/Drone/client_config_tmp.ini /home/pi/clever-show/Drone/client_config.ini") + os.system("chown -R pi:pi /home/pi/clever-show") + +@messaging.message_callback("reboot_all") +def _command_reboot_all(*args, **kwargs): + reboot_fcu() + execute_command("reboot") @messaging.message_callback("reboot_fcu") def _command_reboot(*args, **kwargs): @@ -410,7 +437,8 @@ def _copter_flip(*args, **kwargs): @messaging.message_callback("takeoff") def _command_takeoff(*args, **kwargs): - task_manager.add_task(time.time(), 0, animation.takeoff, + print("Takeoff at {}".format(datetime.datetime.now())) + task_manager.add_task(0, 0, animation.takeoff, task_kwargs={ "z": client.active_client.TAKEOFF_HEIGHT, "timeout": client.active_client.TAKEOFF_TIME, @@ -419,6 +447,23 @@ def _command_takeoff(*args, **kwargs): } ) +@messaging.message_callback("takeoff_z") +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())) + task_manager.add_task(0, 0, FlightLib.reach_point, + task_kwargs={ + "x": telem.x, + "y": telem.y, + "z": float(z_str), + "frame_id": client.active_client.FRAME_ID, + "timeout": client.active_client.TAKEOFF_TIME, + "auto_arm": True, + } + ) + @messaging.message_callback("land") def _command_land(*args, **kwargs): @@ -474,7 +519,9 @@ def _play_animation(*args, **kwargs): 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, - ratio=client.active_client.RATIO, + 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, @@ -557,8 +604,77 @@ 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)) + 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: + 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.)) + 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)) + 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') + 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 = client.active_client.client_id + '`' + 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() diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index 9b85d6b..43440a0 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -202,12 +202,16 @@ class CopterDataModel(QtCore.QAbstractTableModel): if role == Qt.CheckStateRole: self.data_contents[row].states.checked = value elif role == Qt.EditRole: # For user actions with data - if col == 0: # and self.on_id_changed: - #self.data_contents[row][col] = "Awaiting for response" - #self.data_contents[row].states.copter_id = None - - self.data_contents[row].client.send_message("id", {"new_id": value}) - self.data_contents[row].client.remove() + if col == 0: + # check user hostname spelling http://man7.org/linux/man-pages/man7/hostname.7.html + if value[0] != '-' and len(value) <= 63 and re.match("^[A-Za-z0-9-]*$", value): + self.data_contents[row].client.send_message("id", {"new_id": value}) + self.data_contents[row].client.remove() + else: + msg = QtWidgets.QMessageBox() + msg.setIcon(QtWidgets.QMessageBox.Critical) + msg.setText("Wrong input for the copter name!\nPlease use only A-Z, a-z, 0-9, and '-' chars.\nDon't use '-' as first char.") + msg.exec_() else: self.data_contents[row][col] = value @@ -301,10 +305,11 @@ 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) diff --git a/Server/server.py b/Server/server.py index 135500c..b612711 100644 --- a/Server/server.py +++ b/Server/server.py @@ -311,14 +311,14 @@ class Client(messaging.ConnectionManager): logging.info("Connection to {} closed!".format(self.copter_id)) def remove(self): - print("closing ") if self.connected: self.close() - print("closed") if self.clients: - self.clients.pop(self.addr[0]) + try: + self.clients.pop(self.addr[0]) + except Exception as e: + logging.error(e) logging.info("Client {} successfully removed!".format(self.copter_id)) - print("REMOVED") @requires_connect def _send(self, data): diff --git a/Server/server_config.ini b/Server/server_config.ini index 5160e2d..96a835e 100644 --- a/Server/server_config.ini +++ b/Server/server_config.ini @@ -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 diff --git a/Server/server_gui.py b/Server/server_gui.py index 794d524..23b1306 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") @@ -52,6 +52,7 @@ class Ui_MainWindow(object): self.formLayout.setWidget(3, QtWidgets.QFormLayout.LabelRole, self.music_text) self.music_delay_spin = QtWidgets.QDoubleSpinBox(self.centralwidget) self.music_delay_spin.setDecimals(1) + self.music_delay_spin.setMaximum(1000.0) self.music_delay_spin.setObjectName("music_delay_spin") self.formLayout.setWidget(3, QtWidgets.QFormLayout.FieldRole, self.music_delay_spin) self.music_checkbox = QtWidgets.QCheckBox(self.centralwidget) @@ -60,8 +61,10 @@ class Ui_MainWindow(object): sizePolicy.setVerticalStretch(0) sizePolicy.setHeightForWidth(self.music_checkbox.sizePolicy().hasHeightForWidth()) self.music_checkbox.setSizePolicy(sizePolicy) + self.music_checkbox.setFocusPolicy(QtCore.Qt.NoFocus) + self.music_checkbox.setContextMenuPolicy(QtCore.Qt.DefaultContextMenu) self.music_checkbox.setLayoutDirection(QtCore.Qt.RightToLeft) - self.music_checkbox.setAutoFillBackground(True) + self.music_checkbox.setAutoFillBackground(False) self.music_checkbox.setText("") self.music_checkbox.setChecked(False) self.music_checkbox.setObjectName("music_checkbox") @@ -132,10 +135,10 @@ class Ui_MainWindow(object): self.formLayout_4.setObjectName("formLayout_4") self.land_button = QtWidgets.QPushButton(self.centralwidget) self.land_button.setObjectName("land_button") - self.formLayout_4.setWidget(3, QtWidgets.QFormLayout.FieldRole, self.land_button) + self.formLayout_4.setWidget(8, QtWidgets.QFormLayout.FieldRole, self.land_button) self.flip_button = QtWidgets.QPushButton(self.centralwidget) self.flip_button.setObjectName("flip_button") - self.formLayout_4.setWidget(2, QtWidgets.QFormLayout.FieldRole, self.flip_button) + self.formLayout_4.setWidget(7, QtWidgets.QFormLayout.FieldRole, self.flip_button) self.takeoff_button = QtWidgets.QPushButton(self.centralwidget) self.takeoff_button.setEnabled(True) self.takeoff_button.setObjectName("takeoff_button") @@ -143,6 +146,22 @@ class Ui_MainWindow(object): self.leds_button = QtWidgets.QPushButton(self.centralwidget) self.leds_button.setObjectName("leds_button") self.formLayout_4.setWidget(0, QtWidgets.QFormLayout.FieldRole, self.leds_button) + self.horizontalLayout_2 = QtWidgets.QHBoxLayout() + self.horizontalLayout_2.setContentsMargins(-1, 0, -1, -1) + self.horizontalLayout_2.setObjectName("horizontalLayout_2") + self.z_checkbox = QtWidgets.QCheckBox(self.centralwidget) + self.z_checkbox.setCursor(QtGui.QCursor(QtCore.Qt.ArrowCursor)) + self.z_checkbox.setFocusPolicy(QtCore.Qt.NoFocus) + self.z_checkbox.setLayoutDirection(QtCore.Qt.LeftToRight) + self.z_checkbox.setObjectName("z_checkbox") + self.horizontalLayout_2.addWidget(self.z_checkbox) + self.z_spin = QtWidgets.QDoubleSpinBox(self.centralwidget) + self.z_spin.setLayoutDirection(QtCore.Qt.LeftToRight) + self.z_spin.setDecimals(1) + self.z_spin.setProperty("value", 1.0) + self.z_spin.setObjectName("z_spin") + self.horizontalLayout_2.addWidget(self.z_spin) + self.formLayout_4.setLayout(4, QtWidgets.QFormLayout.FieldRole, self.horizontalLayout_2) self.verticalLayout.addLayout(self.formLayout_4) self.line_4 = QtWidgets.QFrame(self.centralwidget) self.line_4.setFrameShape(QtWidgets.QFrame.HLine) @@ -226,6 +245,8 @@ class Ui_MainWindow(object): self.action_remove_row.setObjectName("action_remove_row") self.action_send_calibrations = QtWidgets.QAction(MainWindow) self.action_send_calibrations.setObjectName("action_send_calibrations") + self.action_reboot_all = QtWidgets.QAction(MainWindow) + self.action_reboot_all.setObjectName("action_reboot_all") self.menuDeveloper_mode.addAction(self.action_send_any_file) self.menuDeveloper_mode.addAction(self.actionSend_any_command) self.menuOptions.addAction(self.action_send_animations) @@ -242,8 +263,10 @@ class Ui_MainWindow(object): self.menuDeveloper_mode_2.addAction(self.action_restart_clever) self.menuDeveloper_mode_2.addAction(self.action_restart_clever_show) self.menuDeveloper_mode_2.addAction(self.action_update_client_repo) + self.menuDeveloper_mode_2.addAction(self.action_reboot_all) self.menuDrone.addAction(self.action_set_z_offset_to_ground) self.menuDrone.addAction(self.action_reset_z_offset) + 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) @@ -278,6 +301,8 @@ class Ui_MainWindow(object): self.flip_button.setText(_translate("MainWindow", "Flip")) self.takeoff_button.setText(_translate("MainWindow", "Takeoff")) self.leds_button.setText(_translate("MainWindow", "Test leds")) + self.z_checkbox.setText(_translate("MainWindow", " Z =")) + self.z_spin.setSuffix(_translate("MainWindow", " m")) self.reboot_fcu.setText(_translate("MainWindow", "Reboot FCU")) self.calibrate_gyro.setText(_translate("MainWindow", "Calibrate gyro")) self.calibrate_level.setText(_translate("MainWindow", "Calibrate level")) @@ -292,7 +317,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")) @@ -310,3 +335,4 @@ class Ui_MainWindow(object): self.action_stop_music.setText(_translate("MainWindow", "Stop music")) self.action_remove_row.setText(_translate("MainWindow", "Remove from table")) self.action_send_calibrations.setText(_translate("MainWindow", "Send camera calibrations")) + self.action_reboot_all.setText(_translate("MainWindow", "Reboot all")) diff --git a/Server/server_gui.ui b/Server/server_gui.ui index 7141df1..31a0090 100644 --- a/Server/server_gui.ui +++ b/Server/server_gui.ui @@ -7,7 +7,7 @@ 0 0 1220 - 750 + 761 @@ -91,6 +91,9 @@ 1 + + 1000.000000000000000 + @@ -101,11 +104,17 @@ 0 + + Qt::NoFocus + + + Qt::DefaultContextMenu + Qt::RightToLeft - true + false @@ -248,14 +257,14 @@ Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - + Land - + Flip @@ -279,6 +288,45 @@ + + + + 0 + + + + + ArrowCursor + + + Qt::NoFocus + + + Qt::LeftToRight + + + Z = + + + + + + + Qt::LeftToRight + + + m + + + 1 + + + 1.000000000000000 + + + + + @@ -370,9 +418,11 @@ + + @@ -417,7 +467,7 @@ - Send launch file to clever + Send launch files @@ -496,13 +546,18 @@ Remove from table - + Send camera calibrations + + + Reboot all + + start_delay_spin diff --git a/Server/server_qt.py b/Server/server_qt.py index 9038bb2..d5eeb16 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() @@ -155,6 +175,7 @@ class MainWindow(QtWidgets.QMainWindow): self.ui.action_restart_clever.triggered.connect(self.restart_clever) self.ui.action_restart_clever_show.triggered.connect(self.restart_clever_show) self.ui.action_update_client_repo.triggered.connect(self.update_client_repo) + self.ui.action_reboot_all.triggered.connect(self.reboot_all_on_selected) self.ui.action_set_start_to_current_position.triggered.connect(self.update_start_to_current_position) self.ui.action_reset_start.triggered.connect(self.reset_start) self.ui.action_set_z_offset_to_ground.triggered.connect(self.set_z_offset_to_ground) @@ -169,7 +190,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 @@ -213,6 +234,39 @@ 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[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] + 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 @@ -276,7 +330,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) @@ -319,7 +373,10 @@ class MainWindow(QtWidgets.QMainWindow): def takeoff_selected(self, **kwargs): for copter in self.model.user_selected(): if takeoff_checks(copter): - copter.client.send_message("takeoff") + if self.ui.z_checkbox.isChecked(): + copter.client.send_message("takeoff_z", {"z":str(self.ui.z_spin.value())}) + else: + copter.client.send_message("takeoff") @pyqtSlot() @confirmation_required("This operation will flip(!!!) copters immediately. Proceed?") @@ -429,12 +486,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() @@ -450,7 +509,12 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def update_client_repo(self): for copter in self.model.user_selected(): - copter.client.send_message("update_repo") + copter.client.send_message("update_repo") + + @pyqtSlot() + def reboot_all_on_selected(self): + for copter in self.model.user_selected(): + copter.client.send_message("reboot_all") @pyqtSlot() def update_start_to_current_position(self): @@ -568,6 +632,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) @@ -576,7 +645,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 diff --git a/builder/assets/clever-show.service b/builder/assets/clever-show.service index 3c8466b..e6b96aa 100644 --- a/builder/assets/clever-show.service +++ b/builder/assets/clever-show.service @@ -7,7 +7,7 @@ After=network.target [Service] WorkingDirectory=/home/pi/clever-show/Drone ExecStart=/bin/bash -c ". /home/pi/catkin_ws/devel/setup.sh; \ - /usr/bin/python /home/pi/clever-show/Drone/copter_client.py" + ROS_HOSTNAME=`hostname`.local /usr/bin/python /home/pi/clever-show/Drone/copter_client.py" KillSignal=SIGKILL Restart=on-failure RestartSec=3 diff --git a/builder/image-build.sh b/builder/image-build.sh index bb68595..aa802d2 100755 --- a/builder/image-build.sh +++ b/builder/image-build.sh @@ -122,6 +122,7 @@ img-chroot ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever-show.service' '/lib/ # Copy config files for clever if [[ -d "${CONFIG_DIR}/launch" ]]; then img-chroot ${IMAGE_PATH} copy ${CONFIG_DIR}'/launch' '/home/pi/catkin_ws/src/clever/clever'; fi if [[ -d "${CONFIG_DIR}/map" ]]; then img-chroot ${IMAGE_PATH} copy ${CONFIG_DIR}'/map' '/home/pi/catkin_ws/src/clever/aruco_pose'; fi +if [[ -d "${CONFIG_DIR}/camera_info" ]]; then img-chroot ${IMAGE_PATH} copy ${CONFIG_DIR}'/camera_info' '/home/pi/catkin_ws/src/clever/clever'; fi # Shrink image img-resize ${IMAGE_PATH} diff --git a/builder/image-software.sh b/builder/image-software.sh index 6916c68..cc21aea 100755 --- a/builder/image-software.sh +++ b/builder/image-software.sh @@ -49,9 +49,6 @@ my_travis_retry() { return $result } -echo_stamp "Change repo owner to pi" -chown -Rf pi:pi /home/pi/clever-show/ - echo_stamp "Update apt cache" apt-get update -qq @@ -73,4 +70,8 @@ source devel/setup.bash catkin_make --pkg clever_flight_routines source devel/setup.bash +echo_stamp "Change clever-show and catkin_ws owner to pi" +chown -Rf pi:pi /home/pi/clever-show/ +chown -Rf pi:pi /home/pi/catkin_ws/ + echo_stamp "End of software installation" diff --git a/messaging_lib.py b/messaging_lib.py index cd1b0ed..40ecb34 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -1,4 +1,5 @@ import io +import os import sys import json import socket @@ -243,7 +244,7 @@ class ConnectionManager(object): with self._close_lock: self._should_close = True - self._set_selector_events_mask('rw') + self._set_selector_events_mask('w') NotifierSock().notify() def _close(self): @@ -389,6 +390,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 pi:pi {}".format(filepath)) def write(self): with self._send_lock: