Merge branch 'master' into feature-branch

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
 

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

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:

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

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

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)

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

Server/server_config.ini

@@ -11,4 +11,4 @@ broadcast_delay = 5
 [NTP]
 use_ntp = False
 host = ntp1.stratum2.ru
-port = 123
+port = 123

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

Server/server_gui.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1220</width>
-    <height>750</height>
+    <height>761</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -91,6 +91,9 @@
             <property name="decimals">
              <number>1</number>
             </property>
+            <property name="maximum">
+             <double>1000.000000000000000</double>
+            </property>
            </widget>
           </item>
           <item row="4" column="1">
@@ -101,11 +104,17 @@
               <verstretch>0</verstretch>
              </sizepolicy>
             </property>
+            <property name="focusPolicy">
+             <enum>Qt::NoFocus</enum>
+            </property>
+            <property name="contextMenuPolicy">
+             <enum>Qt::DefaultContextMenu</enum>
+            </property>
             <property name="layoutDirection">
              <enum>Qt::RightToLeft</enum>
             </property>
             <property name="autoFillBackground">
-             <bool>true</bool>
+             <bool>false</bool>
             </property>
             <property name="text">
              <string/>
@@ -248,14 +257,14 @@
           <property name="formAlignment">
            <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
           </property>
-          <item row="3" column="1">
+          <item row="8" column="1">
            <widget class="QPushButton" name="land_button">
             <property name="text">
              <string>Land</string>
             </property>
            </widget>
           </item>
-          <item row="2" column="1">
+          <item row="7" column="1">
            <widget class="QPushButton" name="flip_button">
             <property name="text">
              <string>Flip</string>
@@ -279,6 +288,45 @@
             </property>
            </widget>
           </item>
+          <item row="4" column="1">
+           <layout class="QHBoxLayout" name="horizontalLayout_2">
+            <property name="topMargin">
+             <number>0</number>
+            </property>
+            <item>
+             <widget class="QCheckBox" name="z_checkbox">
+              <property name="cursor">
+               <cursorShape>ArrowCursor</cursorShape>
+              </property>
+              <property name="focusPolicy">
+               <enum>Qt::NoFocus</enum>
+              </property>
+              <property name="layoutDirection">
+               <enum>Qt::LeftToRight</enum>
+              </property>
+              <property name="text">
+               <string>  Z     =</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QDoubleSpinBox" name="z_spin">
+              <property name="layoutDirection">
+               <enum>Qt::LeftToRight</enum>
+              </property>
+              <property name="suffix">
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>1</number>
+              </property>
+              <property name="value">
+               <double>1.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+           </layout>
+          </item>
          </layout>
         </item>
         <item>
@@ -370,9 +418,11 @@
      <addaction name="action_restart_clever"/>
      <addaction name="action_restart_clever_show"/>
      <addaction name="action_update_client_repo"/>
+     <addaction name="action_reboot_all"/>
     </widget>
     <addaction name="action_set_z_offset_to_ground"/>
     <addaction name="action_reset_z_offset"/>
+    <addaction name="action_remove_row"/>
     <addaction name="separator"/>
     <addaction name="menuDeveloper_mode_2"/>
     <addaction name="action_remove_row"/>
@@ -417,7 +467,7 @@
   </action>
   <action name="action_send_launch_file">
    <property name="text">
-    <string>Send launch file to clever</string>
+    <string>Send launch files</string>
    </property>
   </action>
   <action name="action_restart_clever">
@@ -496,13 +546,18 @@
   <action name="action_remove_row">
    <property name="text">
     <string>Remove from table</string>
-   </property> 
+   </property>
   </action>
   <action name="action_send_calibrations">
    <property name="text">
     <string>Send camera calibrations</string>
    </property>
   </action>
+  <action name="action_reboot_all">
+   <property name="text">
+    <string>Reboot all</string>
+   </property>
+  </action>
  </widget>
  <tabstops>
   <tabstop>start_delay_spin</tabstop>

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

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

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}

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"

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: