diff --git a/Drone/FCU/clever_lpe_flow.params b/Drone/FCU/clever_lpe_flow.params new file mode 100644 index 0000000..04a358b --- /dev/null +++ b/Drone/FCU/clever_lpe_flow.params @@ -0,0 +1,6 @@ +1 1 LPE_FLW_OFF_Z 0.000000000000000000 9 +1 1 LPE_FLW_QMIN 60 6 +1 1 LPE_FLW_R 0.200000002980232239 9 +1 1 LPE_FLW_RR 0.000000000000000000 9 +1 1 LPE_FLW_SCALE 1.000000000000000000 9 +1 1 LPE_FUSION 86 6 \ No newline at end of file diff --git a/Drone/FCU/clever_lpe_gps.params b/Drone/FCU/clever_lpe_gps.params new file mode 100644 index 0000000..359101b --- /dev/null +++ b/Drone/FCU/clever_lpe_gps.params @@ -0,0 +1,4 @@ +1 1 ATT_EXT_HDG_M 0 6 +1 1 ATT_W_EXT_HDG 0.000000000000000000 9 +1 1 ATT_W_MAG 0.100000001490116119 9 +1 1 LPE_FUSION 145 6 \ No newline at end of file diff --git a/Drone/FCU/clever_lpe_no_ext_hdg.params b/Drone/FCU/clever_lpe_no_ext_hdg.params new file mode 100644 index 0000000..77fbfde --- /dev/null +++ b/Drone/FCU/clever_lpe_no_ext_hdg.params @@ -0,0 +1,3 @@ +1 1 ATT_EXT_HDG_M 0 6 +1 1 ATT_W_EXT_HDG 0.000000000000000000 9 +1 1 ATT_W_MAG 0.000000000000000000 9 \ No newline at end of file diff --git a/Drone/FCU/clever_lpe_viz.params b/Drone/FCU/clever_lpe_viz.params new file mode 100644 index 0000000..9b69a0e --- /dev/null +++ b/Drone/FCU/clever_lpe_viz.params @@ -0,0 +1,4 @@ +1 1 ATT_EXT_HDG_M 1 6 +1 1 ATT_W_EXT_HDG 0.500000000000000000 9 +1 1 ATT_W_MAG 0.000000000000000000 9 +1 1 LPE_FUSION 84 6 diff --git a/Drone/FCU/clever_mc_mpc.params b/Drone/FCU/clever_mc_mpc.params new file mode 100644 index 0000000..e8010e1 --- /dev/null +++ b/Drone/FCU/clever_mc_mpc.params @@ -0,0 +1,58 @@ +1 1 MC_PITCHRATE_D 0.003000000026077032 9 +1 1 MC_PITCHRATE_FF 0.000000000000000000 9 +1 1 MC_PITCHRATE_I 0.019999999552965164 9 +1 1 MC_PITCHRATE_MAX 220.000000000000000000 9 +1 1 MC_PITCHRATE_P 0.100000001490116119 9 +1 1 MC_PITCH_P 6.500000000000000000 9 +1 1 MC_PR_INT_LIM 0.300000011920928955 9 +1 1 MC_RATT_TH 0.800000011920928955 9 +1 1 MC_ROLLRATE_D 0.003000000026077032 9 +1 1 MC_ROLLRATE_FF 0.000000000000000000 9 +1 1 MC_ROLLRATE_I 0.019999999552965164 9 +1 1 MC_ROLLRATE_MAX 220.000000000000000000 9 +1 1 MC_ROLLRATE_P 0.100000001490116119 9 +1 1 MC_ROLL_P 6.500000000000000000 9 +1 1 MPC_ACC_DOWN_MAX 10.000000000000000000 9 +1 1 MPC_ACC_HOR 5.000000000000000000 9 +1 1 MPC_ACC_HOR_ESTM 0.500000000000000000 9 +1 1 MPC_ACC_HOR_MAX 10.000000000000000000 9 +1 1 MPC_ACC_UP_MAX 10.000000000000000000 9 +1 1 MPC_ALT_MODE 0 6 +1 1 MPC_CRUISE_90 3.000000000000000000 9 +1 1 MPC_DEC_HOR_SLOW 5.000000000000000000 9 +1 1 MPC_FLT_TSK 0 6 +1 1 MPC_HOLD_DZ 0.100000001490116119 9 +1 1 MPC_HOLD_MAX_XY 0.800000011920928955 9 +1 1 MPC_HOLD_MAX_Z 0.600000023841857910 9 +1 1 MPC_JERK_MAX 0.000000000000000000 9 +1 1 MPC_JERK_MIN 1.000000000000000000 9 +1 1 MPC_LAND_ALT1 10.000000000000000000 9 +1 1 MPC_LAND_ALT2 5.000000000000000000 9 +1 1 MPC_LAND_SPEED 0.699999988079071045 9 +1 1 MPC_MANTHR_MAX 1.000000000000000000 9 +1 1 MPC_MANTHR_MIN 0.079999998211860657 9 +1 1 MPC_MAN_TILT_MAX 35.000000000000000000 9 +1 1 MPC_MAN_Y_MAX 200.000000000000000000 9 +1 1 MPC_THR_HOVER 0.550000011920928955 9 +1 1 MPC_THR_MAX 1.000000000000000000 9 +1 1 MPC_THR_MIN 0.119999997317790985 9 +1 1 MPC_TILTMAX_AIR 45.000000000000000000 9 +1 1 MPC_TILTMAX_LND 12.000000000000000000 9 +1 1 MPC_TKO_RAMP_T 0.400000005960464478 9 +1 1 MPC_TKO_SPEED 1.000000000000000000 9 +1 1 MPC_VELD_LP 5.000000000000000000 9 +1 1 MPC_VEL_MANUAL 10.000000000000000000 9 +1 1 MPC_XY_CRUISE 5.000000000000000000 9 +1 1 MPC_XY_MAN_EXPO 0.000000000000000000 9 +1 1 MPC_XY_P 1.000000000000000000 9 +1 1 MPC_XY_VEL_D 0.006000000052154064 9 +1 1 MPC_XY_VEL_I 0.004999999888241291 9 +1 1 MPC_XY_VEL_MAX 12.000000000000000000 9 +1 1 MPC_XY_VEL_P 0.090000003576278687 9 +1 1 MPC_Z_MAN_EXPO 0.000000000000000000 9 +1 1 MPC_Z_P 0.949999988079071045 9 +1 1 MPC_Z_VEL_D 0.000000000000000000 9 +1 1 MPC_Z_VEL_I 0.014999999664723873 9 +1 1 MPC_Z_VEL_MAX_DN 1.000000000000000000 9 +1 1 MPC_Z_VEL_MAX_UP 3.000000000000000000 9 +1 1 MPC_Z_VEL_P 0.250000000000000000 9 \ No newline at end of file diff --git a/Drone/Firmware/px4fmu-v4-1.8.2-clever.5.px4 b/Drone/FCU/px4fmu-v4-1.8.2-clever.5.px4 similarity index 100% rename from Drone/Firmware/px4fmu-v4-1.8.2-clever.5.px4 rename to Drone/FCU/px4fmu-v4-1.8.2-clever.5.px4 diff --git a/Drone/Firmware/clever-3-lpe-flow.params b/Drone/Firmware/clever-3-lpe-flow.params deleted file mode 100644 index ddc5c5c..0000000 --- a/Drone/Firmware/clever-3-lpe-flow.params +++ /dev/null @@ -1,53 +0,0 @@ -# Onboard parameters for LPE setup -# -# Stack: PX4 Pro -# Vehicle: Multi-Rotor -# Version: 1.8.2 -# - -# Vehicle-Id Component-Id Name Value Type -1 1 ATT_EXT_HDG_M 1 6 -1 1 ATT_W_EXT_HDG 0.500000000000000000 9 -1 1 ATT_W_MAG 0.000000000000000000 9 -1 1 CBRK_IO_SAFETY 22027 6 -1 1 CBRK_USB_CHK 197848 6 -1 1 COM_ARM_MAG 1.000000000000000000 9 -1 1 COM_DISARM_LAND 1 6 -1 1 COM_FLTMODE1 8 6 -1 1 COM_FLTMODE4 2 6 -1 1 COM_FLTMODE6 2 6 -1 1 COM_LOW_BAT_ACT 2 6 -1 1 COM_RC_LOSS_T 2.000000000000000000 9 -1 1 LNDMC_ROT_MAX 45.000000000000000000 9 -1 1 LNDMC_THR_RANGE 0.800000000000000000 9 -1 1 LNDMC_Z_VEL_MAX 1.000000000000000000 9 -1 1 LPE_FLW_QMIN 10 6 -1 1 LPE_FLW_R 0.200000001490116119 9 -1 1 LPE_FLW_RR 0.000000000000000000 9 -1 1 LPE_FLW_SCALE 1.000000000000000000 9 -1 1 LPE_FUSION 86 6 -1 1 LPE_VIS_DELAY 0.000000000000000000 9 -1 1 LPE_VIS_Z 0.10000005960464478 9 -1 1 MC_PITCHRATE_D 0.001200000056996942 9 -1 1 MC_PITCHRATE_P 0.119999997317790985 9 -1 1 MC_PITCH_P 4.500000000000000000 9 -1 1 MC_ROLLRATE_D 0.001200000056996942 9 -1 1 MC_ROLLRATE_P 0.119999997317790985 9 -1 1 MC_ROLL_P 4.500000000000000000 9 -1 1 MC_YAW_P 2.500000000000000000 9 -1 1 MPC_ACC_DOWN_MAX 2.000000000000000000 9 -1 1 MPC_ACC_UP_MAX 2.000000000000000000 9 -1 1 MPC_MANTHR_MAX 0.899999976158142090 9 -1 1 MPC_THR_HOVER 0.550011920928955 9 -1 1 MPC_THR_MAX 0.899999976158142090 9 -1 1 MPC_XY_P 1.400000023841857910 9 -1 1 MPC_XY_VEL_P 0.100000003576278687 9 -1 1 MPC_Z_VEL_MAX_UP 1.000000000000000000 9 -1 1 MPC_Z_VEL_P 0.300000011920928955 9 -1 1 RC_MAP_FLTMODE 5 6 -1 1 SENS_FLOW_MAXHGT 4.000000000000000000 9 -1 1 SENS_FLOW_MAXR 2.500000000000000000 9 -1 1 SENS_FLOW_MINHGT 0.01 9 -1 1 SENS_FLOW_ROT 0 6 -1 1 SYS_HAS_BARO 0 6 -1 1 SYS_MC_EST_GROUP 1 6 diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index 6f490df..943ff0a 100644 --- a/Drone/animation_lib.py +++ b/Drone/animation_lib.py @@ -40,7 +40,7 @@ def get_id(filepath="animation.csv"): print("No animation id in file") return anim_id -def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0): +def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, ratio=1): imported_frames = [] global anim_id try: @@ -62,9 +62,9 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0): frame_number, x, y, z, yaw, red, green, blue = row_0 imported_frames.append({ 'number': int(frame_number), - 'x': float(x) + x0, - 'y': float(y) + y0, - 'z': float(z) + z0, + 'x': ratio*float(x) + x0, + 'y': ratio*float(y) + y0, + 'z': ratio*float(z) + z0, 'yaw': float(yaw), 'red': int(red), 'green': int(green), @@ -74,9 +74,9 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0): frame_number, x, y, z, yaw, red, green, blue = row imported_frames.append({ 'number': int(frame_number), - 'x': float(x) + x0, - 'y': float(y) + y0, - 'z': float(z) + z0, + 'x': ratio*float(x) + x0, + 'y': ratio*float(y) + y0, + '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 f824e5e..2b6664b 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -7,6 +7,7 @@ import logging import collections import ConfigParser import selectors2 as selectors +import threading from contextlib import closing @@ -15,7 +16,7 @@ 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.INFO) +logging.basicConfig(level=logging.DEBUG) logger = logging.getLogger(__name__) import messaging_lib as messaging @@ -102,7 +103,7 @@ class Client(object): try: while True: self._reconnect() - self._process_connections() + #self._process_connections() except (KeyboardInterrupt, ): logger.critical("Caught interrupt, exiting!") @@ -141,7 +142,7 @@ class Client(object): def _connect(self): self.connected = True self.client_socket.setblocking(False) - events = selectors.EVENT_READ | selectors.EVENT_WRITE + events = selectors.EVENT_READ # | selectors.EVENT_WRITE self.selector.register(self.client_socket, events, data=self.server_connection) self.server_connection.connect(self.selector, self.client_socket, (self.server_host, self.server_port)) self._process_connections() @@ -186,25 +187,26 @@ class Client(object): def _process_connections(self): while True: events = self.selector.select(timeout=1) - if events: - for key, mask in events: - if key.data is None: - pass - else: - connection = key.data - try: - connection.process_events(mask) + # logging.debug("tick") + for key, mask in events: # TODO add notifier to client! + connection = key.data + if connection is None: + pass + else: + try: + connection.process_events(mask) - except Exception as error: - logger.error( - "Exception {} occurred for {}! Resetting connection!".format(error, connection.addr) - ) - self.server_connection.close() - self.connected = False + except Exception as error: + logger.error( + "Exception {} occurred for {}! Resetting connection!".format(error, connection.addr) + ) + self.server_connection.close() + self.connected = False + + if isinstance(error, OSError): + if error.errno == errno.EINTR: + raise KeyboardInterrupt - if isinstance(error, OSError): - if error.errno == errno.EINTR: - raise KeyboardInterrupt if not self.selector.get_map(): logger.warning("No active connections left!") diff --git a/Drone/client_config.ini b/Drone/client_config.ini index f554f64..a3a4d36 100644 --- a/Drone/client_config.ini +++ b/Drone/client_config.ini @@ -16,12 +16,11 @@ port = 123 [ANIMATION] takeoff_animation_check = True land_animation_check = True -frame_delay = 0.1 +frame_delay = 0.15 +ratio = 0.8 [COPTERS] -frame_id = aruco_map_flipped -frame_flipped_width = 1.5 -frame_flipped_height = 6.5 +frame_id = map takeoff_height = 2.0 takeoff_time = 8.0 safe_takeoff = False @@ -29,11 +28,22 @@ reach_first_point_time = 8.0 land_time = 3.0 x0_common = 0 y0_common = 0 +z0_common = 0 + +[FLOOR FRAME] +parent = aruco_map +x = 0.0 +y = 0.0 +z = 6.5 +roll = 180 +pitch = 0 +yaw = -90 [PRIVATE] id = /hostname use_leds = True -led_pin = 18 +led_pin = 21 x0 = 0 y0 = 0 +z0 = 0 diff --git a/Drone/client_setup.sh b/Drone/client_setup.sh index 1eecf4d..69bc2e1 100755 --- a/Drone/client_setup.sh +++ b/Drone/client_setup.sh @@ -52,7 +52,6 @@ sed -i "/127.0.1.1/c 127.0.1.1 $3" /etc/hosts # set hostname for ROS sed -i "/ROS_HOSTNAME/c ROS_HOSTNAME=\'$3\'" /home/pi/.bashrc -sed -i "/ROS_HOSTNAME/c ROS_HOSTNAME=$3" /lib/systemd/system/roscore.env # set ssh message cat << EOF | tee /etc/motd @@ -65,7 +64,7 @@ EOF cat << EOF | tee /etc/chrony/chrony.conf server $4 iburst driftfile /var/lib/chrony/drift -makestep 1.0 3 +makestep 1.0 -1 rtcsync EOF diff --git a/Drone/copter_client.py b/Drone/copter_client.py index ad85d80..92cd73b 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -45,11 +45,14 @@ class CopterClient(client.Client): self.LAND_TIME = self.config.getfloat('COPTERS', 'land_time') self.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common') self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common') + self.Z0_COMMON = self.config.getfloat('COPTERS', 'z0_common') 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.X0 = self.config.getfloat('PRIVATE', 'x0') self.Y0 = self.config.getfloat('PRIVATE', 'y0') + 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') @@ -63,20 +66,28 @@ class CopterClient(client.Client): if self.USE_LEDS: LedLib.init_led(self.LED_PIN) task_manager_instance.start() - if self.FRAME_ID == "aruco_map_flipped": + if self.FRAME_ID == "floor": try: - self.FRAME_FLIPPED_HEIGHT = self.config.getfloat('COPTERS', 'frame_flipped_height') - self.FRAME_FLIPPED_WIDTH = self.config.getfloat('COPTERS', 'frame_flipped_width') + 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: - pass + raise Exception("Can't make floor frame!") + quit() else: trans = TransformStamped() - trans.transform.translation.x = 0. - trans.transform.translation.y = self.FRAME_FLIPPED_WIDTH - trans.transform.translation.z = self.FRAME_FLIPPED_HEIGHT - trans.transform.rotation = Quaternion(*quaternion_from_euler(math.pi,0,0)) - trans.header.frame_id = "aruco_map" - trans.child_frame_id = "aruco_map_flipped" + 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) start_subscriber() # print(check_state_topic()) @@ -138,6 +149,8 @@ def _response_animation_id(): 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, + ratio=client.active_client.RATIO, ) # Correct start and land frames in animation corrected_frames, start_action, start_delay = animation.correct_animation(frames, @@ -202,6 +215,7 @@ def _command_move_start_to_current_position(**kwargs): 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, ) # Correct start and land frames in animation corrected_frames, start_action, start_delay = animation.correct_animation(frames, @@ -225,6 +239,21 @@ def _command_reset_start(**kwargs): client.active_client.load_config() print ("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0)) +@messaging.message_callback("set_z_to_ground") +def _command_set_z(**kwargs): + telem = FlightLib.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() + print ("Set z offset to {:.2f}".format(client.active_client.Z0)) + +@messaging.message_callback("reset_z_offset") +def _command_reset_z(**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)) + @messaging.message_callback("update_repo") def _command_update_repo(**kwargs): @@ -333,6 +362,8 @@ def _play_animation(**kwargs): 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, + ratio=client.active_client.RATIO, ) # Correct start and land frames in animation corrected_frames, start_action, start_delay = animation.correct_animation(frames, diff --git a/README.md b/README.md index 022eab2..2f7bf21 100644 --- a/README.md +++ b/README.md @@ -1,16 +1,19 @@ # clever-show -[![Build Status](https://travis-ci.org/artem30801/CleverSwarm.svg?branch=master)](https://travis-ci.org/artem30801/CleverSwarm) -[![CodeFactor](https://www.codefactor.io/repository/github/artem30801/cleverswarm/badge)](https://www.codefactor.io/repository/github/artem30801/cleverswarm) +[Русская версия](README_RU.md) -Програмное обеспечение для запуска шоу дронов под управлением Raspberry Pi с пакетом COEX [Clever](https://github.com/copterexpress/clever). +Software for making the drone show controlled by Raspberry Pi and COEX [Clever](https://github.com/CopterExpress/clever) package. -### Пакет включает в себя: -* [Набор ПО для дрона](https://github.com/artem30801/CleverSwarm/tree/master/Drone), включащее в себя библиотеку для автономного полёта, модуль для воспроизведения анимаций и клиентское приложение для удаленного синхронизированного управления -* [Серверное приложение](https://github.com/artem30801/CleverSwarm/tree/master/Server) для удаленного синхронизированного управления дронами и удобной настройки системы для воспроизведения анимации -* [Аддон для Blender 2.8](https://github.com/artem30801/CleverSwarm/tree/master/blender-addon) для преобразования анимации полёта коптеров, созданной в Blender, в файлы полётов для каждого коптера -* [Образ для Raspberry Pi](https://github.com/artem30801/CleverSwarm/releases/latest) для быстрого запуска ПО на коптере +[![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 +Start tutorial is located [here](docs/start-tutorial.md). + +Detailed documentation is located in the [docs](https://github.com/CopterExpress/clever-show/tree/master/docs) folder. -## Документация -Инструкция по запуску ПО находится [здесь](docs/start-tutorial.md). -Подробная документация расположена в папке [docs](https://github.com/artem30801/CleverSwarm/tree/master/docs). diff --git a/README_RU.md b/README_RU.md new file mode 100644 index 0000000..19b7d9d --- /dev/null +++ b/README_RU.md @@ -0,0 +1,19 @@ +# clever-show +[English version](README.md) + +Програмное обеспечение для запуска шоу дронов под управлением Raspberry Pi с пакетом COEX [Clever](https://github.com/CopterExpress/clever). + +[![Build Status](https://travis-ci.org/CopterExpress/clever-show.svg?branch=master)](https://travis-ci.org/CopterExpress/clever-show) + +## Пакет включает в себя +* [Набор ПО для дрона](https://github.com/CopterExpress/clever-show/tree/master/Drone) с библиотекой для автономного полёта, модулем воспроизведения анимаций и клиентским приложением для удаленного синхронизированного управления дронами +* [Серверное приложение](https://github.com/CopterExpress/clever-show/tree/master/Server) для создания шоу и настройки дронов, анимации и музыки +* [Аддон для Blender 2.8](https://github.com/CopterExpress/clever-show/tree/master/blender-addon) для преобразования анимации полёта коптеров, созданной в Blender, в файлы полётов для каждого коптера +* [Образ для Raspberry Pi](https://github.com/CopterExpress/clever-show/releases/latest) для быстрого запуска ПО на коптере + +## Документация +Инструкция по запуску ПО находится [здесь](docs/start-tutorial.md). + +Подробная документация расположена в папке [docs](https://github.com/CopterExpress/clever-show/tree/master/docs). + + diff --git a/Server/chrony.conf b/Server/chrony.conf index bccbf9e..32d980f 100644 --- a/Server/chrony.conf +++ b/Server/chrony.conf @@ -1,5 +1,11 @@ -server master iburst +pool 0.ru.pool.ntp.org iburst minpoll 10 +pool 1.ru.pool.ntp.org iburst minpoll 10 +pool 2.ru.pool.ntp.org iburst minpoll 10 +pool 3.ru.pool.ntp.org iburst minpoll 10 driftfile /var/lib/chrony/drift +local stratum 8 allow 192.168.0.0/16 makestep 1.0 3 -rtcsync \ No newline at end of file +smoothtime 50000 0.01 +rtcsync + diff --git a/Server/copter_table_models.py b/Server/copter_table_models.py index 22c56b3..b5f9e53 100644 --- a/Server/copter_table_models.py +++ b/Server/copter_table_models.py @@ -170,74 +170,49 @@ def col_check(col): def check_anim(item): if not item: return None - if str(item) == 'No animation': - return False - else: - return True - + return str(item) != 'No animation' @col_check(2) def check_bat_v(item): if not item: return None - if float(item) > 3.2: # todo config - return True - else: - return False - + return float(item) > 3.2 @col_check(3) def check_bat_p(item): if not item: return None - if float(item) > 30: # todo config - return True - else: - return False - #return True #For testing + return float(item) > 30 @col_check(4) def check_sys_status(item): if not item: return None - if item == "STANDBY": - return True - else: - return False + return item == "STANDBY" @col_check(5) def check_cal_status(item): if not item: return None - if item == "OK": - return True - else: - return False + return item == "OK" @col_check(6) def check_selfcheck(item): if not item: return None - if item == "OK": - return True - else: - return False + return item == "OK" @col_check(7) def check_cal_status(item): if not item: return None - else: - return True + return True @col_check(8) def check_time_delta(item): if not item: return None - if abs(float(item)) < 1: - return True - else: - return False + return abs(float(item)) < 1 def all_checks(copter_item): @@ -263,9 +238,7 @@ def flip_checks(copter_item): return True def calibrating_check(copter_item): - if copter_item[5] == "CALIBRATING": - return True - return False + return copter_item[5] == "CALIBRATING" def calibration_ready_check(copter_item): if not CopterDataModel.checks[4](copter_item[4]): diff --git a/Server/server.py b/Server/server.py index bf8ed6c..66cffff 100644 --- a/Server/server.py +++ b/Server/server.py @@ -143,20 +143,27 @@ class Server: logging.info("Client processor (selector) thread started!") self.server_socket.listen() self.server_socket.setblocking(False) - self.sel.register(self.server_socket, selectors.EVENT_READ | selectors.EVENT_WRITE, data=None) + self.sel.register(self.server_socket, selectors.EVENT_READ, data=None) #| selectors.EVENT_WRITE + + messaging.NotifierSock().bind((self.ip, self.port)) while self.client_processor_thread_running.is_set(): events = self.sel.select() + logging.error('tick') for key, mask in events: - if key.data is None: + # logging.error(mask) + # logging.error(str(key.data)) + client = key.data + if client is None: self._connect_client(key.fileobj) - else: - client = key.data + elif isinstance(client, messaging.ConnectionManager): try: client.process_events(mask) except Exception as error: logging.error("Exception {} occurred for {}! Resetting connection!".format(error, client.addr)) client.close() + else: # Notifier + client.process_events(mask) logging.info("Client autoconnect thread stopped!") @@ -165,7 +172,11 @@ class Server: logging.info("Got connection from: {}".format(str(addr))) conn.setblocking(False) - if not any([client_addr == addr[0] for client_addr in Client.clients.keys()]): + if addr[0] == self.ip and messaging.NotifierSock().addr is None: + client = messaging.NotifierSock() + logging.info("Notifier sock client") + + elif not any([client_addr == addr[0] for client_addr in Client.clients.keys()]): client = Client(addr[0]) logging.info("New client") else: diff --git a/Server/server_gui.py b/Server/server_gui.py index a9c4ad0..0826274 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, 613) + MainWindow.resize(1220, 750) self.centralwidget = QtWidgets.QWidget(MainWindow) self.centralwidget.setEnabled(True) self.centralwidget.setObjectName("centralwidget") @@ -43,31 +43,41 @@ class Ui_MainWindow(object): self.verticalLayout.setSizeConstraint(QtWidgets.QLayout.SetMaximumSize) self.verticalLayout.setObjectName("verticalLayout") self.formLayout = QtWidgets.QFormLayout() - self.formLayout.setLabelAlignment(QtCore.Qt.AlignLeading|QtCore.Qt.AlignLeft|QtCore.Qt.AlignVCenter) - self.formLayout.setFormAlignment(QtCore.Qt.AlignLeading|QtCore.Qt.AlignLeft|QtCore.Qt.AlignTop) + self.formLayout.setLabelAlignment(QtCore.Qt.AlignCenter) + self.formLayout.setFormAlignment(QtCore.Qt.AlignCenter) self.formLayout.setObjectName("formLayout") - self.check_button = QtWidgets.QPushButton(self.centralwidget) - self.check_button.setEnabled(True) - self.check_button.setObjectName("check_button") - self.formLayout.setWidget(1, QtWidgets.QFormLayout.SpanningRole, self.check_button) - self.start_text = QtWidgets.QLabel(self.centralwidget) - self.start_text.setAlignment(QtCore.Qt.AlignCenter) - self.start_text.setObjectName("start_text") - self.formLayout.setWidget(2, QtWidgets.QFormLayout.LabelRole, self.start_text) + self.music_text = QtWidgets.QLabel(self.centralwidget) + self.music_text.setLayoutDirection(QtCore.Qt.RightToLeft) + self.music_text.setObjectName("music_text") + 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.setObjectName("music_delay_spin") + self.formLayout.setWidget(3, QtWidgets.QFormLayout.FieldRole, self.music_delay_spin) + self.music_checkbox = QtWidgets.QCheckBox(self.centralwidget) + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Minimum, QtWidgets.QSizePolicy.Fixed) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.music_checkbox.sizePolicy().hasHeightForWidth()) + self.music_checkbox.setSizePolicy(sizePolicy) + self.music_checkbox.setLayoutDirection(QtCore.Qt.RightToLeft) + self.music_checkbox.setAutoFillBackground(True) + self.music_checkbox.setText("") + self.music_checkbox.setChecked(False) + self.music_checkbox.setObjectName("music_checkbox") + self.formLayout.setWidget(4, QtWidgets.QFormLayout.FieldRole, self.music_checkbox) + self.music_play_text = QtWidgets.QLabel(self.centralwidget) + self.music_play_text.setLayoutDirection(QtCore.Qt.RightToLeft) + self.music_play_text.setObjectName("music_play_text") + self.formLayout.setWidget(4, QtWidgets.QFormLayout.LabelRole, self.music_play_text) self.start_delay_spin = QtWidgets.QSpinBox(self.centralwidget) self.start_delay_spin.setObjectName("start_delay_spin") self.formLayout.setWidget(2, QtWidgets.QFormLayout.FieldRole, self.start_delay_spin) - self.start_button = QtWidgets.QPushButton(self.centralwidget) - self.start_button.setEnabled(True) - self.start_button.setFlat(False) - self.start_button.setObjectName("start_button") - self.formLayout.setWidget(3, QtWidgets.QFormLayout.SpanningRole, self.start_button) - self.pause_button = QtWidgets.QPushButton(self.centralwidget) - self.pause_button.setObjectName("pause_button") - self.formLayout.setWidget(4, QtWidgets.QFormLayout.SpanningRole, self.pause_button) - self.stop_button = QtWidgets.QPushButton(self.centralwidget) - self.stop_button.setObjectName("stop_button") - self.formLayout.setWidget(5, QtWidgets.QFormLayout.SpanningRole, self.stop_button) + self.start_text = QtWidgets.QLabel(self.centralwidget) + self.start_text.setLayoutDirection(QtCore.Qt.RightToLeft) + self.start_text.setAlignment(QtCore.Qt.AlignCenter) + self.start_text.setObjectName("start_text") + self.formLayout.setWidget(2, QtWidgets.QFormLayout.LabelRole, self.start_text) self.verticalLayout.addLayout(self.formLayout) self.line = QtWidgets.QFrame(self.centralwidget) self.line.setFrameShape(QtWidgets.QFrame.HLine) @@ -77,15 +87,21 @@ class Ui_MainWindow(object): self.formLayout_2 = QtWidgets.QFormLayout() self.formLayout_2.setFormAlignment(QtCore.Qt.AlignLeading|QtCore.Qt.AlignLeft|QtCore.Qt.AlignVCenter) self.formLayout_2.setObjectName("formLayout_2") - self.disarm_button = QtWidgets.QPushButton(self.centralwidget) - self.disarm_button.setObjectName("disarm_button") - self.formLayout_2.setWidget(7, QtWidgets.QFormLayout.SpanningRole, self.disarm_button) - self.emergency_button = QtWidgets.QPushButton(self.centralwidget) - self.emergency_button.setObjectName("emergency_button") - self.formLayout_2.setWidget(6, QtWidgets.QFormLayout.SpanningRole, self.emergency_button) - self.disarm_all_button = QtWidgets.QPushButton(self.centralwidget) - self.disarm_all_button.setObjectName("disarm_all_button") - self.formLayout_2.setWidget(8, QtWidgets.QFormLayout.SpanningRole, self.disarm_all_button) + self.stop_button = QtWidgets.QPushButton(self.centralwidget) + self.stop_button.setObjectName("stop_button") + self.formLayout_2.setWidget(10, QtWidgets.QFormLayout.FieldRole, self.stop_button) + self.pause_button = QtWidgets.QPushButton(self.centralwidget) + self.pause_button.setObjectName("pause_button") + self.formLayout_2.setWidget(9, QtWidgets.QFormLayout.FieldRole, self.pause_button) + self.start_button = QtWidgets.QPushButton(self.centralwidget) + self.start_button.setEnabled(True) + self.start_button.setFlat(False) + self.start_button.setObjectName("start_button") + self.formLayout_2.setWidget(8, QtWidgets.QFormLayout.FieldRole, self.start_button) + self.check_button = QtWidgets.QPushButton(self.centralwidget) + self.check_button.setEnabled(True) + self.check_button.setObjectName("check_button") + self.formLayout_2.setWidget(7, QtWidgets.QFormLayout.FieldRole, self.check_button) self.verticalLayout.addLayout(self.formLayout_2) self.line_2 = QtWidgets.QFrame(self.centralwidget) self.line_2.setFrameShape(QtWidgets.QFrame.HLine) @@ -96,19 +112,15 @@ class Ui_MainWindow(object): self.formLayout_3.setFormAlignment(QtCore.Qt.AlignLeading|QtCore.Qt.AlignLeft|QtCore.Qt.AlignVCenter) self.formLayout_3.setVerticalSpacing(6) self.formLayout_3.setObjectName("formLayout_3") - self.leds_button = QtWidgets.QPushButton(self.centralwidget) - self.leds_button.setObjectName("leds_button") - self.formLayout_3.setWidget(0, QtWidgets.QFormLayout.SpanningRole, self.leds_button) - self.takeoff_button = QtWidgets.QPushButton(self.centralwidget) - self.takeoff_button.setEnabled(True) - self.takeoff_button.setObjectName("takeoff_button") - self.formLayout_3.setWidget(1, QtWidgets.QFormLayout.SpanningRole, self.takeoff_button) - self.flip_button = QtWidgets.QPushButton(self.centralwidget) - self.flip_button.setObjectName("flip_button") - self.formLayout_3.setWidget(2, QtWidgets.QFormLayout.SpanningRole, self.flip_button) - self.land_button = QtWidgets.QPushButton(self.centralwidget) - self.land_button.setObjectName("land_button") - self.formLayout_3.setWidget(3, QtWidgets.QFormLayout.SpanningRole, self.land_button) + self.disarm_all_button = QtWidgets.QPushButton(self.centralwidget) + self.disarm_all_button.setObjectName("disarm_all_button") + self.formLayout_3.setWidget(3, QtWidgets.QFormLayout.FieldRole, self.disarm_all_button) + self.disarm_button = QtWidgets.QPushButton(self.centralwidget) + self.disarm_button.setObjectName("disarm_button") + self.formLayout_3.setWidget(2, QtWidgets.QFormLayout.FieldRole, self.disarm_button) + self.emergency_button = QtWidgets.QPushButton(self.centralwidget) + self.emergency_button.setObjectName("emergency_button") + self.formLayout_3.setWidget(0, QtWidgets.QFormLayout.FieldRole, self.emergency_button) self.verticalLayout.addLayout(self.formLayout_3) self.line_3 = QtWidgets.QFrame(self.centralwidget) self.line_3.setFrameShape(QtWidgets.QFrame.HLine) @@ -116,18 +128,40 @@ class Ui_MainWindow(object): self.line_3.setObjectName("line_3") self.verticalLayout.addWidget(self.line_3) self.formLayout_4 = QtWidgets.QFormLayout() - self.formLayout_4.setFormAlignment(QtCore.Qt.AlignBottom|QtCore.Qt.AlignLeading|QtCore.Qt.AlignLeft) + self.formLayout_4.setFormAlignment(QtCore.Qt.AlignLeading|QtCore.Qt.AlignLeft|QtCore.Qt.AlignVCenter) 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.flip_button = QtWidgets.QPushButton(self.centralwidget) + self.flip_button.setObjectName("flip_button") + self.formLayout_4.setWidget(2, QtWidgets.QFormLayout.FieldRole, self.flip_button) + self.takeoff_button = QtWidgets.QPushButton(self.centralwidget) + self.takeoff_button.setEnabled(True) + self.takeoff_button.setObjectName("takeoff_button") + self.formLayout_4.setWidget(1, QtWidgets.QFormLayout.FieldRole, self.takeoff_button) + 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.verticalLayout.addLayout(self.formLayout_4) + self.line_4 = QtWidgets.QFrame(self.centralwidget) + self.line_4.setFrameShape(QtWidgets.QFrame.HLine) + self.line_4.setFrameShadow(QtWidgets.QFrame.Sunken) + self.line_4.setObjectName("line_4") + self.verticalLayout.addWidget(self.line_4) + self.formLayout_6 = QtWidgets.QFormLayout() + self.formLayout_6.setFormAlignment(QtCore.Qt.AlignLeading|QtCore.Qt.AlignLeft|QtCore.Qt.AlignVCenter) + self.formLayout_6.setObjectName("formLayout_6") self.reboot_fcu = QtWidgets.QPushButton(self.centralwidget) self.reboot_fcu.setObjectName("reboot_fcu") - self.formLayout_4.setWidget(0, QtWidgets.QFormLayout.SpanningRole, self.reboot_fcu) + self.formLayout_6.setWidget(0, QtWidgets.QFormLayout.FieldRole, self.reboot_fcu) self.calibrate_gyro = QtWidgets.QPushButton(self.centralwidget) self.calibrate_gyro.setObjectName("calibrate_gyro") - self.formLayout_4.setWidget(1, QtWidgets.QFormLayout.SpanningRole, self.calibrate_gyro) + self.formLayout_6.setWidget(1, QtWidgets.QFormLayout.FieldRole, self.calibrate_gyro) self.calibrate_level = QtWidgets.QPushButton(self.centralwidget) self.calibrate_level.setObjectName("calibrate_level") - self.formLayout_4.setWidget(2, QtWidgets.QFormLayout.SpanningRole, self.calibrate_level) - self.verticalLayout.addLayout(self.formLayout_4) + self.formLayout_6.setWidget(2, QtWidgets.QFormLayout.FieldRole, self.calibrate_level) + self.verticalLayout.addLayout(self.formLayout_6) self.horizontalLayout.addLayout(self.verticalLayout) self.horizontalLayout.setStretch(0, 1) self.gridLayout.addLayout(self.horizontalLayout, 0, 0, 1, 1) @@ -143,6 +177,10 @@ class Ui_MainWindow(object): self.menuTable.setObjectName("menuTable") self.menuAnimation = QtWidgets.QMenu(self.menubar) self.menuAnimation.setObjectName("menuAnimation") + self.menuDrone = QtWidgets.QMenu(self.menubar) + self.menuDrone.setObjectName("menuDrone") + self.menuMusic = QtWidgets.QMenu(self.menubar) + self.menuMusic.setObjectName("menuMusic") MainWindow.setMenuBar(self.menubar) self.action_send_animations = QtWidgets.QAction(MainWindow) self.action_send_animations.setObjectName("action_send_animations") @@ -166,6 +204,16 @@ class Ui_MainWindow(object): self.action_set_start_to_current_position.setObjectName("action_set_start_to_current_position") self.action_reset_start = QtWidgets.QAction(MainWindow) self.action_reset_start.setObjectName("action_reset_start") + self.action_set_z_offset_to_ground = QtWidgets.QAction(MainWindow) + self.action_set_z_offset_to_ground.setObjectName("action_set_z_offset_to_ground") + self.action_reset_z_offset = QtWidgets.QAction(MainWindow) + self.action_reset_z_offset.setObjectName("action_reset_z_offset") + self.action_select_music_file = QtWidgets.QAction(MainWindow) + self.action_select_music_file.setObjectName("action_select_music_file") + self.action_play_music = QtWidgets.QAction(MainWindow) + self.action_play_music.setObjectName("action_play_music") + self.action_test_music_after = QtWidgets.QAction(MainWindow) + self.action_test_music_after.setObjectName("action_test_music_after") self.menuDeveloper_mode.addAction(self.action_send_launch_file) self.menuDeveloper_mode.addAction(self.action_restart_clever) self.menuDeveloper_mode.addAction(self.action_restart_clever_show) @@ -178,35 +226,40 @@ class Ui_MainWindow(object): self.menuTable.addAction(self.action_select_all_rows) self.menuAnimation.addAction(self.action_set_start_to_current_position) self.menuAnimation.addAction(self.action_reset_start) + self.menuDrone.addAction(self.action_set_z_offset_to_ground) + self.menuDrone.addAction(self.action_reset_z_offset) + self.menuMusic.addAction(self.action_select_music_file) + self.menuMusic.addAction(self.action_play_music) + self.menuMusic.addAction(self.action_test_music_after) self.menubar.addAction(self.menuOptions.menuAction()) self.menubar.addAction(self.menuAnimation.menuAction()) + self.menubar.addAction(self.menuDrone.menuAction()) + self.menubar.addAction(self.menuMusic.menuAction()) self.menubar.addAction(self.menuTable.menuAction()) self.retranslateUi(MainWindow) QtCore.QMetaObject.connectSlotsByName(MainWindow) - MainWindow.setTabOrder(self.check_button, self.start_delay_spin) - MainWindow.setTabOrder(self.start_delay_spin, self.start_button) - MainWindow.setTabOrder(self.start_button, self.pause_button) - MainWindow.setTabOrder(self.pause_button, self.stop_button) - MainWindow.setTabOrder(self.stop_button, self.disarm_button) - MainWindow.setTabOrder(self.disarm_button, self.tableView) + MainWindow.setTabOrder(self.start_delay_spin, self.tableView) def retranslateUi(self, MainWindow): _translate = QtCore.QCoreApplication.translate MainWindow.setWindowTitle(_translate("MainWindow", "Clever Drone Animation Player")) - self.check_button.setText(_translate("MainWindow", "Preflight check")) - self.start_text.setText(_translate("MainWindow", "Start after")) - self.start_delay_spin.setSuffix(_translate("MainWindow", " seconds")) - self.start_button.setText(_translate("MainWindow", "Start animation")) - self.pause_button.setText(_translate("MainWindow", "Pause")) + self.music_text.setText(_translate("MainWindow", " Music after")) + self.music_delay_spin.setSuffix(_translate("MainWindow", " s")) + self.music_play_text.setText(_translate("MainWindow", " Play music")) + self.start_delay_spin.setSuffix(_translate("MainWindow", " s")) + self.start_text.setText(_translate("MainWindow", " Start after")) self.stop_button.setText(_translate("MainWindow", "Stop and land all")) + self.pause_button.setText(_translate("MainWindow", "Pause")) + self.start_button.setText(_translate("MainWindow", "Start animation")) + self.check_button.setText(_translate("MainWindow", "Preflight check")) + self.disarm_all_button.setText(_translate("MainWindow", "Disarm ALL")) self.disarm_button.setText(_translate("MainWindow", "Disarm selected")) self.emergency_button.setText(_translate("MainWindow", "Emergency land")) - self.disarm_all_button.setText(_translate("MainWindow", "Disarm ALL")) - self.leds_button.setText(_translate("MainWindow", "Test leds")) - self.takeoff_button.setText(_translate("MainWindow", "Takeoff")) - self.flip_button.setText(_translate("MainWindow", "Flip")) self.land_button.setText(_translate("MainWindow", "Land")) + self.flip_button.setText(_translate("MainWindow", "Flip")) + self.takeoff_button.setText(_translate("MainWindow", "Takeoff")) + self.leds_button.setText(_translate("MainWindow", "Test leds")) self.reboot_fcu.setText(_translate("MainWindow", "Reboot FCU")) self.calibrate_gyro.setText(_translate("MainWindow", "Calibrate gyro")) self.calibrate_level.setText(_translate("MainWindow", "Calibrate level")) @@ -214,6 +267,8 @@ class Ui_MainWindow(object): self.menuDeveloper_mode.setTitle(_translate("MainWindow", "Developer mode")) self.menuTable.setTitle(_translate("MainWindow", "Table")) self.menuAnimation.setTitle(_translate("MainWindow", "Animation")) + self.menuDrone.setTitle(_translate("MainWindow", "Drone")) + self.menuMusic.setTitle(_translate("MainWindow", "Music")) self.action_send_animations.setText(_translate("MainWindow", "Send Animations")) self.action_send_configurations.setText(_translate("MainWindow", "Send Configurations")) self.action_send_Aruco_map.setText(_translate("MainWindow", "Send Aruco map")) @@ -224,5 +279,10 @@ class Ui_MainWindow(object): self.action_restart_clever_show.setText(_translate("MainWindow", "Restart clever-show service")) self.action_select_all_rows.setText(_translate("MainWindow", "Select All")) self.action_select_all_rows.setShortcut(_translate("MainWindow", "Ctrl+A")) - self.action_set_start_to_current_position.setText(_translate("MainWindow", "Set start to current position")) + self.action_set_start_to_current_position.setText(_translate("MainWindow", "Set start X Y to current position")) self.action_reset_start.setText(_translate("MainWindow", "Reset start position")) + self.action_set_z_offset_to_ground.setText(_translate("MainWindow", "Set Z offset to ground")) + self.action_reset_z_offset.setText(_translate("MainWindow", "Reset Z offset")) + self.action_select_music_file.setText(_translate("MainWindow", "Select music file")) + self.action_play_music.setText(_translate("MainWindow", "Play music")) + self.action_test_music_after.setText(_translate("MainWindow", "Test music after")) diff --git a/Server/server_gui.ui b/Server/server_gui.ui index 4c45f2d..abeb6af 100644 --- a/Server/server_gui.ui +++ b/Server/server_gui.ui @@ -7,7 +7,7 @@ 0 0 1220 - 613 + 750 @@ -68,62 +68,80 @@ - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Qt::AlignCenter - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + Qt::AlignCenter - - - - true + + + + Qt::RightToLeft - Preflight check + Music after - - - - Start after + + + + s - - Qt::AlignCenter + + 1 + + + + + + + + 0 + 0 + + + + Qt::RightToLeft + + + true + + + + + + false + + + + + + + Qt::RightToLeft + + + Play music - seconds + s - - - - true + + + + Qt::RightToLeft - Start animation + Start after - - false - - - - - - - Pause - - - - - - - Stop and land all + + Qt::AlignCenter @@ -141,24 +159,40 @@ Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - + + - Disarm selected + Stop and land all - - + + - Emergency land + Pause - - + + + + true + - Disarm ALL + Start animation + + + false + + + + + + + true + + + Preflight check @@ -179,34 +213,24 @@ 6 - - + + - Test leds + Disarm ALL - - - - true - + + - Takeoff + Disarm selected - - + + - Flip - - - - - - - Land + Emergency land @@ -222,23 +246,68 @@ - Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - + + + + Land + + + + + + + Flip + + + + + + + true + + + Takeoff + + + + + + + Test leds + + + + + + + + + Qt::Horizontal + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + Reboot FCU - + Calibrate gyro - + Calibrate level @@ -294,8 +363,25 @@ + + + Drone + + + + + + + Music + + + + + + + @@ -348,7 +434,7 @@ - Set start to current position + Set start X Y to current position @@ -356,14 +442,34 @@ Reset start position + + + Set Z offset to ground + + + + + Reset Z offset + + + + + Select music file + + + + + Play music + + + + + Test music after + + - check_button start_delay_spin - start_button - pause_button - stop_button - disarm_button tableView diff --git a/Server/server_qt.py b/Server/server_qt.py index 3ba0fe9..15e8db6 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -1,12 +1,14 @@ import os import glob import math +import asyncio -from PyQt5 import QtWidgets +from PyQt5 import QtWidgets, QtMultimedia from PyQt5.QtGui import QStandardItemModel, QStandardItem -from PyQt5.QtCore import Qt, pyqtSlot, pyqtSignal, QObject +from PyQt5.QtCore import Qt, pyqtSlot, pyqtSignal, QObject, QUrl from PyQt5.QtWidgets import QFileDialog, QMessageBox +from quamash import QEventLoop # Importing gui form from server_gui import Ui_MainWindow @@ -18,6 +20,18 @@ from emergency import * import threading +def wait(end, interrupter=threading.Event(), maxsleep=0.1): + # Added features to interrupter sleep and set max sleeping interval + + while not interrupter.is_set(): # Basic implementation of pause module until() + now = time.time() + diff = min(end - now, maxsleep) + if diff <= 0: + break + else: + time.sleep(diff / 2) + + def confirmation_required(text="Are you sure?", label="Confirm operation?"): def inner(f): @@ -41,7 +55,7 @@ def confirmation_required(text="Are you sure?", label="Confirm operation?"): # noinspection PyArgumentList,PyCallByClass class MainWindow(QtWidgets.QMainWindow): - def __init__(self): + def __init__(self, loop): super(MainWindow, self).__init__() self.ui = Ui_MainWindow() @@ -54,6 +68,8 @@ class MainWindow(QtWidgets.QMainWindow): self.gyro_calibrated = {} self.level_calibrated = {} self.first_col_is_checked = False + self.player = QtMultimedia.QMediaPlayer() + self.loop = loop self.init_model() @@ -123,6 +139,11 @@ class MainWindow(QtWidgets.QMainWindow): self.ui.action_update_client_repo.triggered.connect(self.update_client_repo) 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) + self.ui.action_reset_z_offset.triggered.connect(self.reset_z_offset) + self.ui.action_select_music_file.triggered.connect(self.select_music_file) + self.ui.action_play_music.triggered.connect(self.play_music) + self.ui.action_test_music_after.triggered.connect(self.test_music_after) # Set most safety-important buttons disabled self.ui.start_button.setEnabled(False) @@ -178,6 +199,11 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def send_starttime_selected(self, **kwargs): dt = self.ui.start_delay_spin.value() + logging.info('Wait {} seconds to start animation'.format(dt)) + if self.ui.music_checkbox.isChecked(): + music_dt = self.ui.music_delay_spin.value() + asyncio.ensure_future(self.play_music_after(music_dt), loop=self.loop) + logging.info('Wait {} seconds to play music'.format(music_dt)) self.selfcheck_selected() for copter in self.model.user_selected(): if all_checks(copter): @@ -351,6 +377,59 @@ class MainWindow(QtWidgets.QMainWindow): for copter in self.model.user_selected(): copter.client.send_message("reset_start") + @pyqtSlot() + def set_z_offset_to_ground(self): + for copter in self.model.user_selected(): + copter.client.send_message("set_z_to_ground") + + @pyqtSlot() + def reset_z_offset(self): + for copter in self.model.user_selected(): + copter.client.send_message("reset_z_offset") + + @pyqtSlot() + def select_music_file(self): + path = QFileDialog.getOpenFileName(self, "Select music file", filter="Music files (*.mp3)")[0] + if path: + media = QUrl.fromLocalFile(path) + content = QtMultimedia.QMediaContent(media) + self.player.setMedia(content) + + @pyqtSlot() + def play_music(self): + if self.player.mediaStatus() == QtMultimedia.QMediaPlayer.InvalidMedia: + logger.info("Can't play media") + return + if self.player.mediaStatus() == QtMultimedia.QMediaPlayer.NoMedia: + logger.info("No media file") + return + + if self.player.state() == QtMultimedia.QMediaPlayer.StoppedState or \ + self.player.state() == QtMultimedia.QMediaPlayer.PausedState: + self.player.play() + else: + self.player.pause() + + + @asyncio.coroutine + def play_music_after(self, delay=0.): + if self.player.mediaStatus() == QtMultimedia.QMediaPlayer.InvalidMedia: + logger.info("Can't play media") + return + if self.player.mediaStatus() == QtMultimedia.QMediaPlayer.NoMedia: + logger.info("No media file") + return + self.player.stop() + wait(time.time()+delay) + logging.info("Play music") + self.player.play() + + @pyqtSlot() + def test_music_after(self): + dt = self.ui.music_delay_spin.value() + asyncio.ensure_future(self.play_music_after(dt), loop=self.loop) + logging.info('Wait {} seconds to play music'.format(dt)) + @pyqtSlot() def emergency(self): client_row_min = 0 @@ -401,13 +480,18 @@ class MainWindow(QtWidgets.QMainWindow): if __name__ == "__main__": app = QtWidgets.QApplication(sys.argv) - window = MainWindow() + loop = QEventLoop(app) + asyncio.set_event_loop(loop) + window = MainWindow(loop) Client.on_first_connect = window.client_connected server = Server(on_stop=app.quit) server.start() - app.exec_() + #app.exec_() + with loop: + loop.run_forever() + server.stop() sys.exit() diff --git a/builder/assets/clever-show.service b/builder/assets/clever-show.service index c93c431..e1885e5 100644 --- a/builder/assets/clever-show.service +++ b/builder/assets/clever-show.service @@ -3,9 +3,9 @@ Description=Clever Show Client Service After=clever.service [Service] -WorkingDirectory=/home/pi/CleverSwarm/Drone -EnvironmentFile=/lib/systemd/system/roscore.env -ExecStart=/usr/bin/python /home/pi/CleverSwarm/Drone/copter_client.py +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" KillSignal=SIGKILL Restart=on-failure RestartSec=3 diff --git a/builder/image-build.sh b/builder/image-build.sh index 7f6e132..543dd8a 100755 --- a/builder/image-build.sh +++ b/builder/image-build.sh @@ -2,7 +2,7 @@ set -e # Exit immidiately on non-zero result -SOURCE_IMAGE="https://github.com/CopterExpress/clever/releases/download/v0.17/clever_v0.17.img.zip" +SOURCE_IMAGE="https://github.com/CopterExpress/clever/releases/download/v0.18/clever_v0.18.img.zip" export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'} export LANG=${LANG:='C.UTF-8'} @@ -41,6 +41,7 @@ if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="$(cd ${REPO_DIR}; git log --forma # IMAGE_VERSION="${TRAVIS_TAG:=$(cd ${REPO_DIR}; git log --format=%h -1)}" REPO_URL="$(cd ${REPO_DIR}; git remote --verbose | grep origin | grep fetch | cut -f2 | cut -d' ' -f1 | sed 's/git@github\.com\:/https\:\/\/github.com\//')" REPO_NAME="$(basename -s '.git' ${REPO_URL})" +echo_stamp "REPO_NAME=${REPO_NAME}" "INFO" IMAGE_NAME="${REPO_NAME}_${IMAGE_VERSION}.img" echo_stamp "IMAGE_NAME=${IMAGE_NAME}" "INFO" IMAGE_PATH="${IMAGES_DIR}/${IMAGE_NAME}" @@ -99,10 +100,10 @@ echo_stamp "Mount dirs ${MOUNT_POINT} & ${MOUNT_POINT}/boot" mount "${DEV_IMAGE}p2" ${MOUNT_POINT} mount "${DEV_IMAGE}p1" ${MOUNT_POINT}/boot -mkdir -p ${MOUNT_POINT}'/home/pi/CleverSwarm/' +mkdir -p ${MOUNT_POINT}'/home/pi/clever-show/' for dir in ${REPO_DIR}/*; do if [[ $dir != *"images" && $dir != *"imgcache" ]]; then - cp -r $dir ${MOUNT_POINT}'/home/pi/CleverSwarm/'$(basename $dir) + cp -r $dir ${MOUNT_POINT}'/home/pi/clever-show/'$(basename $dir) fi; done diff --git a/builder/image-software.sh b/builder/image-software.sh index 9edee3e..9db2f4a 100755 --- a/builder/image-software.sh +++ b/builder/image-software.sh @@ -50,7 +50,7 @@ my_travis_retry() { } echo_stamp "Change repo owner to pi" -chown -Rf pi:pi /home/pi/CleverSwarm/ +chown -Rf pi:pi /home/pi/clever-show/ echo_stamp "Update apt cache" apt-get update -qq @@ -65,8 +65,13 @@ chrony \ echo_stamp "Install python libs" my_travis_retry pip install selectors2 -cd /home/pi/catkin_ws +echo_stamp "Install catkin packages" +cd /home/pi/catkin_ws/src +git clone https://github.com/CopterExpress/clever_tools.git +cd .. source devel/setup.bash +catkin_make --pkg clever_flight_routines catkin_make aruco_pose +source devel/setup.bash echo_stamp "End of software installation" diff --git a/messaging_lib.py b/messaging_lib.py index 64a44a7..d2444fa 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -1,6 +1,7 @@ import io import sys import json +import socket import struct import random import logging @@ -12,13 +13,28 @@ try: except ImportError: import selectors2 as selectors -#import logging_lib +# import logging_lib PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on", "callback", "callback_args", "callback_kwargs", ]) logger = logging.getLogger(__name__) -#logger = logging_lib.Logger(_logger, True) + + +# logger = logging_lib.Logger(_logger, True) + + +class _Singleton(type): + """ A metaclass that creates a Singleton base class when called. """ + _instances = {} + + def __call__(cls, *args, **kwargs): + if cls not in cls._instances: + cls._instances[cls] = super(_Singleton, cls).__call__(*args, **kwargs) + return cls._instances[cls] + + +class Singleton(_Singleton('SingletonMeta', (object,), {})): pass class MessageManager: @@ -196,14 +212,17 @@ class ConnectionManager(object): events = selectors.EVENT_READ | selectors.EVENT_WRITE else: raise ValueError("Invalid events mask mode {}.".format(mode)) - self.selector.modify(self.socket, events, data=self) + + key = self.selector.modify(self.socket, events, data=self) + logging.debug("Switched selector of {} to mode {}".format(self.addr, key.events)) + return key def connect(self, client_selector, client_socket, client_addr): self.selector = client_selector self.socket = client_socket self.addr = client_addr - self._set_selector_events_mask('rw') + self._set_selector_events_mask('r') def close(self): logger.info("Closing connection to {}".format(self.addr)) @@ -269,7 +288,8 @@ class ConnectionManager(object): def process_received(self, income_message): message_type = income_message.jsonheader["message-type"] - logger.debug("Received message! Header: {}, content: {}".format(income_message.jsonheader, income_message.content)) + logger.debug( + "Received message! Header: {}, content: {}".format(income_message.jsonheader, income_message.content)) if message_type == "message": self._process_message(income_message) @@ -339,6 +359,8 @@ class ConnectionManager(object): self._send_buffer += message if self._send_buffer: self._write() + else: + self._set_selector_events_mask('r') # we're done writing def _write(self): try: @@ -347,7 +369,8 @@ class ConnectionManager(object): # Resource temporarily unavailable (errno EWOULDBLOCK) pass except Exception as error: - logger.warning("Attempt to send message {} to {} failed due error: {}".format(self._send_buffer, self.addr, error)) + logger.warning( + "Attempt to send message {} to {} failed due error: {}".format(self._send_buffer, self.addr, error)) if not self.resume_queue: self._send_buffer = b'' @@ -361,6 +384,10 @@ class ConnectionManager(object): with self._send_lock: self._send_queue.append(data) + if self.selector.get_key(self.socket).events != selectors.EVENT_WRITE: + self._set_selector_events_mask('w') + NotifierSock().notify() + def get_response(self, requested_value, callback, request_args=None, # timeout=30, callback_args=(), callback_kwargs=None): if request_args is None: @@ -397,3 +424,43 @@ class ConnectionManager(object): self._send(MessageManager.create_message( data, "binary", "filetransfer", "binary", {"filepath": dest_filepath} )) + + +class NotifierSock(Singleton): + def __init__(self): + self.receive_socket = None + self.addr = None + + self._notify_socket = None + self._notify_lock = threading.Lock() + + def bind(self, server_addr): + self._notify_socket = socket.socket() + self._notify_socket.connect(server_addr) + logger.info("Notify socket: bind") + + def connect(self, _, client_socket, client_addr): + self.receive_socket = client_socket + self.addr = client_addr + + logger.info("Notify socket: connected") + + def notify(self): + with self._notify_lock: + if self.addr is not None: + self._notify_socket.sendall(bytes(1)) + logger.debug("Notify socket: notified") + + def process_events(self, mask): + if mask & selectors.EVENT_READ: + try: + data = self.receive_socket.recv(1024) + except Exception: # TODO remove + pass + else: + if data: + logger.debug("Notifier received {} from {}".format(data, self.addr)) + else: + self.addr = None + logger.warning("Notifier: connection to {} lost!".format(self.addr)) +