Merge pull request #75 from CopterExpress/gps

Add GPS support

Drone/animation_lib.py

@@ -1,10 +1,12 @@
-import time
+import os
 import csv
 import copy
+import math
+import time
+import numpy
 import rospy
 import logging
 import threading
-import ConfigParser
 
 try:
     from FlightLib import FlightLib
@@ -15,198 +17,350 @@ try:
 except ImportError:
     print("Can't import LedLib")
 
-import tasking_lib as tasking
-
 logger = logging.getLogger(__name__)
 
 interrupt_event = threading.Event()
 
-anim_id = "Empty id"
+def moving(f1, f2, delta, x=True, y=True, z=True):
+    return ((abs(f1.x - f2.x) > delta) and x
+        or  (abs(f1.y - f2.y) > delta) and y
+        or  (abs(f1.z - f2.z) > delta) and z)
 
-# TODO refactor as class
-# TODO separate code for frames transformations (e.g. for gps)
+def get_numbers(frames):
+    numbers = []
+    if frames:
+        for frame in frames:
+            numbers.append(frame.number)
+    return numbers
 
-def get_id(filepath="animation.csv"):
-    global anim_id
-    try:
-        animation_file = open(filepath)
-    except IOError:
-        logger.debug("File {} can't be opened".format(filepath))
-        anim_id = "No animation"
-        return anim_id
-    else:
-        with animation_file:
-            csv_reader = csv.reader(
-                animation_file, delimiter=',', quotechar='|'
-            )
-            row_0 = csv_reader.next()
-            if len(row_0) == 1:
-                anim_id = row_0[0]
-                logger.debug("Got animation_id: {}".format(anim_id))
-            else:
-                anim_id = "Empty id"
-                logger.debug("No animation id in file")
-    return anim_id
+class Frame(object):
+    params_dict = {
+        "number": None,
+        "x": None,
+        "y": None,
+        "z": None,
+        "yaw": None,
+        "red": None,
+        "green": None,
+        "blue": None,
+        "delay": None,
+    }
+    def __init__(self, csv_row=None, delay=None):
+        for key, value in self.params_dict.items():
+            setattr(self, key, value)
+        if csv_row:
+            self.load_csv_row(csv_row)
+        if delay:
+            self.delay = delay
 
-def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1, z_ratio=1):
-    try:
-        animation_file = open(filepath)
-    except IOError:
-        logger.debug("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_frame = csv_reader.next()
-            except:
-                return float('nan'), float('nan')
-            if len(row_frame) == 1:
-                anim_id = row_frame[0]
-                logger.debug("Got animation_id: {}".format(row_frame[0]))
-                row_frame = csv_reader.next()
-            if len(row_frame) == 2:
-                logger.debug("Got frame delay: {}".format(row_frame[1]))
-                row_frame - csv_reader.next()
-            try:
-                frame_number, x, y, z, yaw, red, green, blue = row_frame
-            except:
-                return float('nan'), float('nan')
-    return float(x)*x_ratio, float(y)*y_ratio
+    def load_csv_row(self, csv_row):
+        number, x, y, z, yaw, red, green, blue = csv_row
+        self.number = int(number)
+        self.x = float(x)
+        self.y = float(y)
+        self.z = float(z)
+        self.yaw = float(yaw)
+        self.red = int(red)
+        self.green = int(green)
+        self.blue = int(blue)
 
+    def get_pos(self):
+        if None in [self.x, self.y, self.z]:
+            return []
+        else:
+            return [self.x, self.y, self.z]
 
-def load_animation(filepath="animation.csv", default_delay = 0.1, x0=0, y0=0, z0=0, x_ratio=1, y_ratio=1, z_ratio=1):
-    imported_frames = []
-    global anim_id
-    try:
-        animation_file = open(filepath)
-    except IOError:
-        logger.debug("File {} can't be opened".format(filepath))
-        anim_id = "No animation"
-    else:
-        with animation_file:
-            current_frame_delay = default_delay
-            csv_reader = csv.reader(
-                animation_file, delimiter=',', quotechar='|'
-            )
-            row_0 = csv_reader.next()
-            if len(row_0) == 1:
-                anim_id = row_0[0]
-                logger.debug("Got animation_id: {}".format(anim_id))
-            elif len(row_0) == 2:
-                current_frame_delay = float(row_0[1])
-                logger.debug("Got new frame delay: {}".format(current_frame_delay))
-            else:
-                logger.debug("No animation id in file")
-                frame_number, x, y, z, yaw, red, green, blue = row_0
-                imported_frames.append({
-                    'number': int(frame_number),
-                    '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),
-                    'blue': int(blue),
-                    'delay': current_frame_delay
-                })
-            for row in csv_reader:
-                if len(row) == 2:
-                    current_frame_delay = float(row[1])
+    def get_color(self):
+        if None in [self.red, self.green, self.blue]:
+            return []
+        else:
+            return [self.red, self.green, self.blue]
+
+    def set_yaw(self, yaw):
+        if yaw != "animation":
+            self.yaw = math.radians(float(yaw))
+
+    def pose_is_valid(self):
+        return self.get_pos() and (self.yaw is not None)
+
+class Animation(object):
+    def __init__(self, config=None, filepath="animation.csv"):
+        self.reset(filepath)
+        if config is not None:
+            self.update_frames(config, filepath)
+
+    def reset(self, filepath):
+        self.id = None
+        self.start_delay = 0
+        self.original_frames = None
+        self.static_begin_frames = None
+        self.static_begin_time = 0
+        self.takeoff_frames = None
+        self.takeoff_time = 0
+        self.route_frames = None
+        self.route_time = 0
+        self.land_frames = None
+        self.land_time = 0
+        self.static_end_frames = None
+        self.static_end_time = 0
+        self.output_frames = None
+        self.output_frames_min_z = None
+        self.filepath = filepath
+        self.state = None
+
+    def load(self, filepath="animation.csv", config = None):
+        self.original_frames = []
+        self.corrected_frames = []
+        self.filepath = filepath
+        self.state = "OK"
+        delay = config.animation_frame_delay
+        try:
+            animation_file = open(filepath)
+        except IOError:
+            logger.debug("File {} can't be opened".format(filepath))
+            self.state = "No animation"
+        else:
+            with animation_file:
+                current_frame_delay = delay
+                csv_reader = csv.reader(
+                    animation_file, delimiter=',', quotechar='|'
+                )
+                row_0 = csv_reader.next()
+                if len(row_0) == 1:
+                    self.id = row_0[0]
+                    logger.debug("Got animation_id: {}".format(self.id))
+                elif len(row_0) == 2:
+                    current_frame_delay = float(row_0[1])
+                    logger.debug("Got new frame delay: {}".format(current_frame_delay))
                 else:
-                    frame_number, x, y, z, yaw, red, green, blue = row
-                    imported_frames.append({
-                        'number': int(frame_number),
-                        '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),
-                        'blue': int(blue),
-                        'delay': current_frame_delay
-                    })
-        return imported_frames
+                    logger.debug("No animation id in file")
+                    self.id = "No animation id"
+                    try:
+                        frame = Frame(row_0, current_frame_delay)
+                    except ValueError as e:
+                        logger.error("Can't parse row in csv file. {}".format(e))
+                        self.state = "Bad animation file"
+                        return
+                    try:
+                        frame.set_yaw(config.animation_yaw)
+                    except ValueError as e:
+                        logger.error("Can't set yaw from configuration")
+                        self.state = "Bad yaw from config"
+                        return
+                    self.original_frames.append(frame)
+                for row in csv_reader:
+                    if len(row) == 2:
+                        current_frame_delay = float(row[1])
+                        logger.debug("Got new frame delay: {}".format(current_frame_delay))
+                    else:
+                        try:
+                            frame = Frame(row, current_frame_delay)
+                        except ValueError as e:
+                            logger.error("Can't parse row in csv file. {}".format(e))
+                            self.state = "Bad animation file"
+                            return
+                        try:
+                            frame.set_yaw(config.animation_yaw)
+                        except ValueError as e:
+                            logger.error("Can't set yaw from configuration")
+                            self.state = "Bad yaw from config"
+                            return
+                        self.original_frames.append(frame)
+        self.split_animation()
 
-def correct_animation(frames, frame_delay=0.1, min_takeoff_height=0.5, move_delta=0.01, check_takeoff=True, check_land=True):
-    corrected_frames = copy.deepcopy(frames)
-    start_action = 'takeoff'
-    frames_to_start = 0
-    time_to_start = 0
-    if len(corrected_frames) == 0:
-        raise Exception('Nothing to correct!')
-    # Check takeoff
-    # If copter takes off in animation file, copter must be armed first and then all animation can be played
-    if (corrected_frames[0]['z'] < min_takeoff_height) and check_takeoff:
-        start_action = 'arm'
-        # If the first point is low, then detect moment to arm,
-        # delete all points, where copter is standing, and count time_delta
-        for i in range(len(corrected_frames)-1):
-            if corrected_frames[i-frames_to_start+1]['z'] - corrected_frames[i-frames_to_start]['z'] > move_delta:
+    '''
+    Split animation into 5 parts: static_begin, takeoff, route, land, static_end
+        * static_begin and static_end are chains of frames in the beginning and the end of animation,
+            where the drone doesn't move
+        * takeoff and land are chains of frames after and before static frames of animation,
+            where the drone doesn't move in xy plane, and it's z coordinate only increases or decreases, respectively.
+        * route is the rest of the animation
+    Count static_begin_time and takeoff_time
+    '''
+    def split_animation(self, move_delta=0.01):
+        self.static_begin_frames = []
+        self.takeoff_frames = []
+        self.route_frames = []
+        self.land_frames = []
+        self.static_end_frames = []
+        self.static_begin_time = 0
+        self.takeoff_time = 0
+        self.route_time = 0
+        self.land_time = 0
+        if len(self.original_frames) == 0:
+            return
+        frames = copy.deepcopy(self.original_frames)
+        i = 0 # Moving index from the beginning
+        # Select static begin frames
+        while i < len(frames) - 1:
+            self.static_begin_time += frames[i].delay
+            if moving(frames[i], frames[i+1], move_delta):
                 break
-            time_to_start += corrected_frames[i-frames_to_start]['delay']
-            del corrected_frames[i-frames_to_start]
-            frames_to_start += 1
-    start_delay = time_to_start
-    # Check Land
-    # If copter lands in animation, landing points can be deleted
-    if (corrected_frames[len(corrected_frames)-1]['z'] < min_takeoff_height) and check_land:
-        for i in range(len(corrected_frames)-1,0,-1):
-            # print i
-            if abs(corrected_frames[i-1]['z'] - corrected_frames[i]['z']) < move_delta:
+            i += 1
+        if i > 0:
+            self.static_begin_frames = frames[:i+1]
+            frames = frames[i+1:]
+            i = 0
+        else:
+            self.static_begin_time = 0
+        # Select takeoff frames
+        while i < len(frames) - 1:
+            self.takeoff_time += frames[i].delay
+            if moving(frames[i], frames[i+1], move_delta, z = False) or (frames[i+1].z - frames[i].z <= 0):
                 break
-            del corrected_frames[i]
-        for i in range(len(corrected_frames)-1,0,-1):
-            if (abs(corrected_frames[i-1]['x'] - corrected_frames[i]['x']) > move_delta or
-                abs(corrected_frames[i-1]['y'] - corrected_frames[i]['y']) > move_delta):
+            i += 1
+        if i > 0:
+            self.takeoff_frames = frames[:i+1]
+            frames = frames[i+1:]
+        else:
+            self.takeoff_time = 0
+        i = len(frames) - 1 # Moving index from the end
+        # Select static end frames
+        while i >= 0:
+            self.static_end_time += frames[i].delay
+            if moving(frames[i], frames[i-1], move_delta):
                 break
-            del corrected_frames[i]
-    return corrected_frames, start_action, start_delay
+            i -= 1
+        if i < len(frames) - 1:
+            self.static_end_frames = frames[i+1:]
+            frames = frames[:i+1]
+            i = len(frames) - 1
+        else:
+            self.static_end_time = 0
+        # Select land frames
+        while i >= 0:
+            self.land_time += frames[i].delay
+            if moving(frames[i], frames[i-1], move_delta, z = False) or (frames[i-1].z - frames[i].z <= 0):
+                break
+            i -= 1
+        if i < len(frames) - 1:
+            self.land_frames = frames[i+1:]
+            frames = frames[:i+1]
+        else:
+            self.land_time = 0
+        # Get route frames
+        self.route_frames = frames
+        for frame in self.route_frames:
+            self.route_time += frame.delay
 
-# Needs for test
-def save_corrected_animation(frames, filename="corrected_animation.csv"):
-    corrected_animation = open(filename, mode='w+')
-    csv_writer = csv.writer(corrected_animation, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
-    for frame in frames:
-        csv_writer.writerow([frame['number'],frame['x'], frame['y'], frame['z'], frame['delay']])
-        # print frame
-    corrected_animation.close()
+    def make_output_frames(self, static_begin, takeoff, route, land, static_end):
+        self.output_frames = []
+        self.output_frames_min_z = None
+        self.start_delay = 0.
+        if not self.original_frames:
+            return
+        if not static_begin and not takeoff and not route and not land and not static_end:
+            return
+        if static_begin:
+            self.output_frames += self.static_begin_frames
+        if not static_begin:
+            self.start_delay += self.static_begin_time
+        if takeoff:
+            self.output_frames += self.takeoff_frames
+        if not static_begin and not takeoff:
+            self.start_delay += self.takeoff_time
+        if route:
+            self.output_frames += self.route_frames
+        if not static_begin and not takeoff and not route:
+            self.start_delay += self.route_time
+        if land:
+            self.output_frames += self.land_frames
+        if not static_begin and not takeoff and not route and not land:
+            self.start_delay += self.land_time
+        if static_end:
+            self.output_frames += self.static_end_frames
+        if self.output_frames:
+            self.output_frames_min_z = min(self.output_frames, key = lambda p: p.z).z
 
-def convert_frame(frame):
-    return ((frame['x'], frame['y'], frame['z']), (frame['red'], frame['green'], frame['blue']), frame['yaw'])
+    def update_frames(self, config, filepath):
+        self.reset(filepath)
+        self.load(filepath, config)
+        self.make_output_frames(config.animation_output_static_begin,
+                                    config.animation_output_takeoff,
+                                    config.animation_output_route,
+                                    config.animation_output_land,
+                                    config.animation_output_static_end)
+
+    def get_scaled_output(self, ratio=(1,1,1), offset=(0,0,0)):
+        x0, y0, z0 = offset
+        x_ratio, y_ratio, z_ratio = ratio
+        scaled_frames = copy.deepcopy(self.output_frames)
+        if scaled_frames:
+            for frame in scaled_frames:
+                frame.x = x_ratio*frame.x + x0
+                frame.y = y_ratio*frame.y + y0
+                frame.z = z_ratio*frame.z + z0
+        return scaled_frames
+
+    def get_scaled_output_min_z(self, ratio=(1,1,1), offset=(0,0,0)):
+        if self.output_frames_min_z is None:
+            return None
+        z0 = offset[2]
+        z_ratio = ratio[2]
+        return self.output_frames_min_z*z_ratio + z0
+
+    def get_start_point(self, ratio=(1,1,1), offset=(0,0,0)):
+        if not self.output_frames:
+            return []
+        x0, y0, z0 = offset
+        x_ratio, y_ratio, z_ratio = ratio
+        first_frame = self.output_frames[0]
+        x = x_ratio*first_frame.x + x0
+        y = y_ratio*first_frame.y + y0
+        z = z_ratio*first_frame.z + z0
+        return [x, y, z]
+
+    def get_start_yaw(self):
+        if not self.output_frames:
+            return float('nan')
+        return math.degrees(self.output_frames[0].yaw)
+
+    def check_ground(self, ground_level=0, ratio=(1,1,1), offset=(0,0,0)):
+        return ground_level <= self.get_scaled_output_min_z(ratio, offset)
+
+    def get_start_action(self, start_action, current_height, takeoff_level,
+                                ground_level, ratio=(1,1,1), offset=(0,0,0)):
+        # Check output frames
+        if not self.output_frames:
+            return 'error: empty output frames'
+        if math.isnan(current_height):
+            return 'error: bad current_height'
+        # Check that bottom point of animation is higher than ground level
+        if ground_level > self.get_scaled_output_min_z(ratio, offset):
+            return 'error: some animation points are lower than ground level'
+        # Select start action
+        if start_action == 'auto':
+            if self.get_start_point(ratio, offset)[2] - current_height > takeoff_level:
+                return 'takeoff'
+            else:
+                return 'fly'
+        elif start_action in ('takeoff', 'fly'):
+            return start_action
+        else:
+            return 'error'
+
+    # Need for tests
+    def save_corrected_animation(self):
+        name, ext = os.path.splitext(self.filepath)
+        filepath = name + '_corrected' + ext
+        with open(filepath, mode='w+') as corrected_animation:
+            csv_writer = csv.writer(corrected_animation, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
+            for frame in self.corrected_frames:
+                csv_writer.writerow([frame.number, frame.x, frame.y, frame.z, frame.red, frame.green, frame.blue, frame.delay])
 
 try:
-    def execute_frame(point=(), color=(), yaw=float('Nan'), frame_id='aruco_map', use_leds=True,
+    def execute_frame(frame, frame_id='aruco_map', use_leds=True,
                     flight_func=FlightLib.navto, auto_arm=False, flight_kwargs=None, interrupter=interrupt_event):
         if flight_kwargs is None:
             flight_kwargs = {}
-
-        flight_func(*point, yaw=yaw, frame_id=frame_id, auto_arm=auto_arm, interrupter=interrupt_event, **flight_kwargs)
+        if frame.pose_is_valid():
+            flight_func(x=frame.x, y=frame.y, z=frame.z, yaw=frame.yaw, frame_id=frame_id, auto_arm=auto_arm, interrupter=interrupt_event, **flight_kwargs)
+        else:
+            logger.debug("Frame pose is not valid for flying")
         if use_leds:
-            if color:
+            if frame.get_color:
                 LedLib.fill(*color)
 
-
-
-
-    def execute_animation(frames, frame_delay, frame_id='aruco_map', use_leds=True, flight_func=FlightLib.navto,
-                        interrupter=interrupt_event):
-        next_frame_time = 0
-        for frame in frames:
-            if interrupter.is_set():
-                logger.warning("Animation playing function interrupted!")
-                interrupter.clear()
-                return
-            execute_frame(*convert_frame(frame), frame_id=frame_id, use_leds=use_leds, flight_func=flight_func,
-                        interrupter=interrupter)
-
-            next_frame_time += frame_delay
-            tasking.wait(next_frame_time, interrupter)
-
-
     def takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, use_leds=True,
                 interrupter=interrupt_event):
         if use_leds:

Drone/config/spec/configspec_client.ini

@@ -39,11 +39,11 @@ decrease_thrust_after = float(default=5.0, min=0)
 
 [COPTER]
 frame_id = string(default=map)
+arming_time = float(default=0.5)
 takeoff_height = float(default=1.0)
 takeoff_time = float(default=5.0, min=0)
-safe_takeoff = boolean(default=False)
 reach_first_point_time = float(default=5.0, min=0)
-land_time = float(default=1.0, min=0)
+land_delay = float(default=1.0, min=0)
 land_timeout = float(default=10.0, min=0)
 # __list__ x y z
 common_offset = float_list(default=list(0, 0, 0), min=3, max=3)
@@ -58,19 +58,33 @@ translation = float_list(default=list(0.0, 0.0, 0.0), min=3, max=3)
 rotation = float_list(default=list(0.0, 0.0, 0.0), min=3, max=3)
 
 [GPS FRAME]
-lat = float(default=0)
-lon = float(default=0)
+lat = string(default=0)
+lon = string(default=0)
 yaw = float(default=0)
 
 [ANIMATION]
-takeoff_detection = boolean(default=True)
-land_detection = boolean(default=True)
+# Available options:
+# 'auto' - automatic action selection from 'takeoff' or 'fly' based on current copter level
+# 'takeoff' - takeoff to first output animation point after static_begin_time then execute 'takeoff logic'
+# 'fly' - execute animation frames after static_begin_time
+start_action = string(default=auto)
+takeoff_level = float(default=0.5)
+ground_level = float(default=0)
 frame_delay = float(default=0.1, min=0.01)
 # Animation ratio (x, y, z)
 # __list__ x y z
 ratio = float_list(default=list(1.0, 1.0, 1.0), min=3, max=3)
-# Available options: 'animation', 'nan' or a number in degrees
+# Available options:
+# 'animation' - take yaw from animation
+# 'nan' - don't change yaw during flight
+# or a number in degrees
 yaw = string(default=180.0)
+[[OUTPUT]]
+static_begin = boolean(default=False)
+takeoff = boolean(default=True)
+route = boolean(default=True)
+land = boolean(default=False)
+static_end = boolean(default=False)
 
 [LED]
 use = boolean(default=False)

Drone/copter_client.py

@@ -9,7 +9,10 @@ import numpy
 try:
     from clever import srv
 except ImportError:
-    from clover import srv
+    try:
+        from clover import srv
+    except ImportError:
+        print("Can't import clever or clover")
 
 import datetime
 import logging
@@ -17,8 +20,17 @@ import threading
 import psutil
 import subprocess
 from collections import namedtuple
+from watchdog.observers import Observer
+from watchdog.events import FileSystemEventHandler
 
-from FlightLib import FlightLib
+try:
+    from FlightLib import FlightLib
+except ImportError:
+    print("Can't import FlightLib")
+try:
+    from FlightLib import LedLib
+except ImportError:
+    print("Can't import LedLib")
 
 import client
 
@@ -33,7 +45,26 @@ from geometry_msgs.msg import Point, Quaternion, TransformStamped
 from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_multiply
 import tf2_ros
 
-static_bloadcaster = tf2_ros.StaticTransformBroadcaster()
+from geographiclib.geodesic import Geodesic
+
+Earth = Geodesic.WGS84
+
+def dist(x, y):
+    return math.sqrt(x**2+y**2)
+
+def azi(x, y):
+    return 90 - math.atan2(y,x)*180/math.pi
+
+def get_xy(dist, azi):
+    return dist*math.sin(math.radians(azi)), dist*math.cos(math.radians(azi))
+
+def valid(pos):
+    for coord in pos:
+        if math.isnan(coord):
+            return False
+    return True
+
+static_broadcaster = tf2_ros.StaticTransformBroadcaster()
 
 emergency = False
 
@@ -83,7 +114,8 @@ class CopterClient(client.Client):
     def __init__(self, config_path="config/client.ini"):
         super(CopterClient, self).__init__(config_path)
         self.load_config()
-        self.frames = {}
+        self.telemetry = None
+        self.animation = animation.Animation(self.config, "animation.csv")
 
     def load_config(self):
         super(CopterClient, self).load_config()
@@ -97,17 +129,22 @@ class CopterClient(client.Client):
         if self.config.led_use:
             from FlightLib import LedLib
             LedLib.init_led(self.config.led_pin)
-        task_manager_instance.start()  # TODO move to self
+        task_manager_instance.start()
+        start_subscriber()
+        self.telemetry = Telemetry()
+        self.telemetry.start_loop()
         if self.config.copter_frame_id == "floor":
             self.start_floor_frame_broadcast()
         elif self.config.copter_frame_id == "gps":
             self.start_gps_frame_broadcast()
-        start_subscriber()
-
-        telemetry.start_loop()
-        super(CopterClient, self).start()
+        client_thread = threading.Thread(target=super(CopterClient, self).start, name="Client thread")
+        client_thread.daemon = True
+        client_thread.start()
+        #super(CopterClient, self).start()
 
     def start_floor_frame_broadcast(self):
+        if self.config.floor_frame_parent == "gps":
+            self.start_gps_frame_broadcast()
         trans = TransformStamped()
         trans.transform.translation.x = self.config.floor_frame_translation[0]
         trans.transform.translation.y = self.config.floor_frame_translation[1]
@@ -117,10 +154,49 @@ class CopterClient(client.Client):
                                                                      math.radians(self.config.floor_frame_rotation[2])))
         trans.header.frame_id = self.config.floor_frame_parent
         trans.child_frame_id = self.config.copter_frame_id
-        static_bloadcaster.sendTransform(trans)
+        static_broadcaster.sendTransform(trans)
 
     def start_gps_frame_broadcast(self):
-        return
+        trans = TransformStamped()
+        trans.transform.translation.x = 0
+        trans.transform.translation.y = 0
+        trans.transform.translation.z = 0
+        trans.transform.rotation = Quaternion(*quaternion_from_euler(0,0,0))
+        trans.header.frame_id = "map"
+        trans.child_frame_id = "gps"
+        static_broadcaster.sendTransform(trans)
+        gps_frame_thread = threading.Thread(target=self.gps_frame_broadcast_loop, name="GPS frame broadcast thread")
+        gps_frame_thread.daemon = True
+        gps_frame_thread.start()
+
+    def gps_frame_broadcast_loop(self):
+        rate = rospy.Rate(1)
+        while not rospy.is_shutdown():
+            telem = FlightLib.get_telemetry_locked(frame_id = "map")
+            if telem is not None:
+                if math.isnan(telem.lat) or math.isnan(telem.lon) or math.isnan(telem.x) or math.isnan(telem.y):
+                    logger.info("Can't get position from telemetry")
+                else:
+                    lat = float(self.config.gps_frame_lat)
+                    lon = float(self.config.gps_frame_lon)
+                    geo_delta = Earth.Inverse(telem.lat, telem.lon, lat, lon)
+                    #logger.info("dist: {} | azi: {}".format(geo_delta['s12'], geo_delta['azi1']))
+                    dx, dy = get_xy(geo_delta['s12'], geo_delta['azi1'])
+                    gps_dx = telem.x + dx
+                    gps_dy = telem.y + dy
+                    #logger.info("GPS frame dx: {} | dy: {}".format(gps_dx, gps_dy))
+                    trans = TransformStamped()
+                    trans.transform.translation.x = gps_dx
+                    trans.transform.translation.y = gps_dy
+                    trans.transform.translation.z = 0
+                    trans.transform.rotation = Quaternion(*quaternion_from_euler(0,0,
+                                                            math.radians(self.config.gps_frame_yaw)))
+                    trans.header.frame_id = "map"
+                    trans.child_frame_id = "gps"
+                    static_broadcaster.sendTransform(trans)
+
+            rate.sleep()
+
 
 def restart_service(name):
     os.system("systemctl restart {}".format(name))
@@ -260,13 +336,13 @@ def _execute(*args, **kwargs):
 def _response_id(*args, **kwargs):
     new_id = kwargs.get("new_id", None)
     if new_id is not None:
-        old_id = client.active_client.client_id
+        old_id = copter.client_id
         if new_id != old_id:
-            client.active_client.config.set('PRIVATE', 'id', new_id, write=True)
-            client.active_client.client_id = new_id
+            copter.config.set('PRIVATE', 'id', new_id, write=True)
+            copter.client_id = new_id
             if new_id != '/hostname':
-                if client.active_client.config.system_restart_after_rename:
-                    hostname = client.active_client.client_id
+                if copter.config.system_restart_after_rename:
+                    hostname = copter.client_id
                     configure_hostname(hostname)
                     configure_hosts(hostname)
                     configure_bashrc(hostname)
@@ -293,28 +369,14 @@ def _response_selfcheck(*args, **kwargs):
 
 @messaging.request_callback("telemetry")
 def _response_telemetry(*args, **kwargs):
-    telemetry.update()
-    return telemetry.create_msg_contents()
+    copter.telemetry.update()
+    return copter.telemetry.create_msg_contents()
 
 
 @messaging.request_callback("anim_id")
 def _response_animation_id(*args, **kwargs):
     # Load animation
-    result = animation.get_id()
-    if result != 'No animation':
-        logger.debug("Saving corrected animation")
-        offset = numpy.array(client.active_client.config.private_offset) + numpy.array(client.active_client.config.copter_common_offset)
-        frames = animation.load_animation(os.path.abspath("animation.csv"), client.active_client.config.animation_frame_delay,
-                                            offset[0], offset[1], offset[2], *client.active_client.config.animation_ratio)
-        # Correct start and land frames in animation
-        corrected_frames, start_action, start_delay = animation.correct_animation(frames,
-                                                        check_takeoff=client.active_client.config.animation_takeoff_detection,
-                                                        check_land=client.active_client.config.animation_land_detection,
-                                                        )
-        logger.debug("Start action: {}".format(start_action))
-        # Save corrected animation
-        animation.save_corrected_animation(corrected_frames)
-    return result
+    return copter.animation.id
 
 
 @messaging.request_callback("batt_voltage")
@@ -351,9 +413,9 @@ def _response_cal_status(*args, **kwargs):
 
 @messaging.request_callback("position")
 def _response_position(*args, **kwargs):
-    telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
+    telem = copter.telemetry.ros_telemetry
     return "{:.2f} {:.2f} {:.2f} {:.1f} {}".format(
-        telem.x, telem.y, telem.z, math.degrees(telem.yaw), client.active_client.config.copter_frame_id)
+        telem.x, telem.y, telem.z, math.degrees(telem.yaw), copter.config.copter_frame_id)
 
 
 @messaging.request_callback("calibrate_gyro")
@@ -382,60 +444,61 @@ def _command_test(*args, **kwargs):
     print("stdout test")
 
 
+@messaging.message_callback("update_animation")
+def _command_update_animation(*args, **kwargs):
+    copter.animation.update_frames(copter.config, "animation.csv")
+
+
 @messaging.message_callback("move_start")
 def _command_move_start_to_current_position(*args, **kwargs):
-    x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"),
-                                            *client.active_client.config.animation_ratio)
-    logger.debug("x_start = {}, y_start = {}".format(x_start, y_start))
-    if not math.isnan(x_start):
-        telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
-        logger.debug("x_telem = {}, y_telem = {}".format(telem.x, telem.y))
-        if not math.isnan(telem.x):
-            client.active_client.config.set('PRIVATE', 'offset',
-                                            [telem.x - x_start, telem.y - y_start, client.active_client.config.private_offset[2]],
-                                            write=True)
-            logger.info("Set start delta: {:.2f} {:.2f}".format(client.active_client.config.private_offset[0],
-                                                                client.active_client.config.private_offset[1]))
-        else:
-            logger.debug("Wrong telemetry")
+    offset = numpy.array(copter.config.private_offset) + numpy.array(copter.config.copter_common_offset)
+    try:
+        xs, ys, zs = copter.animation.get_start_point(copter.config.ratio, offset)
+    except ValueError:
+        logger.error("Can't get start point. Check animation file!")
     else:
-        logger.debug("Wrong animation file")
+        logger.debug("start x = {}, y = {}".format(xs, ys))
+        telem = copter.telemetry.ros_telemetry
+        logger.debug("telemetry x = {}, y = {}".format(telem.x, telem.y))
+        if valid([telem.x, telem.y, telem.z]):
+            copter.config.set('PRIVATE', 'offset',
+                [telem.x - xs, telem.y - ys, copter.config.private_offset[2]], write=True)
+            logger.info("Set start delta: {:.2f} {:.2f}".format(copter.config.private_offset[0],
+                                                                copter.config.private_offset[1]))
+        else:
+            logger.error("Wrong telemetry")
 
 
 @messaging.message_callback("reset_start")
 def _command_reset_start(*args, **kwargs):
-    client.active_client.config.set('PRIVATE', 'offset',
-                                    [0, 0, client.active_client.config.private_offset[2]],
-                                    write=True)
-    logger.info("Reset start to {:.2f} {:.2f}".format(client.active_client.config.private_offset[0],
-                                                    client.active_client.config.private_offset[1]))
+    copter.config.set('PRIVATE', 'offset',
+                                    [0, 0, copter.config.private_offset[2]], write=True)
+    logger.info("Reset start to {:.2f} {:.2f}".format(copter.config.private_offset[0],
+                                                    copter.config.private_offset[1]))
 
 
 @messaging.message_callback("set_z_to_ground")
 def _command_set_z(*args, **kwargs):
-    telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
-    client.active_client.config.set('PRIVATE', 'offset',
-                                    [client.active_client.config.private_offset[0], client.active_client.config.private_offset[1], telem.z],
-                                    write=True)
-    logger.info("Set z offset to {:.2f}".format(client.active_client.config.private_offset[2]))
+    telem = copter.telemetry.ros_telemetry
+    if valid([telem.x, telem.y, telem.z]):
+        copter.config.set('PRIVATE', 'offset',
+            [copter.config.private_offset[0], copter.config.private_offset[1], telem.z], write=True)
+        logger.info("Set z offset to {:.2f}".format(copter.config.private_offset[2]))
+    else:
+        logger.error("Wrong telemetry")
 
 
 @messaging.message_callback("reset_z_offset")
 def _command_reset_z(*args, **kwargs):
-    client.active_client.config.set('PRIVATE', 'offset',
-                                    [client.active_client.config.private_offset[0], client.active_client.config.private_offset[1], 0],
-                                    write=True)
-    logger.info("Reset z offset to {:.2f}".format(client.active_client.config.private_offset[2]))
+    copter.config.set('PRIVATE', 'offset',
+        [copter.config.private_offset[0], copter.config.private_offset[1], 0], write=True)
+    logger.info("Reset z offset to {:.2f}".format(copter.config.private_offset[2]))
 
 
 @messaging.message_callback("update_repo")
 def _command_update_repo(*args, **kwargs):
-    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 --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")
 
 
@@ -453,12 +516,16 @@ def _command_reboot(*args, **kwargs):
 @messaging.message_callback("service_restart")
 def _command_service_restart(*args, **kwargs):
     service = kwargs["name"]
+    if service=="clover":
+        restart_service("clever")
+    if service=="clever-show":
+        restart_service("clever-show@{}".format(copter.client_id))
     restart_service(service)
 
 
 @messaging.message_callback("repair_chrony")
 def _command_chrony_repair(*args, **kwargs):
-    repair_chrony(client.active_client.config.server_host)
+    repair_chrony(copter.config.server_host)
 
 
 @messaging.message_callback("led_test")
@@ -473,13 +540,12 @@ def _command_led_fill(*args, **kwargs):
     r = kwargs.get("red", 0)
     g = kwargs.get("green", 0)
     b = kwargs.get("blue", 0)
-
     LedLib.fill(r, g, b)
 
 
 @messaging.message_callback("flip")
 def _copter_flip(*args, **kwargs):
-    FlightLib.flip(frame_id=client.active_client.config.copter_frame_id)
+    FlightLib.flip(frame_id=copter.config.copter_frame_id)
 
 
 @messaging.message_callback("takeoff")
@@ -487,30 +553,36 @@ def _command_takeoff(*args, **kwargs):
     logger.info("Takeoff at {}".format(datetime.datetime.now()))
     task_manager.add_task(0, 0, animation.takeoff,
                           task_kwargs={
-                              "z": client.active_client.config.copter_takeoff_height,
-                              "timeout": client.active_client.config.copter_takeoff_time,
-                              "safe_takeoff": client.active_client.config.copter_safe_takeoff,
-                              "use_leds": client.active_client.config.led_use & client.active_client.config.led_takeoff_indication,
-                          }
-                          )
+                              "z": copter.config.copter_takeoff_height,
+                              "timeout": copter.config.copter_takeoff_time,
+                              "safe_takeoff": False,
+                              "use_leds": copter.config.led_use & copter.config.led_takeoff_indication,
+                          })
 
 
 @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_locked(client.active_client.config.copter_frame_id)
-        logger.info("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now()))
-        task_manager.add_task(0, 0, FlightLib.reach_point,
-                              task_kwargs={
-                                  "x": telem.x,
-                                  "y": telem.y,
-                                  "z": float(z_str),
-                                  "frame_id": client.active_client.config.copter_frame_id,
-                                  "timeout": client.active_client.config.copter_takeoff_time,
-                                  "auto_arm": True,
-                              }
-                              )
+    try:
+        z = float(kwargs.get("z", None))
+    except TypeError:
+        logger.error("takeoff_z: No z argument!")
+    except ValueError:
+        logger.error("takeoff_z: Wrong z argument!")
+    else:
+        telem = FlightLib.get_telemetry_locked(copter.config.copter_frame_id)
+        if valid([telem.x, telem.y, telem.z]):
+            logger.info("Takeoff to z = {} at {}".format(z, datetime.datetime.now()))
+            task_manager.add_task(0, 0, FlightLib.reach_point,
+                                task_kwargs={
+                                    "x": telem.x,
+                                    "y": telem.y,
+                                    "z": z,
+                                    "frame_id": copter.config.copter_frame_id,
+                                    "timeout": copter.config.copter_takeoff_time,
+                                    "auto_arm": True,
+                                })
+        else:
+            logger.error("Wrong telemetry!")
 
 
 @messaging.message_callback("land")
@@ -518,12 +590,11 @@ def _command_land(*args, **kwargs):
     task_manager.reset()
     task_manager.add_task(0, 0, animation.land,
                           task_kwargs={
-                              "z": client.active_client.config.copter_takeoff_height,
-                              "timeout": client.active_client.config.copter_takeoff_time,
-                              "frame_id": client.active_client.config.copter_frame_id,
-                              "use_leds": client.active_client.config.led_use & client.active_client.config.led_land_indication,
-                          }
-                          )
+                              "z": copter.config.copter_takeoff_height,
+                              "timeout": copter.config.copter_land_timeout,
+                              "frame_id": copter.config.copter_frame_id,
+                              "use_leds": copter.config.led_use & copter.config.led_land_indication,
+                          })
 
 
 @messaging.message_callback("emergency_land")
@@ -537,8 +608,7 @@ def _command_disarm(*args, **kwargs):
     task_manager.add_task(-5, 0, FlightLib.arming_wrapper,
                           task_kwargs={
                               "state": False
-                          }
-                          )
+                          })
 
 
 @messaging.message_callback("stop")
@@ -558,109 +628,107 @@ def _command_resume(*args, **kwargs):
 
 @messaging.message_callback("start")
 def _play_animation(*args, **kwargs):
-    start_time = float(kwargs["time"])
-    # Check if animation file is available
-    if animation.get_id() == 'No animation':
-        logger.error("Can't start animation without animation file!")
+
+    # Validate start_time
+    try:
+        start_time = float(kwargs["time"])
+    except ValueError:
+        logger.error("start: Wrong time argument!")
+        return
+    except KeyError:
+        logger.error("start: No time argument!")
         return
 
+    # Check animation state
+    if copter.animation.state is not "OK":
+        logger.error("start: Bad animation state")
+        return
+
+    # Get output frames
+    offset = numpy.array(copter.config.private_offset) + numpy.array(copter.config.copter_common_offset)
+    frames = copter.animation.get_scaled_output(copter.config.animation_ratio, offset)
+    if not frames:
+        logger.error("start: No frames in animation!")
+        return
+
+    # Get current telemetry
+    telem = copter.telemetry.ros_telemetry
+    if not valid([telem.x, telem.y, telem.z]):
+        logger.error("start: Position is not valid!")
+        return
+
+    # Get start action and delay
+    try:
+        start_action, start_delay = copter.telemetry.start_position[-2:]
+    except ValueError:
+        logger.error("start: Can't get animation start position and delay")
+        return
+    # Reset task manager
     task_manager.reset(interrupt_next_task=False)
 
-    logger.info("Start time = {}, wait for {} seconds".format(start_time, start_time - time.time()))
-    # Load animation
-    offset = numpy.array(client.active_client.config.private_offset) + numpy.array(client.active_client.config.copter_common_offset)
-    frames = animation.load_animation(os.path.abspath("animation.csv"), client.active_client.config.animation_frame_delay, 
-                                            offset[0], offset[1], offset[2], *client.active_client.config.animation_ratio)
-    # Correct start and land frames in animation
-    corrected_frames, start_action, start_delay = animation.correct_animation(frames,
-                                                        check_takeoff=client.active_client.config.animation_takeoff_detection,
-                                                        check_land=client.active_client.config.animation_land_detection,
-                                                        )
-    # Choose start action
+    # Set animation logic
     if start_action == 'takeoff':
-        # Takeoff first
-        task_manager.add_task(start_time, 0, animation.takeoff,
+        # Takeoff first at start_time + start_delay_time
+        takeoff_time = start_time + start_delay
+        task_manager.add_task(takeoff_time, 0, animation.takeoff,
                               task_kwargs={
-                                  "z": client.active_client.config.copter_takeoff_height,
-                                  "timeout": client.active_client.config.copter_takeoff_time,
-                                  "safe_takeoff": client.active_client.config.copter_safe_takeoff,
-                                  # "frame_id": client.active_client.config.copter_frame_id,
-                                  "use_leds": client.active_client.config.led_use & client.active_client.config.led_takeoff_indication,
-                              }
-                              )
+                                  "z": copter.config.copter_takeoff_height,
+                                  "timeout": copter.config.copter_takeoff_time,
+                                  "safe_takeoff": False,
+                                  "use_leds": copter.config.led_use & copter.config.led_takeoff_indication,
+                              })
         # Fly to first point
-        rfp_time = start_time + client.active_client.config.copter_takeoff_time
-        if client.active_client.config.animation_yaw == "animation":
-            yaw = frame["yaw"]
-        else:
-            yaw = math.radians(float(client.active_client.config.animation_yaw))
+        rfp_time = takeoff_time + copter.config.copter_takeoff_time
         task_manager.add_task(rfp_time, 0, animation.execute_frame,
                               task_kwargs={
-                                  "point": animation.convert_frame(corrected_frames[0])[0],
-                                  "color": animation.convert_frame(corrected_frames[0])[1],
-                                  "yaw": yaw,
-                                  "frame_id": client.active_client.config.copter_frame_id,
-                                  "use_leds": client.active_client.config.led_use,
+                                  "frame": frames[0],
+                                  "frame_id": copter.config.copter_frame_id,
+                                  "use_leds": copter.config.led_use,
                                   "flight_func": FlightLib.reach_point,
-                              }
-                              )
+                              })
         # Calculate first frame start time
-        frame_time = rfp_time + client.active_client.config.copter_reach_first_point_time
+        frame_time = rfp_time + copter.config.copter_reach_first_point_time
 
-    elif start_action == 'arm':
+    elif start_action == 'fly':
         # Calculate start time
-        start_time += start_delay
-        frame_time = start_time  # + 1.0
-        point, color, yaw = animation.convert_frame(corrected_frames[0])
-        task_manager.add_task(frame_time, 0, animation.execute_frame,
+        arm_time = start_time + start_delay  # + 1.0
+        task_manager.add_task(arm_time, 0, animation.execute_frame,
                               task_kwargs={
-                                  "point": point,
-                                  "color": color,
-                                  "frame_id": client.active_client.config.copter_frame_id,
-                                  "use_leds": client.active_client.config.led_use,
+                                  "frame": frames[0],
+                                  "frame_id": copter.config.copter_frame_id,
+                                  "use_leds": copter.config.led_use,
                                   "flight_func": FlightLib.navto,
                                   "auto_arm": True,
-                              }
-                              )
+                              })
         # Calculate first frame start time
-        frame_time += corrected_frames[0]["delay"]  # TODO Think about arming time
-    logger.debug(task_manager.task_queue)
+        frame_time = arm_time + copter.config.copter_arming_time
+    logger.debug("Start queue {}".format(task_manager.task_queue))
     # Play animation file
-    for frame in corrected_frames:
-        point, color, yaw = animation.convert_frame(frame)
-        if client.active_client.config.animation_yaw == "animation":
-            yaw = frame["yaw"]
-        else:
-            yaw = math.radians(float(client.active_client.config.animation_yaw))
+    for frame in frames:
         task_manager.add_task(frame_time, 0, animation.execute_frame,
                               task_kwargs={
-                                  "point": point,
-                                  "color": color,
-                                  "yaw": yaw,
-                                  "frame_id": client.active_client.config.copter_frame_id,
-                                  "use_leds": client.active_client.config.led_use,
+                                  "frame": frame,
+                                  "frame_id": copter.config.copter_frame_id,
+                                  "use_leds": copter.config.led_use,
                                   "flight_func": FlightLib.navto,
-                              }
-                              )
-        frame_time += frame["delay"]
-
+                              })
+        frame_time += frame.delay
     # Calculate land_time
-    land_time = frame_time + client.active_client.config.copter_land_time
+    land_time = frame_time + copter.config.copter_land_delay
     # Land
     task_manager.add_task(land_time, 0, animation.land,
                           task_kwargs={
-                              "timeout": client.active_client.config.copter_land_timeout,
-                              "frame_id": client.active_client.config.copter_frame_id,
-                              "use_leds": client.active_client.config.led_use & client.active_client.config.led_land_indication,
-                          },
-                          )
+                              "timeout": copter.config.copter_land_timeout,
+                              "frame_id": copter.config.copter_frame_id,
+                              "use_leds": copter.config.led_use & copter.config.led_land_indication,
+                          })
 
 
 # noinspection PyAttributeOutsideInit
 class Telemetry:
     params_default_dict = {
         "git_version": None,
-        "animation_id": None,
+        "animation_info": None,
         "battery": None,
         "armed": False,
         "fcu_status": None,
@@ -705,19 +773,25 @@ class Telemetry:
 
     @classmethod
     def get_config_version(cls):
-        return "{} V{}".format(client.active_client.config.config_name, client.active_client.config.config_version)
+        return "{} V{}".format(copter.config.config_name, copter.config.config_version)
 
-    @classmethod
-    def get_start_position(cls):
-        x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"),
-                                                  *client.active_client.config.animation_ratio)
-        offset = numpy.array(client.active_client.config.private_offset) + numpy.array(client.active_client.config.copter_common_offset)
-        x = x_start + offset[0]
-        y = y_start + offset[1]
-        z = offset[2]
-        if not FlightLib._check_nans(x, y, z):
-            return x, y, z
-        return 'NO_POS'
+    def get_start_position(self):
+        offset = numpy.array(copter.config.private_offset) + numpy.array(copter.config.copter_common_offset)
+        try:
+            x, y, z = copter.animation.get_start_point(copter.config.animation_ratio, offset)
+        except ValueError:
+            return [float('nan'),float('nan'),float('nan'),float('nan'),'error: no start pos in animation',float('nan')]
+        else:
+            start_delay = copter.animation.start_delay
+            yaw = copter.animation.get_start_yaw()
+            if not self.ros_telemetry:
+                start_action = 'error: no telemetry data'
+            else:
+                start_action = copter.animation.get_start_action(
+                                copter.config.animation_start_action, self.ros_telemetry.z,
+                                copter.config.animation_takeoff_level, copter.config.animation_ground_level,
+                                copter.config.animation_ratio, offset)
+            return [x,y,z,yaw,start_action,start_delay]
 
     @classmethod
     def get_battery(cls, ros_telemetry):
@@ -751,16 +825,21 @@ class Telemetry:
 
     @classmethod
     def get_position(cls, ros_telemetry):
-        x, y, z = ros_telemetry.x, ros_telemetry.y, ros_telemetry.z
+        try:
+            x, y, z = ros_telemetry.x, ros_telemetry.y, ros_telemetry.z
+        except AttributeError:
+            return 'NO_POS'
         if not math.isnan(x):
-            return x, y, z, math.degrees(ros_telemetry.yaw), client.active_client.config.copter_frame_id
+            return x, y, z, math.degrees(ros_telemetry.yaw), copter.config.copter_frame_id
         return 'NO_POS'
 
+    def get_ros_telemetry(self):
+        return self.ros_telemetry
+
     def update_telemetry_fast(self):
-        self.start_position = self.get_start_position()
         self.last_task = task_manager.get_current_task()
         try:
-            self.ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
+            self.ros_telemetry = FlightLib.get_telemetry_locked(copter.config.copter_frame_id)
             if self.ros_telemetry.connected:
                 self.armed = self.ros_telemetry.armed
                 self.mode = self.ros_telemetry.mode
@@ -779,9 +858,10 @@ class Telemetry:
         self.round_telemetry()
 
     def update_telemetry_slow(self):
-        self.animation_id = animation.get_id()
+        self.animation_info = [copter.animation.id, copter.animation.state]
         self.git_version = self.get_git_version()
         self.config_version = self.get_config_version()
+        self.start_position = self.get_start_position()
         try:
             self.calibration_status = get_calibration_status()
             self.fcu_status = get_sys_status()
@@ -848,7 +928,7 @@ class Telemetry:
 
     def transmit_message(self):  # todo if connected
         try:
-            client.active_client.server_connection.send_message('telemetry', kwargs={'value': self.create_msg_contents()})
+            copter.server_connection.send_message('telemetry', kwargs={'value': self.create_msg_contents()})
         except AttributeError as e:
             logger.debug(e)
 
@@ -878,7 +958,7 @@ class Telemetry:
             self.update_telemetry_fast()
             self.check_failsafe_and_interruption()
 
-            if client.active_client.config.telemetry_transmit and client.active_client.connected:
+            if copter.config.telemetry_transmit and copter.connected:
                 self.transmit_message()
 
             rate.sleep()
@@ -887,18 +967,20 @@ class Telemetry:
         rate = rospy.Rate(1)
         while not rospy.is_shutdown():
             self.update_telemetry_slow()
-            if client.active_client.config.telemetry_log_resources:
+            if copter.config.telemetry_log_resources:
                 self.log_cpu_and_memory()
             rate.sleep()
 
     def start_loop(self):
-        if client.active_client.config.telemetry_frequency > 0:
+        if copter.config.telemetry_frequency > 0:
             telemetry_thread = threading.Thread(target=self._update_loop, name="Telemetry getting thread",
-                                                args=(client.active_client.config.telemetry_frequency,))  # TODO MOVE? Daemon?
+                                                args=(copter.config.telemetry_frequency,))
+            telemetry_thread.daemon = True
+            telemetry_thread.start()
             slow_telemetry_thread = threading.Thread(target=self._slow_update_loop,
                                                      name="Slow telemetry getting thread")
+            slow_telemetry_thread.daemon = True
             slow_telemetry_thread.start()
-            telemetry_thread.start()
         else:
             logger.info("Telemetry loop is not created because of zero or negative telemetry frequency")
 
@@ -914,9 +996,25 @@ def emergency_callback(data):
     emergency = data.data
 
 
+class AnimationEventHandler(FileSystemEventHandler):
+    def on_any_event(self, event):
+        logger.info('{} is {}'.format(event.src_path, event.event_type))
+        # logger.info(os.path.splitext(event.src_path))
+        if (os.path.splitext(event.src_path)[-1] == '.csv' and event.event_type != "deleted") or event.src_path.split('/')[-1] == 'client.ini':
+            if os.path.exists("animation.csv"):
+                logger.info("Update frames from animation.csv")
+                copter.animation.update_frames(copter.config, "animation.csv")
+
+
 if __name__ == "__main__":
-    telemetry = Telemetry()
-    copter_client = CopterClient()
+    copter = CopterClient()
     task_manager = tasking.TaskManager()
     rospy.Subscriber('/emergency', Bool, emergency_callback)
-    copter_client.start(task_manager)
+    event_handler = AnimationEventHandler()
+    observer = Observer()
+    observer.schedule(event_handler, ".", recursive=True)
+    observer.daemon = True
+    observer.start()
+    copter.start(task_manager)
+    while not rospy.is_shutdown():
+        rospy.sleep(0.1)

Drone/requirements.txt

@@ -1,3 +1,4 @@
 selectors2
 psutil
 configobj
+watchdog

Server/config/spec/configspec_server.ini

@@ -17,17 +17,17 @@ config_version = float(default='1.0')
         [[[DEFAULT]]]
             copter_id = preset_param(default=list(True, 100))
             git_version = preset_param(default=list(True, 75))
-            config_version = preset_param(default=list(True, 140))
-            animation_id = preset_param(default=list(True, 100))
+            config_version = preset_param(default=list(True, 105))
+            animation_info = preset_param(default=list(True, 100))
             battery = preset_param(default=list(True, 100))
             fcu_status = preset_param(default=list(True, 100))
             calibration_status = preset_param(default=list(True, 65))
             mode = preset_param(default=list(True, 100))
             selfcheck = preset_param(default=list(True, 65))
             current_position = preset_param(default=list(True, 250))
-            start_position = preset_param(default=list(True, 150))
-            last_task = preset_param(default=list(True, 250))
-            time_delta = preset_param(default=list(True, 100))
+            start_position = preset_param(default=list(True, 240))
+            last_task = preset_param(default=list(True, 275))
+            time_delta = preset_param(default=list(True, 70))
         [[[__many__]]]
             __many__ = preset_param
 

Server/copter_table.py

@@ -226,8 +226,8 @@ class CopterTableWidget(QTableView):
     @pyqtSlot(QtCore.QPoint)
     def open_menu(self, point):
         menu = QMenu(self)
-        index = self.indexAt(point)
-        item = self.model.get_row_data(index)
+        id = self.indexAt(point).siblingAtColumn(0).data()
+        item = self.model.get_row_by_attr('copter_id', id)
 
         edit_config = QAction("Edit config")
         edit_config.triggered.connect(partial(self.edit_copter_config, item))

Server/copter_table_models.py

@@ -81,16 +81,19 @@ class ModelChecks:
 def check_ver(item):
     if not ModelChecks.check_git:
         return True
-    
+
     version = get_git_version()
     if version is not None:
         return version == item
     return True
 
 
-@ModelChecks.column_check("animation_id")
+@ModelChecks.column_check("animation_info")
 def check_anim(item):
-    return str(item) != 'No animation'
+    if item:
+        return str(item[1]) == 'OK'
+    else:
+        return False
 
 
 @ModelChecks.column_check("battery")
@@ -142,6 +145,10 @@ def check_time_delta(item):
 @ModelChecks.column_check("start_position", pass_context=True)
 def check_start_pos(item, context):
 
+    if len(item) == 6:
+        if not item[4] in ["takeoff", "fly"]:
+            return False
+
     if ModelChecks.start_pos_delta_max == 0:
         return True
 
@@ -150,6 +157,7 @@ def check_start_pos(item, context):
 
     delta = get_distance(get_position(context.current_position),
                          get_position(context.start_position))
+
     if math.isnan(delta):
         return False
 
@@ -157,7 +165,7 @@ def check_start_pos(item, context):
 
 
 def get_position(position):
-    if position != 'NO_POS' and position[0] != 'nan':  # float('nan')?
+    if not isinstance(position, str) and position[0] != float('nan'):
         return position[:3]
     return [float('nan')] * 3
 
@@ -276,6 +284,17 @@ def place_id(value):
         msgbox.exec_()
         return None
 
+@ModelFormatter.view_formatter("animation_info")
+def view_animation_info(value):
+    try:
+        id, state = value
+    except ValueError:
+        return ""
+    else:
+        if state == 'OK':
+            return id
+        else:
+            return state
 
 @ModelFormatter.place_formatter("battery")
 def place_battery(value):
@@ -307,15 +326,18 @@ def view_selfcheck(value):
 def view_current_position(value):
     if isinstance(value, list):
         x, y, z, yaw, frame = value
-        return f"{x: .2f} {y: .2f} {z: .2f} {int(yaw): d} {frame}"
+        return f"{x: .2f} {y: .2f} {z: .2f} {yaw: .0f} {frame}"
     return value
 
 
 @ModelFormatter.view_formatter("start_position")
 def view_start_position(value):
     if isinstance(value, list):
-        x, y, z = value
-        return f"{x: .2f} {y: .2f} {z: .2f}"
+        x, y, z, yaw, action, delay = value
+        if action in ['fly', 'takeoff']:
+            return f"{x: .2f} {y: .2f} {z: .2f} {yaw: .0f} {action} {delay: .1f}"
+        else:
+            return f"{action}"
     return value
 
 
@@ -340,14 +362,14 @@ class CopterDataModel(QtCore.QAbstractTableModel):
     columns_dict = {'copter_id': 'copter ID',
                     'git_version': 'version',
                     'config_version': 'configuration',
-                    'animation_id': ' animation ID ',
+                    'animation_info': 'animation ID',
                     'battery': '  battery  ',
                     'fcu_status': 'FCU status',
                     'calibration_status': 'sensors',
                     'mode': '  mode  ',
                     'selfcheck': ' checks ',
                     'current_position': 'current x y z yaw frame_id',
-                    'start_position': '    start x y z     ',
+                    'start_position': 'start x y z yaw action delay',
                     'last_task': 'last task',
                     'time_delta': 'dt',
                     }

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(1360, 816)
+        MainWindow.resize(1360, 869)
         self.centralwidget = QtWidgets.QWidget(MainWindow)
         self.centralwidget.setEnabled(True)
         self.centralwidget.setObjectName("centralwidget")
@@ -44,30 +44,22 @@ class Ui_MainWindow(object):
         self.verticalLayout.setSizeConstraint(QtWidgets.QLayout.SetMaximumSize)
         self.verticalLayout.setObjectName("verticalLayout")
         self.formLayout = QtWidgets.QFormLayout()
-        self.formLayout.setLabelAlignment(QtCore.Qt.AlignHCenter|QtCore.Qt.AlignTop)
+        self.formLayout.setLabelAlignment(QtCore.Qt.AlignLeading|QtCore.Qt.AlignLeft|QtCore.Qt.AlignTop)
         self.formLayout.setFormAlignment(QtCore.Qt.AlignHCenter|QtCore.Qt.AlignTop)
         self.formLayout.setObjectName("formLayout")
         self.start_text = QtWidgets.QLabel(self.centralwidget)
         self.start_text.setLayoutDirection(QtCore.Qt.RightToLeft)
-        self.start_text.setAlignment(QtCore.Qt.AlignCenter)
+        self.start_text.setAlignment(QtCore.Qt.AlignLeading|QtCore.Qt.AlignLeft|QtCore.Qt.AlignVCenter)
         self.start_text.setObjectName("start_text")
         self.formLayout.setWidget(0, QtWidgets.QFormLayout.LabelRole, self.start_text)
         self.start_delay_spin = QtWidgets.QSpinBox(self.centralwidget)
         self.start_delay_spin.setObjectName("start_delay_spin")
         self.formLayout.setWidget(0, QtWidgets.QFormLayout.FieldRole, self.start_delay_spin)
-        self.music_text = QtWidgets.QLabel(self.centralwidget)
-        self.music_text.setLayoutDirection(QtCore.Qt.RightToLeft)
-        self.music_text.setObjectName("music_text")
-        self.formLayout.setWidget(1, 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(1, QtWidgets.QFormLayout.FieldRole, self.music_delay_spin)
-        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(2, QtWidgets.QFormLayout.LabelRole, self.music_play_text)
         self.music_checkbox = QtWidgets.QCheckBox(self.centralwidget)
         sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Minimum, QtWidgets.QSizePolicy.Fixed)
         sizePolicy.setHorizontalStretch(0)
@@ -82,6 +74,14 @@ class Ui_MainWindow(object):
         self.music_checkbox.setChecked(False)
         self.music_checkbox.setObjectName("music_checkbox")
         self.formLayout.setWidget(2, QtWidgets.QFormLayout.FieldRole, self.music_checkbox)
+        self.music_text = QtWidgets.QLabel(self.centralwidget)
+        self.music_text.setLayoutDirection(QtCore.Qt.RightToLeft)
+        self.music_text.setObjectName("music_text")
+        self.formLayout.setWidget(1, QtWidgets.QFormLayout.LabelRole, self.music_text)
+        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(2, QtWidgets.QFormLayout.LabelRole, self.music_play_text)
         self.verticalLayout.addLayout(self.formLayout)
         self.line = QtWidgets.QFrame(self.centralwidget)
         self.line.setFrameShape(QtWidgets.QFrame.HLine)
@@ -212,7 +212,7 @@ class Ui_MainWindow(object):
         self.gridLayout.addLayout(self.horizontalLayout, 0, 0, 1, 1)
         MainWindow.setCentralWidget(self.centralwidget)
         self.menubar = QtWidgets.QMenuBar(MainWindow)
-        self.menubar.setGeometry(QtCore.QRect(0, 0, 1360, 25))
+        self.menubar.setGeometry(QtCore.QRect(0, 0, 1360, 22))
         self.menubar.setObjectName("menubar")
         self.menuOptions = QtWidgets.QMenu(self.menubar)
         self.menuOptions.setObjectName("menuOptions")
@@ -301,6 +301,8 @@ class Ui_MainWindow(object):
         self.action_configure_columns.setObjectName("action_configure_columns")
         self.actionSomething = QtWidgets.QAction(MainWindow)
         self.actionSomething.setObjectName("actionSomething")
+        self.action_send_animation = QtWidgets.QAction(MainWindow)
+        self.action_send_animation.setObjectName("action_send_animation")
         self.menuMusic_2.addAction(self.action_select_music_file)
         self.menuMusic_2.addAction(self.action_play_music)
         self.menuMusic_2.addAction(self.action_stop_music)
@@ -319,10 +321,12 @@ class Ui_MainWindow(object):
         self.menuTable.addSeparator()
         self.menuTable.addAction(self.action_configure_columns)
         self.menuSend.addAction(self.action_send_animations)
+        self.menuSend.addAction(self.action_send_calibrations)
+        self.menuSend.addSeparator()
+        self.menuSend.addAction(self.action_send_aruco_map)
+        self.menuSend.addAction(self.action_send_animation)
         self.menuSend.addAction(self.action_send_configurations)
         self.menuSend.addAction(self.action_send_launch_file)
-        self.menuSend.addAction(self.action_send_aruco_map)
-        self.menuSend.addAction(self.action_send_calibrations)
         self.menuSend.addAction(self.action_send_fcu_parameters)
         self.menuSend.addSeparator()
         self.menuSend.addAction(self.action_send_any_file)
@@ -357,8 +361,8 @@ class Ui_MainWindow(object):
         MainWindow.setWindowTitle(_translate("MainWindow", "Clever Drone Show"))
         self.start_text.setText(_translate("MainWindow", "  Start after"))
         self.start_delay_spin.setSuffix(_translate("MainWindow", " s"))
-        self.music_text.setText(_translate("MainWindow", "  Music after"))
         self.music_delay_spin.setSuffix(_translate("MainWindow", " s"))
+        self.music_text.setText(_translate("MainWindow", "  Music after"))
         self.music_play_text.setText(_translate("MainWindow", "  Play music"))
         self.check_button.setText(_translate("MainWindow", "Preflight check"))
         self.start_button.setText(_translate("MainWindow", "Start animation"))
@@ -389,7 +393,7 @@ class Ui_MainWindow(object):
         self.action_send_aruco_map.setText(_translate("MainWindow", "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", "Launch files"))
+        self.action_send_launch_file.setText(_translate("MainWindow", "Launch files folder"))
         self.action_restart_clever.setText(_translate("MainWindow", "clever"))
         self.action_restart_clever_show.setText(_translate("MainWindow", "clever-show"))
         self.action_select_all_rows.setText(_translate("MainWindow", "Select all drones"))
@@ -410,7 +414,7 @@ class Ui_MainWindow(object):
         self.action_send_calibrations.setText(_translate("MainWindow", "Camera calibrations"))
         self.action_reboot_all.setText(_translate("MainWindow", "Reboot"))
         self.action_restart_chrony.setText(_translate("MainWindow", "chrony"))
-        self.action_send_fcu_parameters.setText(_translate("MainWindow", "FCU parameters"))
+        self.action_send_fcu_parameters.setText(_translate("MainWindow", "FCU parameters file"))
         self.action_toggle_select.setText(_translate("MainWindow", "Toggle select"))
         self.action_toggle_select.setShortcut(_translate("MainWindow", "Ctrl+A"))
         self.action_select_all.setText(_translate("MainWindow", "Select all"))
@@ -424,3 +428,4 @@ class Ui_MainWindow(object):
         self.action_restart_server.setText(_translate("MainWindow", "Restart server"))
         self.action_configure_columns.setText(_translate("MainWindow", "Configure columns"))
         self.actionSomething.setText(_translate("MainWindow", "something"))
+        self.action_send_animation.setText(_translate("MainWindow", "Animation"))

Server/server_gui.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1360</width>
-    <height>816</height>
+    <height>869</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -71,7 +71,7 @@
         <item>
          <layout class="QFormLayout" name="formLayout">
           <property name="labelAlignment">
-           <set>Qt::AlignHCenter|Qt::AlignTop</set>
+           <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
           </property>
           <property name="formAlignment">
            <set>Qt::AlignHCenter|Qt::AlignTop</set>
@@ -85,7 +85,7 @@
              <string>  Start after</string>
             </property>
             <property name="alignment">
-             <set>Qt::AlignCenter</set>
+             <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
             </property>
            </widget>
           </item>
@@ -96,16 +96,6 @@
             </property>
            </widget>
           </item>
-          <item row="1" column="0">
-           <widget class="QLabel" name="music_text">
-            <property name="layoutDirection">
-             <enum>Qt::RightToLeft</enum>
-            </property>
-            <property name="text">
-             <string>  Music after</string>
-            </property>
-           </widget>
-          </item>
           <item row="1" column="1">
            <widget class="QDoubleSpinBox" name="music_delay_spin">
             <property name="suffix">
@@ -119,16 +109,6 @@
             </property>
            </widget>
           </item>
-          <item row="2" column="0">
-           <widget class="QLabel" name="music_play_text">
-            <property name="layoutDirection">
-             <enum>Qt::RightToLeft</enum>
-            </property>
-            <property name="text">
-             <string>  Play music</string>
-            </property>
-           </widget>
-          </item>
           <item row="2" column="1">
            <widget class="QCheckBox" name="music_checkbox">
             <property name="sizePolicy">
@@ -157,6 +137,26 @@
             </property>
            </widget>
           </item>
+          <item row="1" column="0">
+           <widget class="QLabel" name="music_text">
+            <property name="layoutDirection">
+             <enum>Qt::RightToLeft</enum>
+            </property>
+            <property name="text">
+             <string>  Music after</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="0">
+           <widget class="QLabel" name="music_play_text">
+            <property name="layoutDirection">
+             <enum>Qt::RightToLeft</enum>
+            </property>
+            <property name="text">
+             <string>  Play music</string>
+            </property>
+           </widget>
+          </item>
          </layout>
         </item>
         <item>
@@ -426,7 +426,7 @@
      <x>0</x>
      <y>0</y>
      <width>1360</width>
-     <height>25</height>
+     <height>22</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuOptions">
@@ -470,10 +470,12 @@
       <string>Send</string>
      </property>
      <addaction name="action_send_animations"/>
+     <addaction name="action_send_calibrations"/>
+     <addaction name="separator"/>
+     <addaction name="action_send_aruco_map"/>
+     <addaction name="action_send_animation"/>
      <addaction name="action_send_configurations"/>
      <addaction name="action_send_launch_file"/>
-     <addaction name="action_send_aruco_map"/>
-     <addaction name="action_send_calibrations"/>
      <addaction name="action_send_fcu_parameters"/>
      <addaction name="separator"/>
      <addaction name="action_send_any_file"/>
@@ -538,7 +540,7 @@
   </action>
   <action name="action_send_launch_file">
    <property name="text">
-    <string>Launch files</string>
+    <string>Launch files folder</string>
    </property>
   </action>
   <action name="action_restart_clever">
@@ -639,7 +641,7 @@
   </action>
   <action name="action_send_fcu_parameters">
    <property name="text">
-    <string>FCU parameters</string>
+    <string>FCU parameters file</string>
    </property>
   </action>
   <action name="action_toggle_select">
@@ -707,6 +709,11 @@
     <string>something</string>
    </property>
   </action>
+  <action name="action_send_animation">
+   <property name="text">
+    <string>Animation</string>
+   </property>
+  </action>
  </widget>
  <tabstops>
   <tabstop>start_delay_spin</tabstop>

Server/server_qt.py

@@ -112,10 +112,6 @@ class MainWindow(QtWidgets.QMainWindow):
 
         self.init_model()
 
-        # self.statusBar = QStatusBar()
-        # self.setStatusBar(self.statusBar)
-        # self.statusBar.showMessage("Hey", 2000)
-
         self.register_callbacks()
         self.player = QtMultimedia.QMediaPlayer()
 
@@ -161,6 +157,7 @@ class MainWindow(QtWidgets.QMainWindow):
 
         self.ui.action_send_animations.triggered.connect(self.send_animations)
         self.ui.action_send_calibrations.triggered.connect(self.send_calibrations)
+        self.ui.action_send_animation.triggered.connect(self.send_animation)
         self.ui.action_send_configurations.triggered.connect(self.send_config)
         self.ui.action_send_aruco_map.triggered.connect(self.send_aruco)
         self.ui.action_send_launch_file.triggered.connect(self.send_launch)
@@ -171,7 +168,7 @@ class MainWindow(QtWidgets.QMainWindow):
         self.ui.action_retrive_any_file.triggered.connect(b_partial(self.request_any_file, client_path=None))
 
         self.ui.action_restart_clever.triggered.connect(
-            b_partial(self.send_to_selected, "service_restart", command_kwargs={"name": "clever"}))
+            b_partial(self.send_to_selected, "service_restart", command_kwargs={"name": "clover"}))
         self.ui.action_restart_clever_show.triggered.connect(self.restart_clever_show)
         self.ui.action_restart_chrony.triggered.connect(self.restart_chrony)
         self.ui.action_reboot_all.triggered.connect(b_partial(self.send_to_selected, "reboot_all"))
@@ -291,7 +288,7 @@ class MainWindow(QtWidgets.QMainWindow):
             try:
                 col = self.model.columns.index(key)
             except ValueError:
-                logging.error(f"No column {key} present!")
+                logging.debug(f"No column {key} present!")
             else:
                 row_data = self.model.get_row_by_attr("client", client)
                 row_num = self.model.get_row_index(row_data)
@@ -481,6 +478,10 @@ class MainWindow(QtWidgets.QMainWindow):
         self.send_directory_files("Select directory with animations", ('.csv', '.txt'), match_id=True,
                                   client_path="", client_filename="animation.csv")
 
+    @pyqtSlot()
+    def send_animation(self):
+        self.send_files("Select animation file", "Animation files (*.csv)", onefile=True, client_filename="animation.csv")
+
     @pyqtSlot()
     def send_calibrations(self):
         self.send_directory_files("Select directory with calibrations", ('.yaml', ), match_id=True,
@@ -646,7 +647,7 @@ if __name__ == "__main__":
     msgbox_handler.setLevel(logging.CRITICAL)
 
     logging.basicConfig(
-        level=logging.DEBUG,
+        level=logging.INFO,
         format="%(asctime)s [%(name)-7.7s] [%(threadName)-19.19s] [%(levelname)-7.7s]  %(message)s",
         handlers=[
             logging.FileHandler("server_logs/{}.log".format(now)),

blender-addon/addon.py

@@ -8,13 +8,13 @@ from bpy.types import Operator
 from bpy.props import StringProperty, BoolProperty, FloatProperty, IntProperty
 
 bl_info = {
-    "name": "Export > CSV Drone Swarm Animation Exporter (.csv)",
-    "author": "Artem Vasiunik",
-    "version": (0, 4, 0),
+    "name": "clever-show animation (.csv)",
+    "author": "Artem Vasiunik & Arthur Golubtsov",
+    "version": (0, 5, 0),
     "blender": (2, 80, 0),
     #"api": 36079,
-    "location": "File > Export > CSV Drone Swarm Animation Exporter (.csv)",
-    "description": "Export > CSV Drone Swarm Animation Exporter (.csv)",
+    "location": "File > Export > clever-show animation (.csv)",
+    "description": "Export > clever-show animation (.csv)",
     "warning": "",
     "wiki_url": "https://github.com/CopterExpress/clever-show/blob/master/blender-addon/README.md",
     "tracker_url": "https://github.com/CopterExpress/clever-show/issues",
@@ -23,20 +23,20 @@ bl_info = {
 
 
 class ExportCsv(Operator, ExportHelper):
-    bl_idname = "export_swarm_anim.folder"
-    bl_label = "Export Drone Swarm animation"
+    bl_idname = "export_animation.folder"
+    bl_label = "Export clever-show animation"
     filename_ext = ''
     use_filter_folder = True
 
     use_namefilter: bpy.props.BoolProperty(
         name="Use name filter for objects",
-        default=True,    
+        default=False,
     )
 
     drones_name: bpy.props.StringProperty(
         name="Name identifier",
         description="Name identifier for all drone objects",
-        default="copter"
+        default="clever"
     )
 
     show_warnings: bpy.props.BoolProperty(
@@ -61,7 +61,7 @@ class ExportCsv(Operator, ExportHelper):
 
     filepath: StringProperty(
         name="File Path",
-        description="File path used for exporting CSV files",
+        description="File path used for exporting csv files",
         maxlen=1024,
         subtype='DIR_PATH',
         default=""
@@ -96,11 +96,11 @@ class ExportCsv(Operator, ExportHelper):
                 distance_exeeded = False
 
                 prev_x, prev_y, prev_z = 0, 0, 0
-				
+
                 animation_file_writer.writerow([
 					os.path.splitext(bpy.path.basename(bpy.data.filepath))[0]
 				])
-				
+
                 for frame_number in range(frame_start, frame_end + 1):
                     scene.frame_set(frame_number)
                     rgb = get_rgb_from_object(drone_obj)
@@ -135,9 +135,7 @@ class ExportCsv(Operator, ExportHelper):
                         round(rot_z, 5),
                         *rgb,
                     ])
-					
-				
-				
+
                 if speed_exeeded:
                     self.report({'WARNING'}, "Drone '%s' speed limits exeeded" % drone_obj.name)
                 if distance_exeeded:
@@ -195,7 +193,7 @@ def calc_distance(start_point, end_point):
 def menu_func(self, context):
     self.layout.operator(
         ExportCsv.bl_idname,
-        text="CSV Drone Swarm Animation Exporter (.csv)"
+        text="clever-show animation (.csv)"
     )
 
 

blender-addon/examples/basic/clever-1.csv

@@ -0,0 +1,51 @@
+basic
+1,0.0,0.0,0.0,0.0,204,2,0
+2,0.0,0.0,0.0,0.0,204,9,1
+3,0.0,0.0,0.0,0.0,204,21,2
+4,0.0,0.0,0.0,0.0,204,37,3
+5,0.0,0.0,0.0,0.0,204,56,4
+6,0.0,0.0,0.0,0.0,204,77,5
+7,0.0,0.0,0.0,0.0,204,97,6
+8,0.0,0.0,0.0,0.0,204,116,6
+9,0.0,0.0,0.0,0.0,204,131,7
+10,0.0,0.0,0.0,0.0,204,143,7
+11,0.0,0.0,0.1,0.0,199,153,7
+12,0.0,0.0,0.2,0.0,185,163,6
+13,0.0,0.0,0.3,0.0,163,172,6
+14,0.0,0.0,0.4,0.0,134,181,5
+15,0.0,0.0,0.5,0.0,102,188,5
+16,0.0,0.0,0.6,0.0,69,194,4
+17,0.0,0.0,0.7,0.0,40,199,3
+18,0.0,0.0,0.8,0.0,18,201,3
+19,0.0,0.0,0.9,0.0,4,203,2
+20,0.0,0.0,1.0,0.0,0,204,2
+21,0.02217,0.0,1.0,0.0,0,204,2
+22,0.08889,0.0,1.0,0.0,0,204,2
+23,0.19737,0.0,1.0,0.0,0,204,2
+24,0.33926,0.0,1.0,0.0,0,204,2
+25,0.5,0.0,1.0,0.0,0,204,2
+26,0.66074,0.0,1.0,0.0,0,204,2
+27,0.80263,0.0,1.0,0.0,0,204,2
+28,0.91111,0.0,1.0,0.0,0,204,2
+29,0.97783,0.0,1.0,0.0,0,204,2
+30,1.0,0.0,1.0,0.0,0,204,2
+31,1.0,0.0,0.9,0.0,3,204,2
+32,1.0,0.0,0.8,0.0,14,204,2
+33,1.0,0.0,0.7,0.0,32,204,2
+34,1.0,0.0,0.6,0.0,56,204,1
+35,1.0,0.0,0.5,0.0,84,204,1
+36,1.0,0.0,0.4,0.0,112,204,0
+37,1.0,0.0,0.3,0.0,138,204,0
+38,1.0,0.0,0.2,0.0,159,204,0
+39,1.0,0.0,0.1,0.0,175,204,0
+40,1.0,0.0,0.0,0.0,183,204,0
+41,1.0,0.0,0.0,0.0,188,199,0
+42,1.0,0.0,0.0,0.0,192,185,0
+43,1.0,0.0,0.0,0.0,196,163,0
+44,1.0,0.0,0.0,0.0,199,135,0
+45,1.0,0.0,0.0,0.0,201,102,0
+46,1.0,0.0,0.0,0.0,202,69,0
+47,1.0,0.0,0.0,0.0,203,40,0
+48,1.0,0.0,0.0,0.0,203,18,0
+49,1.0,0.0,0.0,0.0,203,5,0
+50,1.0,0.0,0.0,0.0,204,0,0

tests/animation_1.csv

@@ -0,0 +1,51 @@
+basic
+1,0.0,0.0,0.0,0.0,204,2,0
+2,0.0,0.0,0.0,0.0,204,9,1
+3,0.0,0.0,0.0,0.0,204,21,2
+4,0.0,0.0,0.0,0.0,204,37,3
+5,0.0,0.0,0.0,0.0,204,56,4
+6,0.0,0.0,0.0,0.0,204,77,5
+7,0.0,0.0,0.0,0.0,204,97,6
+8,0.0,0.0,0.0,0.0,204,116,6
+9,0.0,0.0,0.0,0.0,204,131,7
+10,0.0,0.0,0.0,0.0,204,143,7
+11,0.0,0.0,0.1,0.0,199,153,7
+12,0.0,0.0,0.2,0.0,185,163,6
+13,0.0,0.0,0.3,0.0,163,172,6
+14,0.0,0.0,0.4,0.0,134,181,5
+15,0.0,0.0,0.5,0.0,102,188,5
+16,0.0,0.0,0.6,0.0,69,194,4
+17,0.0,0.0,0.7,0.0,40,199,3
+18,0.0,0.0,0.8,0.0,18,201,3
+19,0.0,0.0,0.9,0.0,4,203,2
+20,0.0,0.0,1.0,0.0,0,204,2
+21,0.02217,0.0,1.0,0.0,0,204,2
+22,0.08889,0.0,1.0,0.0,0,204,2
+23,0.19737,0.0,1.0,0.0,0,204,2
+24,0.33926,0.0,1.0,0.0,0,204,2
+25,0.5,0.0,1.0,0.0,0,204,2
+26,0.66074,0.0,1.0,0.0,0,204,2
+27,0.80263,0.0,1.0,0.0,0,204,2
+28,0.91111,0.0,1.0,0.0,0,204,2
+29,0.97783,0.0,1.0,0.0,0,204,2
+30,1.0,0.0,1.0,0.0,0,204,2
+31,1.0,0.0,0.9,0.0,3,204,2
+32,1.0,0.0,0.8,0.0,14,204,2
+33,1.0,0.0,0.7,0.0,32,204,2
+34,1.0,0.0,0.6,0.0,56,204,1
+35,1.0,0.0,0.5,0.0,84,204,1
+36,1.0,0.0,0.4,0.0,112,204,0
+37,1.0,0.0,0.3,0.0,138,204,0
+38,1.0,0.0,0.2,0.0,159,204,0
+39,1.0,0.0,0.1,0.0,175,204,0
+40,1.0,0.0,0.0,0.0,183,204,0
+41,1.0,0.0,0.0,0.0,188,199,0
+42,1.0,0.0,0.0,0.0,192,185,0
+43,1.0,0.0,0.0,0.0,196,163,0
+44,1.0,0.0,0.0,0.0,199,135,0
+45,1.0,0.0,0.0,0.0,201,102,0
+46,1.0,0.0,0.0,0.0,202,69,0
+47,1.0,0.0,0.0,0.0,203,40,0
+48,1.0,0.0,0.0,0.0,203,18,0
+49,1.0,0.0,0.0,0.0,203,5,0
+50,1.0,0.0,0.0,0.0,204,0,0

tests/animation_3.csv

@@ -0,0 +1,12 @@
+route
+20,0.0,0.0,1.0,0.0,0,204,2
+21,0.02217,0.0,1.0,0.0,0,204,2
+22,0.08889,0.0,1.0,0.0,0,204,2
+23,0.19737,0.0,1.0,0.0,0,204,2
+24,0.33926,0.0,1.0,0.0,0,204,2
+25,0.5,0.0,1.0,0.0,0,204,2
+26,0.66074,0.0,1.0,0.0,0,204,2
+27,0.80263,0.0,1.0,0.0,0,204,2
+28,0.91111,0.0,1.0,0.0,0,204,2
+29,0.97783,0.0,1.0,0.0,0,204,2
+30,1.0,0.0,1.0,0.0,0,204,2

tests/animation_4.csv

@@ -0,0 +1,161 @@
+two_drones_test
+1,0.2,1.4,1.0,0.15708,0,0,0
+2,0.2,1.4,1.0,0.15708,0,0,0
+3,0.2,1.4,1.0,0.15708,0,0,0
+4,0.2,1.4,1.0,0.15708,0,0,0
+5,0.2,1.4,1.0,0.15708,0,0,0
+6,0.2,1.4,1.0,0.15708,0,0,0
+7,0.2,1.4,1.0,0.15708,0,0,0
+8,0.2,1.4,1.0,0.15708,0,0,0
+9,0.2,1.4,1.0,0.15708,0,0,0
+10,0.2,1.4,1.0,0.15708,0,0,0
+11,0.20441,1.4,1.0,0.15708,0,0,0
+12,0.21774,1.4,1.0,0.15708,0,0,0
+13,0.24002,1.4,1.0,0.15708,0,0,0
+14,0.27111,1.4,1.0,0.15708,0,0,0
+15,0.31063,1.4,1.0,0.15708,0,0,0
+16,0.3579,1.4,1.0,0.15708,0,0,0
+17,0.41193,1.4,1.0,0.15708,0,0,0
+18,0.47141,1.4,1.0,0.15708,0,0,0
+19,0.53472,1.4,1.0,0.15708,0,0,0
+20,0.6,1.4,1.0,0.15708,0,0,0
+21,0.66528,1.4,1.0,0.15708,0,0,0
+22,0.72859,1.4,1.0,0.15708,0,0,0
+23,0.78807,1.4,1.0,0.15708,0,0,0
+24,0.8421,1.4,1.0,0.15708,0,0,0
+25,0.88937,1.4,1.0,0.15708,0,0,0
+26,0.92889,1.4,1.0,0.15708,0,0,0
+27,0.95998,1.4,1.0,0.15708,0,0,0
+28,0.98226,1.4,1.0,0.15708,0,0,0
+29,0.99559,1.4,1.0,0.15708,0,0,0
+30,1.0,1.4,1.0,0.15708,0,0,0
+31,1.0,1.40441,1.0,0.15708,0,0,0
+32,1.0,1.41774,1.0,0.15708,0,0,0
+33,1.0,1.44002,1.0,0.15708,0,0,0
+34,1.0,1.47111,1.0,0.15708,0,0,0
+35,1.0,1.51063,1.0,0.15708,0,0,0
+36,1.0,1.5579,1.0,0.15708,0,0,0
+37,1.0,1.61193,1.0,0.15708,0,0,0
+38,1.0,1.67141,1.0,0.15708,0,0,0
+39,1.0,1.73472,1.0,0.15708,0,0,0
+40,1.0,1.8,1.0,0.15708,0,0,0
+41,1.0,1.86528,1.0,0.15708,0,0,0
+42,1.0,1.92859,1.0,0.15708,0,0,0
+43,1.0,1.98807,1.0,0.15708,0,0,0
+44,1.0,2.0421,1.0,0.15708,0,0,0
+45,1.0,2.08937,1.0,0.15708,0,0,0
+46,1.0,2.12889,1.0,0.15708,0,0,0
+47,1.0,2.15998,1.0,0.15708,0,0,0
+48,1.0,2.18226,1.0,0.15708,0,0,0
+49,1.0,2.19559,1.0,0.15708,0,0,0
+50,1.0,2.2,1.0,0.15708,0,0,0
+51,0.99559,2.2,1.0,0.15708,0,0,0
+52,0.98226,2.2,1.0,0.15708,0,0,0
+53,0.95998,2.2,1.0,0.15708,0,0,0
+54,0.92889,2.2,1.0,0.15708,0,0,0
+55,0.88937,2.2,1.0,0.15708,0,0,0
+56,0.8421,2.2,1.0,0.15708,0,0,0
+57,0.78807,2.2,1.0,0.15708,0,0,0
+58,0.72859,2.2,1.0,0.15708,0,0,0
+59,0.66528,2.2,1.0,0.15708,0,0,0
+60,0.6,2.2,1.0,0.15708,0,0,0
+61,0.53472,2.2,1.0,0.15708,0,0,0
+62,0.47141,2.2,1.0,0.15708,0,0,0
+63,0.41193,2.2,1.0,0.15708,0,0,0
+64,0.3579,2.2,1.0,0.15708,0,0,0
+65,0.31063,2.2,1.0,0.15708,0,0,0
+66,0.27111,2.2,1.0,0.15708,0,0,0
+67,0.24002,2.2,1.0,0.15708,0,0,0
+68,0.21774,2.2,1.0,0.15708,0,0,0
+69,0.20441,2.2,1.0,0.15708,0,0,0
+70,0.2,2.2,1.0,0.15708,0,0,0
+71,0.2,2.19559,1.0,0.15708,0,0,0
+72,0.2,2.18226,1.0,0.15708,0,0,0
+73,0.2,2.15998,1.0,0.15708,0,0,0
+74,0.2,2.12889,1.0,0.15708,0,0,0
+75,0.2,2.08937,1.0,0.15708,0,0,0
+76,0.2,2.0421,1.0,0.15708,0,0,0
+77,0.2,1.98807,1.0,0.15708,0,0,0
+78,0.2,1.92859,1.0,0.15708,0,0,0
+79,0.2,1.86528,1.0,0.15708,0,0,0
+80,0.2,1.8,1.0,0.15708,0,0,0
+81,0.2,1.73472,1.0,0.15708,0,0,0
+82,0.2,1.67141,1.0,0.15708,0,0,0
+83,0.2,1.61193,1.0,0.15708,0,0,0
+84,0.2,1.5579,1.0,0.15708,0,0,0
+85,0.2,1.51063,1.0,0.15708,0,0,0
+86,0.2,1.47111,1.0,0.15708,0,0,0
+87,0.2,1.44002,1.0,0.15708,0,0,0
+88,0.2,1.41774,1.0,0.15708,0,0,0
+89,0.2,1.40441,1.0,0.15708,0,0,0
+90,0.2,1.4,1.0,0.15708,0,0,0
+91,0.2062,1.4,1.0,0.15708,0,0,0
+92,0.22355,1.4,1.0,0.15708,0,0,0
+93,0.25043,1.4,1.0,0.15708,0,0,0
+94,0.28553,1.4,1.0,0.15708,0,0,0
+95,0.32771,1.4,1.0,0.15708,0,0,0
+96,0.3759,1.4,1.0,0.15708,0,0,0
+97,0.42903,1.4,1.0,0.15708,0,0,0
+98,0.48579,1.4,1.0,0.15708,0,0,0
+99,0.54421,1.4,1.0,0.15708,0,0,0
+100,0.6,1.4,1.0,0.15708,0,0,0
+101,0.66257,1.40492,1.025,0.15708,0,0,0
+102,0.72361,1.41958,1.05,0.31416,0,0,0
+103,0.7816,1.4436,1.075,0.47124,0,0,0
+104,0.83511,1.47639,1.1,0.62832,0,0,0
+105,0.88284,1.51716,1.125,0.7854,0,0,0
+106,0.92361,1.56489,1.15,0.94248,0,0,0
+107,0.9564,1.6184,1.175,1.09956,0,0,0
+108,0.98042,1.67639,1.2,1.25664,0,0,0
+109,0.99508,1.73743,1.225,1.41372,0,0,0
+110,1.0,1.8,1.25,1.5708,0,0,0
+111,0.99508,1.86257,1.275,1.72788,0,0,0
+112,0.98042,1.92361,1.3,1.88496,0,0,0
+113,0.9564,1.9816,1.325,2.04204,0,0,0
+114,0.92361,2.03511,1.35,2.19912,0,0,0
+115,0.88284,2.08284,1.375,2.35619,0,0,0
+116,0.83511,2.12361,1.4,2.51327,0,0,0
+117,0.7816,2.1564,1.425,2.67035,0,0,0
+118,0.72361,2.18042,1.45,2.82743,0,0,0
+119,0.66257,2.19508,1.475,2.98451,0,0,0
+120,0.6,2.2,1.5,-3.14159,0,0,0
+121,0.53743,2.19508,1.525,-2.98451,0,0,0
+122,0.47639,2.18042,1.55,-2.82743,0,0,0
+123,0.4184,2.1564,1.575,-2.67035,0,0,0
+124,0.36489,2.12361,1.6,-2.51327,0,0,0
+125,0.31716,2.08284,1.625,-2.35619,0,0,0
+126,0.27639,2.03511,1.65,-2.19911,0,0,0
+127,0.2436,1.9816,1.675,-2.04203,0,0,0
+128,0.21958,1.92361,1.7,-1.88495,0,0,0
+129,0.20492,1.86257,1.725,-1.72788,0,0,0
+130,0.2,1.8,1.75,-1.5708,0,0,0
+131,0.20492,1.73743,1.775,-1.41372,0,0,0
+132,0.21958,1.67639,1.8,-1.25664,0,0,0
+133,0.2436,1.6184,1.825,-1.09956,0,0,0
+134,0.27639,1.56489,1.85,-0.94248,0,0,0
+135,0.31716,1.51716,1.875,-0.7854,0,0,0
+136,0.36489,1.47639,1.9,-0.62832,0,0,0
+137,0.4184,1.4436,1.925,-0.47124,0,0,0
+138,0.47639,1.41958,1.95,-0.31416,0,0,0
+139,0.53743,1.40492,1.975,-0.15708,0,0,0
+140,0.6,1.4,2.0,0.0,0,0,0
+141,0.6,1.4,2.0,0.0,0,0,0
+142,0.6,1.4,2.0,0.0,0,0,0
+143,0.6,1.4,2.0,0.0,0,0,0
+144,0.6,1.4,2.0,0.0,0,0,0
+145,0.6,1.4,2.0,0.0,0,0,0
+146,0.6,1.4,2.0,0.0,0,0,0
+147,0.6,1.4,2.0,0.0,0,0,0
+148,0.6,1.4,2.0,0.0,0,0,0
+149,0.6,1.4,2.0,0.0,0,0,0
+150,0.6,1.4,2.0,0.0,0,0,0
+151,0.6,1.4,2.0,0.0,0,0,0
+152,0.6,1.4,2.0,0.0,0,0,0
+153,0.6,1.4,2.0,0.0,0,0,0
+154,0.6,1.4,2.0,0.0,0,0,0
+155,0.6,1.4,2.0,0.0,0,0,0
+156,0.6,1.4,2.0,0.0,0,0,0
+157,0.6,1.4,2.0,0.0,0,0,0
+158,0.6,1.4,2.0,0.0,0,0,0
+159,0.6,1.4,2.0,0.0,0,0,0
+160,0.6,1.4,2.0,0.0,0,0,0

tests/animation_test.py

@@ -0,0 +1,140 @@
+import os
+import sys
+import shutil
+from pytest import approx
+import pytest
+
+# Add parent dir to PATH to import config
+import inspect
+current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
+parent_dir = os.path.dirname(current_dir)
+sys.path.insert(0, parent_dir)
+sys.path.insert(0, os.path.join(parent_dir,"Drone"))
+
+from config import ConfigManager
+
+config_path = 'animation_config/config'
+spec_path = os.path.join(config_path,'spec')
+if not os.path.exists(spec_path):
+    try:
+        os.makedirs(spec_path)
+    except OSError:
+        print("Creation of the directory {} failed".format(spec_path))
+    else:
+        print("Successfully created the directory {}".format(spec_path))
+
+shutil.copy("../Drone/config/spec/configspec_client.ini", spec_path)
+
+config = ConfigManager()
+config.load_config_and_spec(os.path.join(config_path,'client.ini'))
+
+assert config.config_name == "client"
+
+import animation_lib
+
+a = animation_lib.Animation()
+
+def test_animation_1():
+    a.update_frames(config, "animation_1.csv")
+    assert a.id == 'basic'
+    assert approx(a.original_frames[0].get_pos()) == [0,0,0]
+    assert a.original_frames[0].get_color() == [204,2,0]
+    assert a.original_frames[0].pose_is_valid()
+    assert animation_lib.get_numbers(a.static_begin_frames) == range(1,11)
+    assert animation_lib.get_numbers(a.takeoff_frames) == range(11,21)
+    assert animation_lib.get_numbers(a.route_frames) == range(21,31)
+    assert animation_lib.get_numbers(a.land_frames) == range(31, 41)
+    assert animation_lib.get_numbers(a.static_end_frames) == range(41, 51)
+    assert animation_lib.get_numbers(a.output_frames) == range(11,31)
+    assert approx(a.static_begin_time) == 1
+    assert approx(a.takeoff_time) == 1
+    assert approx(a.output_frames_min_z) == 0.1
+    assert approx(a.get_scaled_output(ratio=[1,2,3], offset=[4,5,6])[0].get_pos()) == [4.,5.,6.3]
+    assert approx(a.get_scaled_output_min_z(ratio=[1,2,3], offset=[4,5,6])) == 6.3
+    assert approx(a.get_start_point(ratio=[1,2,3], offset=[4,5,6])) == [4.,5.,6.3]
+
+def test_animation_2():
+    a.update_frames(config, "animation_2.csv")
+    assert a.id == 'parad'
+    assert approx(a.original_frames[271].get_pos()) == [-1.00519,2.65699,0.24386]
+    assert a.original_frames[271].get_color() == [7,255,0]
+    assert a.original_frames[271].pose_is_valid()
+    assert animation_lib.get_numbers(a.static_begin_frames) == range(271)
+    assert animation_lib.get_numbers(a.takeoff_frames) == range(271,285)
+    assert animation_lib.get_numbers(a.route_frames) == range(285,1065)
+    assert animation_lib.get_numbers(a.land_frames) == []
+    assert animation_lib.get_numbers(a.static_end_frames) == []
+    assert animation_lib.get_numbers(a.output_frames) == range(271, 1065)
+    assert approx(a.static_begin_time) == 27.1
+    assert approx(a.takeoff_time) == 1.4
+    assert approx(a.output_frames_min_z) == 0.24386
+    assert approx(a.get_scaled_output(ratio=[1,2,3], offset=[4,5,6])[0].get_pos()) == [2.99481, 10.31398, 6.73158]
+    assert approx(a.get_scaled_output_min_z(ratio=[1,2,3], offset=[4,5,6])) == 6.73158
+    assert approx(a.get_start_point(ratio=[1,2,3], offset=[4,5,6])) == [2.99481, 10.31398, 6.73158]
+
+def test_animation_3():
+    a.update_frames(config, "animation_3.csv")
+    assert a.id == 'route'
+    assert approx(a.original_frames[9].get_pos()) == [0.97783,0.0,1.0]
+    assert a.original_frames[9].get_color() == [0,204,2]
+    assert a.original_frames[9].pose_is_valid()
+    assert animation_lib.get_numbers(a.static_begin_frames) == []
+    assert animation_lib.get_numbers(a.takeoff_frames) == []
+    assert animation_lib.get_numbers(a.route_frames) == range(20,31)
+    assert animation_lib.get_numbers(a.land_frames) == []
+    assert animation_lib.get_numbers(a.static_end_frames) == []
+    assert approx(a.static_begin_time) == 0
+    assert approx(a.takeoff_time) == 0
+    assert approx(a.output_frames_min_z) == 1
+    assert approx(a.get_scaled_output(ratio=[1,2,3], offset=[4,5,6])[0].get_pos()) == [4,5,9]
+    assert approx(a.get_scaled_output_min_z(ratio=[1,2,3], offset=[4,5,6])) == 9
+    assert approx(a.get_start_point(ratio=[1,2,3], offset=[4,5,6])) == [4,5,9]
+
+def test_animation_4():
+    a.update_frames(config, "animation_4.csv")
+    assert a.id == 'two_drones_test'
+    assert approx(a.original_frames[11].get_pos()) == [0.21774,1.4,1.0]
+    assert a.original_frames[11].get_color() == [0,0,0]
+    assert a.original_frames[11].pose_is_valid()
+    assert animation_lib.get_numbers(a.static_begin_frames) == range(1,12)
+    assert animation_lib.get_numbers(a.takeoff_frames) == []
+    assert animation_lib.get_numbers(a.route_frames) == range(12,141)
+    assert animation_lib.get_numbers(a.land_frames) == []
+    assert animation_lib.get_numbers(a.static_end_frames) == range(141,161)
+    assert animation_lib.get_numbers(a.output_frames) == range(12,141)
+    assert approx(a.static_begin_time) == 1.1
+    assert approx(a.takeoff_time) == 0
+    assert approx(a.output_frames_min_z) == 1
+    assert approx(a.get_scaled_output(ratio=[1,2,3], offset=[4,5,6])[0].get_pos()) == [4.21774,7.8,9]
+    assert approx(a.get_scaled_output_min_z(ratio=[1,2,3], offset=[4,5,6])) == 9
+    assert approx(a.get_start_point(ratio=[1,2,3], offset=[4,5,6])) == [4.21774,7.8,9]
+
+def test_animation_no_file():
+    a.update_frames(config, "zzz.csv")
+    assert a.id == None
+    assert a.original_frames == []
+    assert a.output_frames == []
+    assert animation_lib.get_numbers(a.static_begin_frames) == []
+    assert animation_lib.get_numbers(a.takeoff_frames) == []
+    assert animation_lib.get_numbers(a.route_frames) == []
+    assert animation_lib.get_numbers(a.land_frames) == []
+    assert animation_lib.get_numbers(a.static_end_frames) == []
+    assert a.static_begin_time == 0
+    assert a.takeoff_time == 0
+    assert a.output_frames_min_z is None
+    assert a.get_scaled_output(ratio=[1,2,3], offset=[4,5,6]) == []
+    assert a.get_scaled_output_min_z(ratio=[1,2,3], offset=[4,5,6]) is None
+    assert a.get_start_point(ratio=[1,2,3], offset=[4,5,6]) == []
+
+
+# print animation_lib.get_numbers(a.static_begin_frames)
+# print animation_lib.get_numbers(a.takeoff_frames)
+# print animation_lib.get_numbers(a.route_frames)
+# print animation_lib.get_numbers(a.land_frames)
+# print animation_lib.get_numbers(a.static_end_frames)
+# print animation_lib.get_numbers(a.output_frames)
+# print a.static_begin_time
+# print a.takeoff_time
+# print a.output_frames_min_z
+
+shutil.rmtree('animation_config')

update_configspec.py

@@ -1,14 +1,34 @@
+import os
+import shutil
 import config
 from Server.copter_table_models import CopterDataModel
 
-cfg_server = config.ConfigObj('SERVER/config/spec/configspec_server.ini', list_values=False)
-widths = {"copter_id": 150}
-default_width = 100
+from config import ConfigManager, ConfigObj
 
-default = {key: f"preset_param(default=list(True, {widths.get(key, default_width)}))"
+config_path = 'temp_config/config'
+spec_path = os.path.join(config_path,'spec')
+if not os.path.exists(spec_path):
+    try:
+        os.makedirs(spec_path)
+    except OSError:
+        print("Creation of the directory {} failed".format(spec_path))
+    else:
+        print("Successfully created the directory {}".format(spec_path))
+
+shutil.copy("Server/config/spec/configspec_server.ini", spec_path)
+
+config = ConfigManager()
+config.load_config_and_spec(os.path.join(config_path,'server.ini'))
+
+preset_params = config.table_presets_default
+default_param = (True, 100)
+
+default = {key: f"preset_param(default=list{preset_params.get(key, default_param)})"
            for key in CopterDataModel.columns}
 
+cfg_server = ConfigObj('Server/config/spec/configspec_server.ini', list_values=False)
 cfg_server['TABLE']['PRESETS']['DEFAULT'] = default
-
 cfg_server.write()
-print('Server configspec updated')
+
+print('Server configspec updated!')
+shutil.rmtree('temp_config')