Drone: Rewrite copter_client with new animation module

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)
@@ -63,20 +63,28 @@ 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)
+[[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
 
@@ -46,6 +58,12 @@ def azi(x, y):
 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
@@ -96,8 +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()
@@ -111,7 +129,7 @@ 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()
@@ -158,11 +176,11 @@ class CopterClient(client.Client):
                     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']))
+                    #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))
+                    #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
@@ -314,13 +332,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)
@@ -347,28 +365,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")
@@ -405,9 +409,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")
@@ -436,60 +440,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")
 
 
@@ -512,7 +517,7 @@ def _command_service_restart(*args, **kwargs):
 
 @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")
@@ -527,13 +532,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")
@@ -541,30 +545,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")
@@ -572,12 +582,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")
@@ -591,8 +600,7 @@ def _command_disarm(*args, **kwargs):
     task_manager.add_task(-5, 0, FlightLib.arming_wrapper,
                           task_kwargs={
                               "state": False
-                          }
-                          )
+                          })
 
 
 @messaging.message_callback("stop")
@@ -612,109 +620,104 @@ 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
+    start_action, start_delay = copter.telemetry.start_action_and_delay
+
+    # 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':
         # 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,
@@ -723,6 +726,7 @@ class Telemetry:
         "selfcheck": None,
         "current_position": None,
         "start_position": None,
+        "start_action_and_delay": None,
         "last_task": None,
         "time_delta": None,
         "config_version": None,
@@ -759,16 +763,16 @@ 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_start, y_start, z_start = animation.get_start_pos(os.path.abspath("animation.csv"),
+                                                  *copter.config.animation_ratio)
+        offset = numpy.array(copter.config.private_offset) + numpy.array(copter.config.copter_common_offset)
         x = x_start + offset[0]
         y = y_start + offset[1]
-        z = offset[2]
+        z = z_start + offset[2]
         if not FlightLib._check_nans(x, y, z):
             return x, y, z
         return 'NO_POS'
@@ -807,14 +811,25 @@ class Telemetry:
     def get_position(cls, ros_telemetry):
         x, y, z = ros_telemetry.x, ros_telemetry.y, ros_telemetry.z
         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 get_start_action_and_delay(self)
+        offset = numpy.array(copter.config.private_offset) + numpy.array(copter.config.copter_common_offset)
+        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)
+        start_delay = copter.animation.start_delay
+        return start_action, start_delay
+
     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
@@ -832,13 +847,12 @@ class Telemetry:
         self.time_delta = time.time()
         self.round_telemetry()
 
-    def get_ros_telemetry(self):
-        return self.ros_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()
+        self.start_action_and_delay = self.get_start_action_and_delay()
         try:
             self.calibration_status = get_calibration_status()
             self.fcu_status = get_sys_status()
@@ -905,7 +919,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)
 
@@ -935,7 +949,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()
@@ -944,14 +958,14 @@ 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,))  # TODO MOVE? Daemon?
             slow_telemetry_thread = threading.Thread(target=self._slow_update_loop,
                                                      name="Slow telemetry getting thread")
             slow_telemetry_thread.start()
@@ -971,8 +985,23 @@ 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))
+        if event.src_path == "./animation.csv" and event.event_type == "modified":
+            copter.animation.update_frames(copter.config, "animation.csv")
+
+
 if __name__ == "__main__":
-    copter_client = CopterClient()
+    copter = CopterClient()
     task_manager = tasking.TaskManager()
     rospy.Subscriber('/emergency', Bool, emergency_callback)
-    copter_client.start(task_manager)
+    copter.start(task_manager)
+    event_handler = AnimationEventHandler()
+    observer = Observer()
+    observer.schedule(event_handler, ".")
+    observer.start()
+    while not rospy.is_shutdown:
+        rospy.sleep(1)
+    observer.stop()
+    observer.join()