Initial commit of new animation_lib and async tasking_lib with suppurt of managing tasks on-the-go and interrupt them immediately. Improvements in FlightLib (including better selfcheck code) and client.

Massive testing required (will be done soon)
2026-05-30 00:39:32 +00:00 · 2019-05-05 22:24:39 +03:00
parent 1bc17e3d67
commit 67a31d70ae
8 changed files with 511 additions and 133 deletions
--- a/Drone/FlightLib/FlightLib.py
+++ b/Drone/FlightLib/FlightLib.py
@@ -11,7 +11,7 @@ from mavros_msgs.srv import SetMode
 from mavros_msgs.srv import CommandBool
 from std_srvs.srv import Trigger

-module_logger = logging.getLogger("FlightLib")
+logger = logging.getLogger(__name__)

 # create proxy service
 navigate = rospy.ServiceProxy('navigate', srv.Navigate)
@@ -22,24 +22,26 @@ get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
 arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
 landing = rospy.ServiceProxy('/land', Trigger)

-module_logger.info("Proxy services inited")
+logger.info("Proxy services inited")

 # globals
-FREQUENCY = 1000/25  # HZ
+FREQUENCY = 1000 / 25  # HZ
 TOLERANCE = 0.2

 interrupt_event = threading.Event()

+checklist = []
+

 def interrupt():
-    module_logger.info("Performing function interrupt")
+    logger.info("Performing function interrupt")
    interrupt_event.set()


 def init(node_name="CleverSwarmFlight", anon=True, no_signals=True):
-    module_logger.info("Initing ROS node")
+    logger.info("Initing ROS node")
    rospy.init_node(node_name, anonymous=anon, disable_signals=no_signals)
-    module_logger.info("Ros node inited")
+    logger.info("Ros node inited")


 def get_distance3d(x1, y1, z1, x2, y2, z2):
@@ -55,89 +57,78 @@ def check(check_name):
                for failure in failures:
                    msg = "[{}]: Failure: {}".format(check_name, failure)
                    msgs.append(msg)
-                    module_logger.warning(msg)
+                    logger.warning(msg)
                return msgs
            else:
-                module_logger.info("[{}]: OK".format(check_name))
+                logger.info("[{}]: OK".format(check_name))
                return None
+
+        checklist.append(wrapper)
        return wrapper
+
    return inner


@check("Linear velocity estimation")
-def check_linear_speeds():
-    failures = []
+def check_linear_speeds(speed_limit=0.1):
    telemetry = get_telemetry(frame_id='body')
-    speed_limit = 0.1
+
    if telemetry.vx >= speed_limit:
-        failures.append("X velocity estimation: {:.3f} m/s".format(telemetry.vx))
+        yield ("X velocity estimation: {:.3f} m/s".format(telemetry.vx))
    if telemetry.vy >= speed_limit:
-        failures.append("Y velocity estimation: {:.3f} m/s".format(telemetry.vy))
+        yield ("Y velocity estimation: {:.3f} m/s".format(telemetry.vy))
    if telemetry.vz >= speed_limit:
-        failures.append("Z velocity estimation: {:.3f} m/s".format(telemetry.vz))
-    return failures
+        yield ("Z velocity estimation: {:.3f} m/s".format(telemetry.vz))


@check("Angular velocity estimation")
-def check_angular_speeds():
-    failures = []
+def check_angular_speeds(rate_limit=0.05):
    telemetry = get_telemetry(frame_id='body')
-    rate_limit = 0.05
+
    if telemetry.pitch_rate >= rate_limit:
-        failures.append("Pitch rate estimation: {:.3f} rad/s".format(telemetry.pitch_rate))
+        yield ("Pitch rate estimation: {:.3f} rad/s".format(telemetry.pitch_rate))
    if telemetry.roll_rate >= rate_limit:
-        failures.append("Roll rate estimation: {:.3f} rad/s".format(telemetry.roll_rate))
+        yield ("Roll rate estimation: {:.3f} rad/s".format(telemetry.roll_rate))
    if telemetry.yaw_rate >= rate_limit:
-        failures.append("Yaw rate estimation: {:.3f} rad/s".format(telemetry.yaw_rate))
-    return failures
+        yield ("Yaw rate estimation: {:.3f} rad/s".format(telemetry.yaw_rate))


@check("Angles estimation")
-def check_angles():
-    failures = []
+def check_angles(angle_limit=math.radians(5)):
    telemetry = get_telemetry(frame_id='body')
-    angle_limit = math.radians(5)
    if abs(telemetry.pitch) >= angle_limit:
-        failures.append("Pitch estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.pitch,
-                                                                             math.degrees(telemetry.pitch)))
+        yield ("Pitch estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.pitch,
+                                                                    math.degrees(telemetry.pitch)))
    if abs(telemetry.roll) >= angle_limit:
-        failures.append("Roll estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.roll,
-                                                                             math.degrees(telemetry.roll)))
+        yield ("Roll estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.roll,
+                                                                   math.degrees(telemetry.roll)))
    if abs(telemetry.yaw) >= angle_limit:
-        failures.append("Yaw estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.yaw,
-                                                                             math.degrees(telemetry.yaw)))
-    return failures
+        yield ("Yaw estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.yaw,
+                                                                  math.degrees(telemetry.yaw)))


 def selfcheck():
-    msgs = []
-    linear_speeds = check_linear_speeds()
-    angular_speeds = check_angular_speeds()
-    angles = check_angles()
-    if linear_speeds:
-        msgs.extend(linear_speeds)
-    if angular_speeds:
-        msgs.extend(angular_speeds)
-    if angles:
-        msgs.extend(angles)
+    checks = []
+    for check_f in checklist:
+        msg = check_f()
+        checks += msg if msg else []

-    return msgs
+    return checks


-def navto(x, y, z, yaw=float('nan'), frame_id='aruco_map'):
+def navto(x, y, z, yaw=float('nan'), frame_id='aruco_map', **kwargs):
    set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw)
    telemetry = get_telemetry(frame_id=frame_id)

-    module_logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
-    module_logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z))
+    logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
+    logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z))

    return True


 def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLERANCE, frame_id='aruco_map',
-                freq=FREQUENCY, timeout=5000, wait=False):
-
-    module_logger.info('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
+                freq=FREQUENCY, timeout=5000, wait=False, interrupter=interrupt_event):
+    logger.info('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
    navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed)

    # waiting for completion
@@ -146,31 +137,32 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLE
    time_start = rospy.get_rostime()

    while (get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z) > tolerance) or wait:
-        if interrupt_event.is_set():
-            module_logger.warning("Flight function interrupted!")
-            interrupt_event.clear()
-            break
+        if interrupter.is_set():
+            logger.warning("Flight function interrupted!")
+            interrupter.clear()
+            return

        telemetry = get_telemetry(frame_id=frame_id)
-        module_logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
+        logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
            telemetry.x, telemetry.y, telemetry.z, telemetry.yaw))
+        logger.info('Current delta: | {:.3f}'.format(
+            get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z)))

        time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000

        if timeout is not None:
            if time_passed >= timeout:
-                module_logger.warning('Reaching point timed out! | time: {:3f} seconds'.format(time_passed / 1000))
+                logger.warning('Reaching point timed out! | time: {:3f} seconds'.format(time_passed / 1000))
                return wait
        rate.sleep()
    else:
-        module_logger.info("Point reached!")
+        logger.info("Point reached!")
        return True


 def reach_attitude(z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLERANCE, frame_id='aruco_map',
-                   freq=FREQUENCY, timeout=5000, wait=False):
-
-    module_logger.info('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw))
+                   freq=FREQUENCY, timeout=5000, wait=False, interrupter=interrupt_event):
+    logger.info('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw))
    current_telem = get_telemetry(frame_id=frame_id)
    navigate(frame_id=frame_id, x=current_telem.x, y=current_telem.y, z=z, yaw=yaw, speed=speed)

@@ -180,31 +172,36 @@ def reach_attitude(z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLERANCE, fram
    time_start = rospy.get_rostime()

    while (abs(z - telemetry.z) > tolerance) or wait:
-        if interrupt_event.is_set():
-            module_logger.warning("Flight function interrupted!")
-            interrupt_event.clear()
-            break
+        if interrupter.is_set():
+            logger.warning("Flight function interrupted!")
+            interrupter.clear()
+            return

        telemetry = get_telemetry(frame_id=frame_id)
-        module_logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
+        logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
            telemetry.x, telemetry.y, telemetry.z, telemetry.yaw))

        time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000
        if timeout is not None:
            if time_passed >= timeout:
-                module_logger.warning('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed / 1000))
+                logger.warning('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed / 1000))
                return wait
        rate.sleep()
    else:
-        module_logger.info("Attitude reached!")
+        logger.info("Attitude reached!")
        return True


+def stop(frame_id='body', hold_speed=1.0):
+    navigate(frame_id=frame_id, yaw=float('nan'), speed=hold_speed)
+
+
 def land(descend=True, z=1.0, frame_id_descend="aruco_map", frame_id_land="aruco_map",
-         timeout_descend=5000, timeout_land=7500, freq=FREQUENCY):
+         timeout_descend=5000, timeout_land=7500, freq=FREQUENCY, interrupter=interrupt_event):
    if descend:
-        module_logger.info("Descending to: | z: {:.3f}".format(z))
-        reach_attitude(z=z, frame_id=frame_id_descend, timeout=timeout_descend, freq=freq, yaw=1.57) #TODO yaw
+        logger.info("Descending to: | z: {:.3f}".format(z))
+        reach_attitude(z=z, frame_id=frame_id_descend, timeout=timeout_descend, freq=freq, yaw=1.57,  # TODO yaw
+                       interrupter=interrupter)
    landing()

    telemetry = get_telemetry(frame_id='aruco_map')
@@ -212,32 +209,32 @@ def land(descend=True, z=1.0, frame_id_descend="aruco_map", frame_id_land="aruco
    time_start = rospy.get_rostime()

    while telemetry.armed:
-        if interrupt_event.is_set():
-            module_logger.warning("Flight function interrupted!")
-            interrupt_event.clear()
-            break
+        if interrupter.is_set():
+            logger.warning("Flight function interrupted!")
+            interrupter.clear()
+            return

        telemetry = get_telemetry(frame_id=frame_id_land)
-        module_logger.info("Landing...")
+        logger.info("Landing... | z: {}".format(telemetry.z))
        time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000

        if timeout_land is not None:
            if time_passed >= timeout_land:
-                module_logger.warning('Landing timed out! | time: {:3f} seconds'.format(time_passed / 1000))
-                module_logger.warning("Disarming!")
+                logger.warning('Landing timed out! | time: {:3f} seconds'.format(time_passed / 1000))
+                logger.warning("Disarming!")
                arming(False)
                return False
        rate.sleep()
    else:
-        module_logger.info("Landing succeeded!")
+        logger.info("Landing succeeded!")
        return True


-def takeoff(z=1.0,  speed=0.8, frame_id='body', freq=FREQUENCY,
-            timeout_arm=2000,  timeout_takeoff=5000, wait=False, tolerance=TOLERANCE, emergency_land=False):
-    module_logger.info("Starting takeoff!")
-    print("Starting takeoff!")
-    module_logger.info("Arming, going to OFFBOARD mode")
+def takeoff(z=1.0, speed=0.8, frame_id='body', freq=FREQUENCY,
+            timeout_arm=2000, timeout_takeoff=5000, wait=False, tolerance=TOLERANCE, emergency_land=False,
+            interrupter=interrupt_event):
+    logger.info("Starting takeoff!")
+    logger.info("Arming, going to OFFBOARD mode")

    # Arming check
    set_rates(thrust=0.1, auto_arm=True)
@@ -246,54 +243,53 @@ def takeoff(z=1.0,  speed=0.8, frame_id='body', freq=FREQUENCY,
    time_start = rospy.get_rostime()

    while (not telemetry.armed) or wait:
-        if interrupt_event.is_set():
-            module_logger.warning("Flight function interrupted!")
-            interrupt_event.clear()
-            return None
+        if interrupter.is_set():
+            logger.warning("Flight function interrupted!")
+            interrupter.clear()
+            return

        telemetry = get_telemetry(frame_id=frame_id)
-        module_logger.info("Arming...")
+        logger.info("Arming...")
        time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000

        if timeout_arm is not None:
            if time_passed >= timeout_arm:
                if not telemetry.armed:
-                    module_logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed / 1000))
+                    logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed / 1000))
                    return False
                else:
                    break
        rate.sleep()

-    module_logger.info("Armed!")
-    print("Armed!")
-    
-    # Reach height
-    telemetry = get_telemetry(frame_id=frame_id)
-    z0 = get_telemetry().z
-    navigate(z=z, speed=speed, frame_id=frame_id, auto_arm=True)
-    current_height = abs(get_telemetry().z - z0 - z)
-    while current_height > tolerance or wait:
-        if interrupt_event.is_set():
-            module_logger.warning("Flight function interrupted!")
-            interrupt_event.clear()
-            return None
+    logger.info("Armed!")

-        current_height = abs(get_telemetry().z - z0 - z)
-        module_logger.info("Takeoff...")
+    # Reach height
+    z0 = get_telemetry().z
+    z_dest = z + z0
+    navigate(z=z_dest, speed=speed, frame_id=frame_id, auto_arm=True)
+    current_height = abs(get_telemetry().z - z_dest)
+    while current_height > tolerance or wait:
+        if interrupter.is_set():
+            logger.warning("Flight function interrupted!")
+            interrupter.clear()
+            return
+
+        current_height = abs(get_telemetry().z - z_dest)
+        logger.info("Takeoff...")
        time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000

        if timeout_takeoff is not None:
            if time_passed >= timeout_takeoff:
                if not wait:
-                    module_logger.warning('Takeoff timed out! | time: {:3f} seconds'.format(time_passed / 1000))
+                    logger.warning('Takeoff timed out! | time: {:3f} seconds'.format(time_passed / 1000))
                    if emergency_land:
-                        module_logger.info("Preforming emergency land")
-                        land(descend=False)
+                        logger.info("Preforming emergency land")
+                        land(descend=False, interrupter=interrupter)
                    return False
                else:
                    break
        rate.sleep()

-    module_logger.info("Takeoff succeeded!")
-    print("Takeoff succeeded!")
+    logger.info("Takeoff succeeded!")
+    # print("Takeoff succeeded!")
    return True
--- a/Drone/animation_lib.py
+++ b/Drone/animation_lib.py
@@ -0,0 +1,90 @@
+import time
+import csv
+import rospy
+import logging
+import threading
+
+from FlightLib import FlightLib
+from FlightLib import LedLib
+
+import tasking_lib as tasking
+
+logger = logging.getLogger(__name__)
+
+interrupt_event = threading.Event()
+
+
+def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0):
+    imported_frames = []
+    try:
+        animation_file = open(filepath)
+    except IOError:
+        logging.error("File {} can't be opened".format(filepath))
+    else:
+        with animation_file:
+            csv_reader = csv.reader(
+                animation_file, delimiter=',', quotechar='|'
+            )
+            for row in csv_reader:
+                frame_number, x, y, z, yaw, red, green, blue = row
+                imported_frames.append({
+                    'number': int(frame_number),
+                    'x': float(x) + x0,
+                    'y': float(y) + y0,
+                    'z': float(z) + z0,
+                    'yaw': float(yaw),
+                    'red': int(red),
+                    'green': int(green),
+                    'blue': int(blue),
+                })
+        return imported_frames
+
+
+def convert_frame(frame):
+    return (frame['x'], frame['y'], frame['z']), (frame["red"], frame["green"], frame["blue"]), frame["yaw"]
+
+
+def execute_frame(point=(), color=(), yaw=float('Nan'), frame_id='aruco_map', use_leds=True,
+                  flight_func=FlightLib.navto, flight_kwargs=None, interrupter=interrupt_event):
+
+    if flight_kwargs is None:
+        flight_kwargs = {}
+
+    flight_func(*point, yaw=yaw, frame_id=frame_id, interrupter=interrupt_event, **flight_kwargs)
+    if use_leds:
+        if 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, timeout=5000, frame_id='aruco_map', use_leds=True,
+            interrupter=interrupt_event):
+    if use_leds:
+        LedLib.wipe_to(255, 0, 0)
+    FlightLib.takeoff(z=z, wait=False, timeout_takeoff=timeout, frame_id=frame_id, emergency_land=safe_takeoff,
+                      interrupter=interrupter)
+    if use_leds:
+        LedLib.blink(0, 255, 0, wait=50)
+
+
+def land(z=1.5, descend=False, timeout=5000, frame_id='aruco_map', use_leds=True,
+         interrupter=interrupt_event):
+    if use_leds:
+        LedLib.blink(255, 0, 0)
+    FlightLib.land(z=z, descend=descend, timeout_land=timeout, frame_id_land=frame_id, interrupter=interrupter)
+    if use_leds:
+        LedLib.off()
--- a/Drone/client.py
+++ b/Drone/client.py
@@ -110,7 +110,7 @@ class Client(object):
                self._reconnect()
                self._process_connections()

-        except (KeyboardInterrupt, InterruptedError):
+        except (KeyboardInterrupt, ):
            logger.critical("Caught interrupt, exiting!")
            self.selector.close()

@@ -187,11 +187,12 @@ class Client(object):
                        try:
                            connection.process_events(mask)
                        except Exception as error:
-                            logger.error(
-                                "Exception {} occurred for {}! Resetting connection!".format(error, connection.addr)
-                            )
-                            self.server_connection.close()
-                            self.connected = False
+                            if error.errno != errno.EINTR:
+                                logger.error(
+                                    "Exception {} occurred for {}! Resetting connection!".format(error, connection.addr)
+                                )
+                                self.server_connection.close()
+                                self.connected = False
                            break

            if not self.selector.get_map():
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -1,41 +1,46 @@
 import os
 import time
 import rospy
+import logging

 from FlightLib import FlightLib
 from FlightLib import LedLib

 import client
+
 import messaging_lib as messaging
-import play_animation
+import tasking_lib as tasking
+import animation_lib as animation
+
 import ros_logging

+logger = logging.getLogger(__name__)
+

 class CopterClient(client.Client):
    def load_config(self):
        super(CopterClient, self).load_config()
        self.FRAME_ID = self.config.get('COPTERS', 'frame_id')
-        play_animation.FRAME_ID = self.FRAME_ID
        self.TAKEOFF_HEIGHT = self.config.getfloat('COPTERS', 'takeoff_height')
        self.TAKEOFF_TIME = self.config.getfloat('COPTERS', 'takeoff_time')
        self.SAFE_TAKEOFF = self.config.getboolean('COPTERS', 'safe_takeoff')
        self.RFP_TIME = self.config.getfloat('COPTERS', 'reach_first_point_time')

-
        self.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common')
        self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common')
        self.X0 = self.config.getfloat('PRIVATE', 'x0')
        self.Y0 = self.config.getfloat('PRIVATE', 'y0')

        self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds')
-        play_animation.USE_LEDS = self.USE_LEDS

    def start(self):
        super(CopterClient, self).start()
+        logger.info("Init ROS node")
        rospy.init_node('Swarm_client', anonymous=True)
        if self.USE_LEDS:
            LedLib.init_led()

+
@messaging.request_callback("selfcheck")
 def _response_selfcheck():
    check = FlightLib.selfcheck()
@@ -54,11 +59,11 @@ def _response_cell():

@messaging.message_callback("service_restart")
 def _command_service_restart(*args, **kwargs):
-    os.system("systemctl restart"+kwargs["name"])
+    os.system("systemctl restart" + kwargs["name"])


@messaging.message_callback("led_test")
-def _command_config_write(*args, **kwargs):
+def _command_led_test(*args, **kwargs):
    LedLib.chase(255, 255, 255)
    time.sleep(2)
    LedLib.off()
@@ -66,14 +71,107 @@ def _command_config_write(*args, **kwargs):

@messaging.message_callback("takeoff")
 def _command_takeoff(*args, **kwargs):
-    play_animation.takeoff(z=client.active_client.TAKEOFF_HEIGHT,
-                           timeout=client.active_client.TAKEOFF_TIME,
-                           safe_takeoff=client.active_client.SAFE_TAKEOFF,
-                           )
+    task_manager.add_task(0, 0, animation.takeoff,
+                          task_kwargs={
+                              "z": client.active_client.TAKEOFF_HEIGHT,
+                              "timeout": client.active_client.TAKEOFF_TIME,
+                              "safe_takeoff": client.active_client.SAFE_TAKEOFF,
+                              "use_leds": client.active_client.USE_LEDS,
+                          }
+                          )
+
+
+@messaging.message_callback("land")
+def _command_land(*args, **kwargs):
+    task_manager.reset()
+    task_manager.add_task(-5, 0, animation.land,
+                          task_kwargs={
+                              "z": client.active_client.TAKEOFF_HEIGHT,
+                              "timeout": client.active_client.TAKEOFF_TIME,
+                              "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                          }
+                          )
+
+
+@messaging.message_callback("disarm")
+def _command_disarm(*args, **kwargs):
+    task_manager.reset()
+    task_manager.add_task(-10, 0, FlightLib.arming(False))
+
+
+@messaging.message_callback("stop")
+def _command_stop(*args, **kwargs):
+    task_manager.stop()
+
+
+@messaging.message_callback("pause")
+def _command_stop(*args, **kwargs):
+    task_manager.pause()
+
+
+@messaging.message_callback("resume")
+def _command_stop(*args, **kwargs):
+    task_manager.resume()
+
+
+@messaging.message_callback("start_animation")
+def _play_animation(*args, **kwargs):
+    gap = 0.25
+    start_time = kwargs["start_time"]  # TODO
+    frames = animation.load_animation(os.path.abspath("animation.csv"),
+                                      x0=client.active_client.X0 + client.active_client.X0_COMMON,
+                                      y0=client.active_client.Y0 + client.active_client.Y0_COMMON,
+                                      )
+
+    task_manager.add_task(start_time, -1, animation.takeoff,
+                          task_kwargs={
+                              "z": client.active_client.TAKEOFF_HEIGHT,
+                              "timeout": client.active_client.TAKEOFF_TIME,
+                              "safe_takeoff": client.active_client.SAFE_TAKEOFF,
+                              "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                          }
+                          )
+
+    rfp_time = start_time + client.active_client.TAKEOFF_TIME + gap
+    task_manager.add_task(rfp_time, -1, animation.execute_frame,
+                          task_kwargs={
+                              "point": animation.convert_frame(frames[0]),
+                              "timeout": client.active_client.RFP_TIME,
+                              "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                              "flight_func": FlightLib.reach_point,
+                          }
+                          )
+
+    animation_time = rfp_time + client.active_client.RFP_TIME + gap
+    frame_delay = 0.125  # TODO from animation file
+    task_manager.add_task(animation_time, -1, animation.execute_animation,
+                          task_kwargs={
+                              "frames": frames,
+                              "frame_delay": frame_delay,
+                              "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                          }
+                          )
+
+    land_time = animation_time + len(frames)*frame_delay + gap
+    task_manager.add_task(land_time, -1, animation.land,
+                          task_kwargs={
+                              "z": client.active_client.TAKEOFF_HEIGHT,
+                              "timeout": client.active_client.TAKEOFF_TIME,
+                              "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                          },
+                          )


 if __name__ == "__main__":
+    # rospy.init_node('Swarm_client', anonymous=True)
    ros_logging.route_logger_to_ros()
-    rospy.init_node('Swarm_client', anonymous=True)
    copter_client = CopterClient()
+    task_manager = tasking.TaskManager()
+
+    task_manager.start()
    copter_client.start()
--- a/Drone/play_animation.py
+++ b/Drone/play_animation.py
@@ -5,7 +5,7 @@ import logging
 from FlightLib import FlightLib
 from FlightLib import LedLib

-module_logger = logging.getLogger("Animation player")
+logger = logging.getLogger(__name__)


 animation_file_path = 'animation.csv'
@@ -17,7 +17,7 @@ def takeoff(z=1.5, safe_takeoff=True, timeout=5000):
    if USE_LEDS:
        LedLib.wipe_to(255, 0, 0)
    FlightLib.takeoff(z=z, wait=True, timeout_takeoff=timeout, emergency_land=safe_takeoff,)
-    LedLib.blink(0, 255, 0)
+    LedLib.blink(0, 255, 0, wait=50)


 def land(descend=False):
--- a/Drone/tasking_lib.py
+++ b/Drone/tasking_lib.py
@@ -0,0 +1,193 @@
+import heapq
+import time
+import logging
+import threading
+import collections
+
+logger = logging.getLogger(__name__)
+Task = collections.namedtuple("Task", ["func", "args", "kwargs", "delayable", ])
+
+
+def wait(end, interrupter=None, maxsleep=1):  # Added features to interrupter sleep and set max sleeping interval
+    def interrupted():
+        if interrupter is None:
+            return False
+        else:
+            return interrupter.is_set()
+
+    while not interrupted():  # Basic implementation of pause module until()
+        now = time.time()
+        diff = min(end - now, maxsleep)
+        if diff <= 0:
+            break
+        else:
+            time.sleep(diff / 2)
+
+
+class TaskManager(object):
+    def __init__(self):
+        self.task_queue = []
+        self._active_task = None
+
+        self._processor_thread = threading.Thread(target=self._task_processor, name="Task processing thread")
+        self._processor_thread.daemon = True
+
+        self._timeout_thread = threading.Thread(target=self._task_time_interrupter, name="Task timeouts thread")
+        self._processor_thread.daemon = True
+
+        self._task_lock = threading.RLock()
+        self._queue_lock = threading.RLock()
+
+        self._running_event = threading.Event()
+        self._interrupt_event = threading.Event()
+        self.shutdown_event = threading.Event()
+
+    def add_task(self, timestamp, priority, task_function,
+                 task_args=(), task_kwargs=None, task_delayable=False):
+        if task_kwargs is None:
+            task_kwargs = {}
+
+        heapq.heappush(self.task_queue, (timestamp, priority,
+                                         Task(task_function, task_args, task_kwargs, task_delayable)
+                                         ))
+        logger.debug(self.task_queue)
+        if self._processor_thread.is_alive():
+            self._update_queue()
+
+    def _remove_task(self, task):
+        with self._queue_lock:
+            self.task_queue.remove(task)
+            heapq.heapify(self.task_queue)
+
+    def start(self, timeouts=False):
+        logger.info("Starting")
+        self._processor_thread.start()
+        if timeouts:
+            self._timeout_thread.start()
+        self.resume()
+
+    def stop(self):
+        self.pause(interrupt=True)
+        with self._queue_lock:
+            self.task_queue = []
+
+    def shutdown(self):
+        self.stop()
+        self.shutdown_event.set()
+        self._running_event.clear()
+        self._processor_thread.join(timeout=5)
+
+    def pause(self, interrupt=False):
+        if interrupt:
+            self.interrupt()
+        self._running_event.clear()
+        logger.info("Task queue paused")
+
+    def resume(self):
+        self._running_event.set()
+        logger.info("Task queue resumed")
+
+    def reset(self):
+        self.stop()
+        self.resume()
+
+    def _update_queue(self):
+        logger.info("Queue updated")
+        with self._queue_lock, self._task_lock:
+            if self.task_queue:
+                if self._active_task is None:
+                    self._active_task = self.task_queue[0]
+                elif self.task_queue[0] is not self._active_task:
+                    if self.task_queue[0] < self._active_task:
+                        self.change_active_task(self.task_queue[0])
+
+    def interrupt(self):
+        self._interrupt_event.set()
+        while self._interrupt_event.is_set():
+            pass
+        logger.info("Task execution successfully interrupted")
+
+    def change_active_task(self, task):
+        self.pause(interrupt=True)
+
+        with self._task_lock:
+            if not self._active_task[2].delayable:
+                self._remove_task(self._active_task)
+            self._active_task = task
+
+        self.resume()
+
+    def execute_task(self):
+        with self._task_lock:
+            task = self._active_task[2]
+            start_time = self._active_task[0]
+
+        logger.info("Waiting util task execution time:{}".format(start_time))
+        wait(start_time, self._interrupt_event, 1)
+
+        if not self._interrupt_event.is_set():
+            logger.info("Executing task {}".format(task))
+            try:
+                task.func(*task.args, interrupter=self._interrupt_event, **task.kwargs)
+            except Exception as e:
+                logger.error("Error '{}' occurred in task {}".format(e, task))
+        else:
+            logger.warning("Task interrupted before execution")
+
+        self._interrupt_event.clear()
+
+        try:
+            logger.debug("Removing task")
+            self._remove_task(self._active_task)
+        except ValueError:
+            logger.warning("Task already removed, probably task changed")
+        else:
+            with self._task_lock:
+                self._active_task = None
+
+            self._update_queue()
+            logger.info("Execution done")
+
+    def _task_processor(self):
+        logger.info("Tasking thread started")
+        self._update_queue()  # Initial tick if tasks added before start
+        while not self.shutdown_event.is_set():
+            self._running_event.wait()
+            if self._active_task is not None:
+                self.execute_task()
+
+    def _task_time_interrupter(self):  # TODO revork; temporary disabled
+        raise NotImplementedError
+        logger.info("Timeouts thread started")
+        while not self.shutdown_event.is_set():
+            self._running_event.wait()
+            try:
+                if self.task_queue[1] is not self._active_task:  # If pending task is more important than current
+                    if self.task_queue[1] < self._active_task:  # TODO look at timeout time
+                        logger.warning("Changing low-priority task due timeout")
+                        self.change_active_task(self.task_queue[1])
+            except IndexError:
+                time.sleep(0.01)
+
+
+if __name__ == "__main__":
+    logger.addHandler(logging.StreamHandler())
+    logger.setLevel(logging.DEBUG)
+
+    def printer(stri, interrupter, *args, **kwargs):
+        logger.info("String: {}, timenow: {}".format(stri, time.time()))
+        wait(time.time()+30, interrupter)
+
+    tasker = TaskManager()  # Lower priority first!
+    tasker.start()
+
+    tasker.add_task(0, 10, printer, ("Task1 ", ))
+    time.sleep(1)
+    tasker.add_task(time.time(), 10, printer, ("Lol ", ))
+    tasker.add_task(time.time()+10, 5, printer, ("Kek ", ))
+    tasker.add_task(time.time()+7, 1, printer, ("Dededededee", ))
+    time.sleep(3)
+    tasker.add_task(time.time()+7, 0, printer, ("Iiiiiii", ))
+
+    while True:
+        pass
--- a/Server/server_qt.py
+++ b/Server/server_qt.py
@@ -52,6 +52,7 @@ class MainWindow(QtWidgets.QMainWindow):
                copter.get_response("batt_voltage", self._set_copter_data, callback_args=(row_num, 2))
                copter.get_response("cell_voltage", self._set_copter_data, callback_args=(row_num, 3))
                copter.get_response("selfcheck", self._set_copter_data, callback_args=(row_num, 4))
+                copter.get_response("time", self._set_copter_data, callback_args=(row_num, 5))

        self.ui.start_button.setEnabled(True)
        self.ui.takeoff_button.setEnabled(True)
@@ -67,6 +68,8 @@ class MainWindow(QtWidgets.QMainWindow):
                model.setData(model.index(row, col), str(value))  # TODO different handling
            else:
                model.setData(model.index(row, col), str(value))
+        elif col == 5:
+            model.setData(model.index(row, col), time.ctime(int(value)))

    @pyqtSlot()
    def send_starttime(self):
--- a/messaging_lib.py
+++ b/messaging_lib.py
@@ -297,22 +297,19 @@ class ConnectionManager(object):
        except KeyError:
            logger.warning("Request {} does not exist!".format(command))
        except Exception as error:  # TODO send response error\cancel
-            logger.error("Error during command {} execution: {}".format(command, error))
+            logger.error("Error during request {} processing: {}".format(command, error))
        else:
            self._send_response(command, request_id, value)

    def _process_response(self, message):
        request_id, requested_value = message.content["request_id"], message.content["requested_value"]
        with self._request_lock:
-            for key, value in self._request_queue.items():
+            for key, value in self._request_queue.items():  # TODO as try []
                if (key == request_id) and (value.requested_value == requested_value):
-                    print(54)
                    request = self._request_queue.pop(key)
-                    print(request)
                    value = message.content["value"]
-                    print(45)
                    logger.debug(
-                        "Request successfully closed with value {}".format(message.content["value"])
+                        "Request {} successfully closed with value {}".format(request, message.content["value"])
                    )

                    f = request.callback