Merge with master

Drone/FCU/clever_failsafe_and_power.params

@@ -0,0 +1,5 @@
+1	1	COM_LOW_BAT_ACT	2	6
+1	1	COM_OBL_ACT	0	6
+1	1	COM_OBL_RC_ACT	4	6
+1	1	BAT_V_CHARGED	4.050000190734863281	9
+1	1	BAT_V_EMPTY	3.500000000000000000	9

Drone/FlightLib/FlightLib.py

@@ -22,7 +22,8 @@ get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
 arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
 landing = rospy.ServiceProxy('/land', Trigger)
 
-services_list = [navigate, set_position, set_rates, set_mode, get_telemetry, arming,landing]
+services_list = ['/navigate', '/set_position', '/set_rates', '/mavros/set_mode', 
+                '/get_telemetry', '/mavros/cmd/arming', '/land', '/mavros/param/get']
 
 logger.info("Proxy services inited")
 
@@ -42,7 +43,11 @@ INTERRUPTER = threading.Event()
 FLIP_MIN_Z = 2.0
 
 checklist = []
+get_telemetry_lock = threading.Lock()
 
+def get_telemetry_locked(*args, **kwargs):
+    with get_telemetry_lock:
+        return get_telemetry(*args, **kwargs)
 
 def arming_wrapper(state=False, *args, **kwargs):
     arming(state)
@@ -76,7 +81,7 @@ def check(check_name):
             if msgs:
                 return msgs
             else:
-                logger.info("[{}]: OK".format(check_name))
+                logger.debug("[{}]: OK".format(check_name))
                 return None
 
         checklist.append(wrapper)
@@ -88,27 +93,34 @@ def check(check_name):
 def _check_nans(*values):
     return any(math.isnan(x) for x in values)
 
+def check_ros_services_unavailable():
+    unavailable_services = []
+    for service in services_list:
+        try:
+            rospy.wait_for_service(service, timeout=0.1)
+        except (rospy.ServiceException, rospy.ROSException):
+            unavailable_services.append(service)
+    return unavailable_services
 
 @check("Ros services")
 def check_ros_services():
-    timeout = 5.0
     for service in services_list:
         try:
-            service.wait_for_service(timeout=timeout)
-        except (rospy.ServiceException, rospy.ROSException) as e:
-            yield ("ROS service {} is not available!".format(service.name))
+            rospy.wait_for_service(service, timeout=0.1)
+        except (rospy.ServiceException, rospy.ROSException):
+            yield ("ROS service {} is not available!".format(service))
 
 
 @check("FCU connection")
 def check_connection():
-    telemetry = get_telemetry()
+    telemetry = get_telemetry_locked()
     if not telemetry.connected:
         yield ("Flight controller is not connected!")
 
 
 @check("Linear velocity estimation")
 def check_linear_speeds(speed_limit=0.15):
-    telemetry = get_telemetry(frame_id='body')
+    telemetry = get_telemetry_locked(frame_id='body')
 
     if _check_nans(telemetry.vx, telemetry.vy, telemetry.vz):
         yield ("Velocity estimation is not available")
@@ -123,7 +135,7 @@ def check_linear_speeds(speed_limit=0.15):
 
 @check("Angular velocity estimation")
 def check_angular_speeds(rate_limit=0.05):
-    telemetry = get_telemetry(frame_id='body')
+    telemetry = get_telemetry_locked(frame_id='body')
 
     if _check_nans(telemetry.pitch_rate, telemetry.roll_rate, telemetry.yaw_rate):
         yield ("Angular velocities estimation is not available")
@@ -138,7 +150,7 @@ def check_angular_speeds(rate_limit=0.05):
 
 @check("Angles estimation")
 def check_angles(angle_limit=math.radians(5)):
-    telemetry = get_telemetry(frame_id='body')
+    telemetry = get_telemetry_locked(frame_id='body')
 
     if _check_nans(telemetry.pitch, telemetry.roll, telemetry.yaw):
         yield ("Angular velocities estimation is not available")
@@ -165,7 +177,7 @@ def selfcheck():
 
 def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, auto_arm=False, **kwargs):
     set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, auto_arm=auto_arm)
-    #telemetry = get_telemetry(frame_id=frame_id)
+    #telemetry = get_telemetry_locked(frame_id=frame_id)
 
     logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
     #print('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
@@ -182,7 +194,7 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO
     navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed, auto_arm=auto_arm)
 
     # waiting for completion
-    telemetry = get_telemetry(frame_id=frame_id)
+    telemetry = get_telemetry_locked(frame_id=frame_id)
     rate = rospy.Rate(freq)
     time_start = time.time()
 
@@ -193,7 +205,7 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO
             interrupter.clear()
             return False
 
-        telemetry = get_telemetry(frame_id=frame_id)
+        telemetry = get_telemetry_locked(frame_id=frame_id)
         rospy.logdebug('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
             telemetry.x, telemetry.y, telemetry.z, telemetry.yaw))
         #print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
@@ -221,10 +233,10 @@ def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, fr
                    freq=FREQUENCY, timeout=TIMEOUT, interrupter=INTERRUPTER, wait=False):
     logger.info('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw))
     #print('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw))
-    current_telem = get_telemetry(frame_id=frame_id)
+    current_telem = get_telemetry_locked(frame_id=frame_id)
     navigate(frame_id=frame_id, x=current_telem.x, y=current_telem.y, z=z, yaw=yaw, speed=speed)
 
-    telemetry = get_telemetry(frame_id=frame_id)
+    telemetry = get_telemetry_locked(frame_id=frame_id)
     rate = rospy.Rate(freq)
     time_start = time.time()
 
@@ -235,7 +247,7 @@ def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, fr
             interrupter.clear()
             return
 
-        telemetry = get_telemetry(frame_id=frame_id)
+        telemetry = get_telemetry_locked(frame_id=frame_id)
         logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
             telemetry.x, telemetry.y, telemetry.z, telemetry.yaw))
         #print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
@@ -268,7 +280,7 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA
     landing()
     #print("Land is started")
 
-    telemetry = get_telemetry(frame_id=frame_id_land)
+    telemetry = get_telemetry_locked(frame_id=frame_id_land)
     rate = rospy.Rate(freq)
     time_start = time.time()
 
@@ -279,7 +291,7 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA
             interrupter.clear()
             return False
 
-        telemetry = get_telemetry(frame_id=frame_id_land)
+        telemetry = get_telemetry_locked(frame_id=frame_id_land)
         logger.info("Landing... | z: {}".format(telemetry.z))
         #print("Landing... | z: {}".format(telemetry.z))
         time_passed = time.time() - time_start
@@ -300,7 +312,7 @@ def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRA
 def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, timeout_takeoff=TIMEOUT, interrupter=INTERRUPTER, emergency_land=False):
     rospy.loginfo("Takeoff started...")
     rate = rospy.Rate(FREQUENCY)
-    start = get_telemetry(frame_id=frame_id)
+    start = get_telemetry_locked(frame_id=frame_id)
     climb = 0.
     time_start = time.time()
     result = navigate(z=height, speed=speed, yaw=float('nan'), frame_id="body", auto_arm=True)
@@ -314,7 +326,7 @@ def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, fra
             interrupter.clear()
             return 'interrupted'
 
-        climb = abs(get_telemetry(frame_id=frame_id).z - start.z)
+        climb = abs(get_telemetry_locked(frame_id=frame_id).z - start.z)
         rospy.logdebug("Takeoff to {:.2f} of {:.2f} meters".format(climb, height))
 
         time_passed = time.time() - time_start
@@ -333,7 +345,7 @@ def takeoff(height=TAKEOFF_HEIGHT, speed=TAKEOFF_SPEED, tolerance=TOLERANCE, fra
 def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different directions
     logger.info("Flip started!")
 
-    start_telemetry = get_telemetry(frame_id=frame_id)  # memorize starting position
+    start_telemetry = get_telemetry_locked(frame_id=frame_id)  # memorize starting position
     
     if start_telemetry.z < min_z - TOLERANCE:
         logger.warning("Can't do flip! Flip failed!")
@@ -347,7 +359,7 @@ def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different direc
         set_rates(roll_rate=30, thrust=0.2)  # maximum roll rate
 
         while True:
-            telem = get_telemetry()
+            telem = get_telemetry_locked()
 
             if abs(telem.roll) > math.pi/2:
                 break

Drone/animation_lib.py

@@ -24,7 +24,7 @@ def get_id(filepath="animation.csv"):
     try:
         animation_file = open(filepath)
     except IOError:
-        logging.error("File {} can't be opened".format(filepath))
+        logger.error("File {} can't be opened".format(filepath))
         anim_id = "No animation"
         return anim_id
     else:
@@ -35,11 +35,45 @@ def get_id(filepath="animation.csv"):
             row_0 = csv_reader.next()
             if len(row_0) == 1:
                 anim_id = row_0[0]
-                print("Got animation_id: {}".format(anim_id))
+                logger.debug("Got animation_id: {}".format(anim_id))
             else:
-                print("No animation id in file")
+                anim_id = "Empty id"
+                logger.debug("No animation id in file")
     return anim_id
 
+def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1):
+    try:
+        animation_file = open(filepath)
+    except IOError:
+        logger.error("File {} can't be opened".format(filepath))
+        anim_id = "No animation"
+        return float('nan'), float('nan')
+    else:
+        with animation_file:
+            csv_reader = csv.reader(
+                animation_file, delimiter=',', quotechar='|'
+            )
+            try:
+                row_0 = csv_reader.next()
+            except:
+                return float('nan'), float('nan')
+            if len(row_0) == 1:
+                anim_id = row_0[0]
+                logger.debug("Got animation_id: {}".format(anim_id))
+                try:
+                    frame_number, x, y, z, yaw, red, green, blue = csv_reader.next()
+                except:
+                    return float('nan'), float('nan')
+            else:
+                anim_id = "Empty id"
+                logger.debug("No animation id in file")
+                try:
+                    frame_number, x, y, z, yaw, red, green, blue = row_0
+                except:
+                    return float('nan'), float('nan')
+    return float(x)*x_ratio, float(y)*y_ratio
+
+
 def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_ratio=1, z_ratio=1):
     imported_frames = []
     global anim_id
@@ -56,9 +90,9 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_rati
             row_0 = csv_reader.next()
             if len(row_0) == 1:
                 anim_id = row_0[0]
-                print("Got animation_id: {}".format(anim_id))
+                logger.debug("Got animation_id: {}".format(anim_id))
             else:
-                print("No animation id in file")
+                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),

Drone/client.py

@@ -17,7 +17,6 @@ current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentfra
 parent_dir = os.path.dirname(current_dir)
 sys.path.insert(0, parent_dir) 
 
-logging.basicConfig(level=logging.DEBUG)
 logger = logging.getLogger(__name__)
 
 import messaging_lib as messaging
@@ -31,7 +30,7 @@ class Client(object):
         self.selector = selectors.DefaultSelector()
         self.client_socket = None
 
-        self.server_connection = messaging.ConnectionManager()
+        self.server_connection = messaging.ConnectionManager("pi")
 
         self.server_host = None
         self.server_port = None

Drone/client_config.ini

@@ -13,6 +13,10 @@ use_ntp = False
 host = ntp1.stratum2.ru
 port = 123
 
+[TELEMETRY]
+frequency = 1
+transmit = True
+
 [ANIMATION]
 takeoff_animation_check = True
 land_animation_check = True
@@ -22,7 +26,11 @@ y_ratio = 1.0
 z_ratio = 1.0
 
 [COPTERS]
+<<<<<<< HEAD
 frame_id = floor
+=======
+frame_id = map
+>>>>>>> master
 takeoff_height = 1.0
 takeoff_time = 5.0
 safe_takeoff = False

Drone/copter_client.py

@@ -1,9 +1,15 @@
 import os
+import sys
 import time
 import math
 import rospy
+from clever import srv
 import datetime
 import logging
+import threading
+import subprocess
+import ConfigParser
+from collections import namedtuple
 
 from FlightLib import FlightLib
 from FlightLib import LedLib
@@ -21,22 +27,54 @@ from tf.transformations import quaternion_from_euler, euler_from_quaternion, qua
 import tf2_ros
 
 static_bloadcaster = tf2_ros.StaticTransformBroadcaster()
+Telemetry = namedtuple("Telemetry", "git_version animation_id battery_v battery_p system_status calibration_status mode selfcheck current_position start_position")
+telemetry = Telemetry('nan', 'No animation', 'nan', 'nan', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_FCU', 'NO_POS', 'NO_POS')
 
-# logging.basicConfig(  # TODO all prints as logs
-#    level=logging.DEBUG, # INFO
-#    format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s",
-#    handlers=[
-#        logging.StreamHandler(),
-#    ])
+# get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+
+logging.basicConfig(  # TODO all prints as logs
+   level=logging.DEBUG, # INFO
+   stream=sys.stdout,
+   format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s",
+   handlers=[
+       logging.StreamHandler(sys.stdout),
+   ])
+
+handler = logging.StreamHandler(sys.stdout)
+handler.setLevel(logging.DEBUG)
+formatter = logging.Formatter("%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s")
+handler.setFormatter(formatter)
 
 logger = logging.getLogger(__name__)
+logger.setLevel(logging.DEBUG)
+logger.addHandler(handler)
 
+animation_logger = logging.getLogger('animation_lib')
+animation_logger.setLevel(logging.INFO)
+animation_logger.addHandler(handler)
 
-# import ros_logging
+client_logger = logging.getLogger('client')
+client_logger.setLevel(logging.DEBUG)
+client_logger.addHandler(handler)
+
+messaging_logger = logging.getLogger('messaging_lib')
+messaging_logger.setLevel(logging.INFO)
+messaging_logger.addHandler(handler)
+
+tasking_logger = logging.getLogger('tasking_lib')
+tasking_logger.setLevel(logging.INFO)
+tasking_logger.addHandler(handler)
+
+flightlib_logger = logging.getLogger('FlightLib')
+flightlib_logger.setLevel(logging.INFO)
+flightlib_logger.addHandler(handler)
 
 class CopterClient(client.Client):
     def load_config(self):
+        self.FLOOR_FRAME_EXISTS = False
         super(CopterClient, self).load_config()
+        self.TELEM_FREQ = self.config.getfloat('TELEMETRY', 'frequency')
+        self.TELEM_TRANSMIT = self.config.getboolean('TELEMETRY', 'transmit')
         self.FRAME_ID = self.config.get('COPTERS', 'frame_id')
         self.FRAME_FLIPPED_HEIGHT = 0.
         self.TAKEOFF_HEIGHT = self.config.getfloat('COPTERS', 'takeoff_height')
@@ -58,7 +96,18 @@ class CopterClient(client.Client):
         self.Z0 = self.config.getfloat('PRIVATE', 'z0')
         self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds')
         self.LED_PIN = self.config.getint('PRIVATE', 'led_pin')
-
+        try:
+            self.FLOOR_DX = self.config.getfloat('FLOOR FRAME', 'x')
+            self.FLOOR_DY = self.config.getfloat('FLOOR FRAME', 'y')
+            self.FLOOR_DZ = self.config.getfloat('FLOOR FRAME', 'z')
+            self.FLOOR_ROLL = self.config.getfloat('FLOOR FRAME', 'roll')
+            self.FLOOR_PITCH = self.config.getfloat('FLOOR FRAME', 'pitch')
+            self.FLOOR_YAW = self.config.getfloat('FLOOR FRAME', 'yaw')
+            self.FLOOR_PARENT = self.config.get('FLOOR FRAME', 'parent')
+            self.FLOOR_FRAME_EXISTS = True           
+        except ConfigParser.Error:
+            rospy.logerror("No floor frame!")
+            self.FLOOR_FRAME_EXISTS = False
         self.RESTART_DHCPCD = self.config.getboolean('PRIVATE', 'restart_dhcpcd')
 
     def on_broadcast_bind(self):
@@ -66,38 +115,31 @@ class CopterClient(client.Client):
         restart_service("chrony")
 
     def start(self, task_manager_instance):
-        client.logger.info("Init ROS node")
+        rospy.loginfo("Init ROS node")
         rospy.init_node('clever_show_client')
         if self.USE_LEDS:
             LedLib.init_led(self.LED_PIN)
         task_manager_instance.start()
         if self.FRAME_ID == "floor":
-            try:
-                self.FLOOR_DX = self.config.getfloat('FLOOR FRAME', 'x')
-                self.FLOOR_DY = self.config.getfloat('FLOOR FRAME', 'y')
-                self.FLOOR_DZ = self.config.getfloat('FLOOR FRAME', 'z')
-                self.FLOOR_ROLL = self.config.getfloat('FLOOR FRAME', 'roll')
-                self.FLOOR_PITCH = self.config.getfloat('FLOOR FRAME', 'pitch')
-                self.FLOOR_YAW = self.config.getfloat('FLOOR FRAME', 'yaw')
-                self.FLOOR_PARENT = self.config.get('FLOOR FRAME', 'parent')
-            except Exception as e:
-                raise Exception("Can't make floor frame!")
-                quit()
+            if self.FLOOR_FRAME_EXISTS: 
+                self.start_floor_frame_broadcast()
             else:
-                trans = TransformStamped()
-                trans.transform.translation.x = self.FLOOR_DX
-                trans.transform.translation.y = self.FLOOR_DY
-                trans.transform.translation.z = self.FLOOR_DZ
-                trans.transform.rotation = Quaternion(*quaternion_from_euler(math.radians(self.FLOOR_ROLL),
-                                                                            math.radians(self.FLOOR_PITCH),
-                                                                            math.radians(self.FLOOR_YAW)))
-                trans.header.frame_id = self.FLOOR_PARENT
-                trans.child_frame_id = self.FRAME_ID
-                static_bloadcaster.sendTransform(trans)
+                rospy.logerror("Can't make floor frame!")
         start_subscriber()
-        # print(check_state_topic())
+        telemetry_thread.start()
         super(CopterClient, self).start()
 
+    def start_floor_frame_broadcast(self):
+        trans = TransformStamped()
+        trans.transform.translation.x = self.FLOOR_DX
+        trans.transform.translation.y = self.FLOOR_DY
+        trans.transform.translation.z = self.FLOOR_DZ
+        trans.transform.rotation = Quaternion(*quaternion_from_euler(math.radians(self.FLOOR_ROLL),
+                                                                    math.radians(self.FLOOR_PITCH),
+                                                                    math.radians(self.FLOOR_YAW)))
+        trans.header.frame_id = self.FLOOR_PARENT
+        trans.child_frame_id = self.FRAME_ID
+        static_bloadcaster.sendTransform(trans)
 
 def restart_service(name):
     os.system("systemctl restart {}".format(name))
@@ -110,7 +152,7 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):
         with open(path, 'r') as f:
             raw_content = f.read()
     except IOError as e:
-        print("Reading error {}".format(e))
+        logger.error("Reading error {}".format(e))
         return False
 
     content = raw_content.split(" ")
@@ -118,11 +160,11 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):
     try:
         current_ip = content[ip_index]
     except IndexError:
-        print("Something wrong with config")
+        logger.error("Something wrong with config")
         return False
 
     if "." not in current_ip:
-        print("That's not ip!")
+        logger.debug("That's not ip!")
         return False
 
     if current_ip != ip:
@@ -132,7 +174,7 @@ def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):
             with open(path, 'w') as f:
                 f.write(" ".join(content))
         except IOError:
-            print("Error writing")
+            logger.error("Error writing")
             return False
 
     return True
@@ -144,7 +186,7 @@ def configure_hostname(hostname):
         with open(path, 'r') as f:
             raw_content = f.read()
     except IOError as e:
-        print("Reading error {}".format(e))
+        logger.error("Reading error {}".format(e))
         return False
 
     current_hostname = str(raw_content)
@@ -155,7 +197,7 @@ def configure_hostname(hostname):
             with open(path, 'w') as f:
                 f.write(content)
         except IOError:
-            print("Error writing")
+            logger.error("Error writing")
             return False
 
     return True
@@ -167,7 +209,7 @@ def configure_hosts(hostname):
         with open(path, 'r') as f:
             raw_content = f.read()
     except IOError as e:
-        print("Reading error {}".format(e))
+        logger.error("Reading error {}".format(e))
         return False
 
     index_start = raw_content.find("127.0.1.1", )
@@ -180,7 +222,7 @@ def configure_hosts(hostname):
             with open(path, 'w') as f:
                 f.write(content)
         except IOError:
-            print("Error writing")
+            logger.error("Error writing")
             return False
 
     return True
@@ -195,7 +237,7 @@ def configure_bashrc(hostname):
         with open(path, 'r') as f:
             raw_content = f.read()
     except IOError as e:
-        print("Reading error {}".format(e))
+        logger.error("Reading error {}".format(e))
         return False
 
     index_start = raw_content.find("ROS_HOSTNAME='", ) + 14
@@ -208,7 +250,7 @@ def configure_bashrc(hostname):
             with open(path, 'w') as f:
                 f.write(content)
         except IOError:
-            print("Error writing")
+            logger.error("Error writing")
             return False
 
     return True
@@ -253,13 +295,17 @@ def _response_selfcheck(*args, **kwargs):
         stop_subscriber()
         return "NOT_CONNECTED_TO_FCU"
 
+@messaging.request_callback("telemetry")
+def _response_telemetry(*args, **kwargs):
+    return create_telemetry_message(telemetry)
+
 
 @messaging.request_callback("anim_id")
 def _response_animation_id(*args, **kwargs):
     # Load animation
     result = animation.get_id()
     if result != 'No animation':
-        print ("Saving corrected animation")
+        logger.debug ("Saving corrected animation")
         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,
@@ -273,7 +319,7 @@ def _response_animation_id(*args, **kwargs):
                                             check_takeoff=client.active_client.TAKEOFF_CHECK,
                                             check_land=client.active_client.LAND_CHECK,
                                             )
-        print("Start action: {}".format(start_action))
+        logger.debug("Start action: {}".format(start_action))
         # Save corrected animation
         animation.save_corrected_animation(corrected_frames)
     return result
@@ -281,7 +327,7 @@ def _response_animation_id(*args, **kwargs):
 @messaging.request_callback("batt_voltage")
 def _response_batt(*args, **kwargs):
     if check_state_topic(wait_new_status=True):
-        return FlightLib.get_telemetry('body').voltage
+        return FlightLib.get_telemetry_locked('body').voltage
     else:
         stop_subscriber()
         return float('nan')
@@ -290,7 +336,7 @@ def _response_batt(*args, **kwargs):
 @messaging.request_callback("cell_voltage")
 def _response_cell(*args, **kwargs):
     if check_state_topic(wait_new_status=True):
-        return FlightLib.get_telemetry('body').cell_voltage
+        return FlightLib.get_telemetry_locked('body').cell_voltage
     else:
         stop_subscriber()
         return float('nan')
@@ -301,11 +347,15 @@ def _response_sys_status(*args, **kwargs):
 
 @messaging.request_callback("cal_status")
 def _response_cal_status(*args, **kwargs):
-    return get_calibration_status()
+    if check_state_topic(wait_new_status=True):
+        return get_calibration_status()
+    else:
+        stop_subscriber()
+        return "NOT_CONNECTED_TO_FCU"
 
 @messaging.request_callback("position")
 def _response_position(*args, **kwargs):
-    telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
+    telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
     return "{:.2f} {:.2f} {:.2f} {:.1f} {}".format(
         telem.x, telem.y, telem.z, math.degrees(telem.yaw), client.active_client.FRAME_ID)
 
@@ -323,32 +373,29 @@ def _calibrate_level(*args, **kwargs):
 @messaging.message_callback("test")
 def _command_test(*args, **kwargs):
     logger.info("logging info test")
+    rospy.logdebug("ros logdebug test")
     print("stdout test")
 
 @messaging.message_callback("move_start")
 def _command_move_start_to_current_position(*args, **kwargs):
-    # Load animation
-    frames = animation.load_animation(os.path.abspath("animation.csv"),
-                                        x0=client.active_client.X0 + client.active_client.X0_COMMON,
-                                        y0=client.active_client.Y0 + client.active_client.Y0_COMMON,
-                                        z0=client.active_client.Z0 + client.active_client.Z0_COMMON,
+    x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"),
                                         x_ratio=client.active_client.X_RATIO,
                                         y_ratio=client.active_client.Y_RATIO,
-                                        z_ratio=client.active_client.Z_RATIO,
                                         )
-    # Correct start and land frames in animation
-    corrected_frames, start_action, start_delay = animation.correct_animation(frames,
-                                        check_takeoff=client.active_client.TAKEOFF_CHECK,
-                                        check_land=client.active_client.LAND_CHECK,
-                                        )
-    x_start = corrected_frames[0]['x']
-    y_start = corrected_frames[0]['y']
-    telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
-    client.active_client.config.set('PRIVATE', 'x0', telem.x - x_start)
-    client.active_client.config.set('PRIVATE', 'y0', telem.y - y_start)
-    client.active_client.rewrite_config()
-    client.active_client.load_config()
-    print ("Start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
+    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.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', 'x0', telem.x - x_start)
+            client.active_client.config.set('PRIVATE', 'y0', telem.y - y_start)
+            client.active_client.rewrite_config()
+            client.active_client.load_config()
+            logger.info ("Set start delta: {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
+        else:
+            logger.debug ("Wrong telemetry")
+    else:
+        logger.debug("Wrong animation file")
 
 @messaging.message_callback("reset_start")
 def _command_reset_start(*args, **kwargs):
@@ -356,22 +403,22 @@ def _command_reset_start(*args, **kwargs):
     client.active_client.config.set('PRIVATE', 'y0', 0)
     client.active_client.rewrite_config()
     client.active_client.load_config()
-    print ("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
+    logger.info ("Reset start to {:.2f} {:.2f}".format(client.active_client.X0, client.active_client.Y0))
 
 @messaging.message_callback("set_z_to_ground")
 def _command_set_z(*args, **kwargs):
-    telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
+    telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
     client.active_client.config.set('PRIVATE', 'z0', telem.z)
     client.active_client.rewrite_config()
     client.active_client.load_config()
-    print ("Set z offset to {:.2f}".format(client.active_client.Z0))
+    logger.info ("Set z offset to {:.2f}".format(client.active_client.Z0))
 
 @messaging.message_callback("reset_z_offset")
 def _command_reset_z(*args, **kwargs):
     client.active_client.config.set('PRIVATE', 'z0', 0)
     client.active_client.rewrite_config()
     client.active_client.load_config()
-    print ("Reset z offset to {:.2f}".format(client.active_client.Z0))
+    logger.info ("Reset z offset to {:.2f}".format(client.active_client.Z0))
 
 
 @messaging.message_callback("update_repo")
@@ -396,7 +443,9 @@ def _command_reboot(*args, **kwargs):
 
 @messaging.message_callback("service_restart")
 def _command_service_restart(*args, **kwargs):
-    restart_service(kwargs["name"])
+    service = kwargs["name"]
+    restart_service(service)
+
 
 @messaging.message_callback("repair_chrony")
 def _command_chrony_repair(*args, **kwargs):
@@ -426,7 +475,7 @@ def _copter_flip(*args, **kwargs):
 
 @messaging.message_callback("takeoff")
 def _command_takeoff(*args, **kwargs):
-    print("Takeoff at {}".format(datetime.datetime.now()))
+    logger.info("Takeoff at {}".format(datetime.datetime.now()))
     task_manager.add_task(0, 0, animation.takeoff,
                           task_kwargs={
                               "z": client.active_client.TAKEOFF_HEIGHT,
@@ -440,8 +489,8 @@ def _command_takeoff(*args, **kwargs):
 def _command_takeoff_z(*args, **kwargs):
     z_str = kwargs.get("z", None)
     if z_str is not None:
-        telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
-        print("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now()))
+        telem = FlightLib.get_telemetry_locked(client.active_client.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,
@@ -497,12 +546,12 @@ def _play_animation(*args, **kwargs):
     start_time = float(kwargs["time"])
     # Check if animation file is available
     if animation.get_id() == 'No animation':
-        print("Can't start animation without animation file!")
+        logger.error("Can't start animation without animation file!")
         return
 
     task_manager.reset(interrupt_next_task=False)
     
-    print("Start time = {}, wait for {} seconds".format(start_time, start_time-time.time()))
+    logger.info("Start time = {}, wait for {} seconds".format(start_time, start_time-time.time()))
     # Load animation
     frames = animation.load_animation(os.path.abspath("animation.csv"),
                                         x0=client.active_client.X0 + client.active_client.X0_COMMON,
@@ -545,7 +594,6 @@ def _play_animation(*args, **kwargs):
         frame_time = rfp_time + client.active_client.RFP_TIME
 
     elif start_action == 'arm':
-        print ("Start_time")
         # Calculate start time
         start_time += start_delay
         # Arm
@@ -568,6 +616,7 @@ def _play_animation(*args, **kwargs):
                         )
         # Calculate first frame start time
         frame_time += client.active_client.FRAME_DELAY # TODO Think about arming time   
+    logger.debug(task_manager.task_queue)
     # Play animation file
     for frame in corrected_frames:
         point, color, yaw = animation.convert_frame(frame)
@@ -592,16 +641,90 @@ def _play_animation(*args, **kwargs):
                         "use_leds": client.active_client.USE_LEDS,
                     },
                     )
-    #print(task_manager.task_queue)
-        
 
+def telemetry_loop():
+    global telemetry
+    rate = rospy.Rate(client.active_client.TELEM_FREQ)
+    while not rospy.is_shutdown():
+        telemetry = telemetry._replace(animation_id = animation.get_id())
+        telemetry = telemetry._replace(git_version = subprocess.check_output("git log --pretty=format:'%h' -n 1", shell=True))
+        x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"),
+                                                    x_ratio=client.active_client.X_RATIO,
+                                                    y_ratio=client.active_client.Y_RATIO,
+                                                    )
+        x_delta = client.active_client.X0 + client.active_client.X0_COMMON
+        y_delta = client.active_client.Y0 + client.active_client.Y0_COMMON
+        z_delta = client.active_client.Z0 + client.active_client.Z0_COMMON
+        if not math.isnan(x_start):
+            telemetry = telemetry._replace(start_position = '{:.2f} {:.2f} {:.2f}'.format(x_start+x_delta, y_start+y_delta, z_delta))
+        else:
+            telemetry = telemetry._replace(start_position = 'NO_POS')
+        services_unavailable = FlightLib.check_ros_services_unavailable()
+        if not services_unavailable:
+            try:
+                ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
+                if ros_telemetry.connected:
+                    telemetry = telemetry._replace(battery_v = '{:.2f}'.format(ros_telemetry.voltage))
+                    batt_empty_param = get_param('BAT_V_EMPTY')
+                    batt_charged_param = get_param('BAT_V_CHARGED')
+                    batt_cells_param = get_param('BAT_N_CELLS')
+                    if batt_empty_param.success and batt_charged_param.success and batt_cells_param.success:
+                        batt_empty = batt_empty_param.value.real
+                        batt_charged = batt_charged_param.value.real
+                        batt_cells = batt_cells_param.value.integer
+                        try:
+                            telemetry = telemetry._replace(battery_p = '{}'.format(int(min((ros_telemetry.voltage/batt_cells - batt_empty)/(batt_charged - batt_empty)*100., 100))))
+                        except ValueError:
+                            telemetry = telemetry._replace(battery_p = 'nan')
+                    else:
+                        telemetry = telemetry._replace(battery_p = 'nan')
+                    telemetry = telemetry._replace(calibration_status = get_calibration_status())
+                    telemetry = telemetry._replace(system_status = get_sys_status())
+                    telemetry = telemetry._replace(mode = ros_telemetry.mode)
+                    check = FlightLib.selfcheck()
+                    if not check:
+                        check = "OK"
+                    telemetry = telemetry._replace(selfcheck = str(check))    
+                    if not math.isnan(ros_telemetry.x):
+                        telemetry = telemetry._replace(current_position = '{:.2f} {:.2f} {:.2f} {:.1f} {}'.format(ros_telemetry.x, ros_telemetry.y, ros_telemetry.z, 
+                                                                                        math.degrees(ros_telemetry.yaw), client.active_client.FRAME_ID))
+                    else: 
+                        telemetry = telemetry._replace(current_position = 'NO_POS in {}'.format(client.active_client.FRAME_ID))
+                else:
+                    telemetry = telemetry._replace(battery_v = 'nan')
+                    telemetry = telemetry._replace(battery_p = 'nan')
+                    telemetry = telemetry._replace(calibration_status = 'NO_FCU')
+                    telemetry = telemetry._replace(system_status = 'NO_FCU')
+                    telemetry = telemetry._replace(mode = 'NO_FCU')
+                    telemetry = telemetry._replace(selfcheck = 'NO_FCU')
+                    telemetry = telemetry._replace(current_position = 'NO_POS')
+            except rospy.ServiceException:
+                logger.debug("Some service is unavailable")
+            except AttributeError as e:
+                logger.debug(e)
+            except rospy.TransportException as e:
+                logger.debug(e)
+        else:
+            telemetry = telemetry._replace(selfcheck = 'WAIT_ROS')
+        if client.active_client.TELEM_TRANSMIT:
+            try:
+                client.active_client.server_connection.send_message('telem', args={'message':create_telemetry_message(telemetry)})
+            except AttributeError as e:
+                logger.debug(e)
+
+        rate.sleep()
+
+def create_telemetry_message(telemetry):
+    msg = client.active_client.client_id + '`'
+    for key in telemetry.__dict__:
+        msg += telemetry.__dict__[key] + '`'
+    msg += repr(time.time())
+    return msg 
+
+telemetry_thread = threading.Thread(target=telemetry_loop, name="Telemetry getting thread")
 
 if __name__ == "__main__":
+    
     copter_client = CopterClient()
     task_manager = tasking.TaskManager()
     copter_client.start(task_manager)
-
-    # ros_logging.route_logger_to_ros()
-    # ros_logging.route_logger_to_ros("__main__")
-    # ros_logging.route_logger_to_ros("client")
-    # ros_logging.route_logger_to_ros("messaging")

Drone/mavros_mavlink.py

@@ -78,20 +78,17 @@ def get_calibration_status():
     mag_status = get_param('CAL_MAG0_ID')
     acc_status = get_param('CAL_ACC0_ID')
     status_text = ""
-    if gyro_status.success == False:
-        status_text += "gyro: wrong_param; "
-    elif gyro_status.value.integer == 0:
+    if gyro_status.value.integer == 0 and gyro_status.success:
         status_text += "gyro: uncalibrated; "
-    if mag_status.success == False:
-        status_text += "mag: wrong_param; "
-    elif mag_status.value.integer == 0:
+    if mag_status.value.integer == 0 and mag_status.success:
         status_text += "mag: uncalibrated; "
-    if acc_status.success == False:
-        status_text += "acc: wrong_param; "
-    elif acc_status.value.integer == 0:
+    if acc_status.value.integer == 0 and acc_status.success:
         status_text += "acc: uncalibrated; "    
     if status_text == "":
-        status_text = "OK"
+        if not gyro_status.success or not mag_status.success or not acc_status.success:
+            status_text = "NO_INFO"
+        else:
+            status_text = "OK"
     else:
         status_text = status_text[:-2]
     return status_text

Drone/tasking_lib.py

@@ -87,7 +87,7 @@ class TaskManager(object):
 
     def start(self):
         #print("Task manager is started")
-        #logger.info("Task manager is started")
+        logger.info("Task manager is started")
         self._processor_thread.start()
         self.resume()
 
@@ -107,7 +107,7 @@ class TaskManager(object):
             self._wait_interrupt_event.set()
             self._task_interrupt_event.set()
         self._running_event.clear()
-        #logger.info("Task queue paused")
+        logger.info("Task queue paused")
         #print("Task queue paused")
 
     def resume(self, time_to_start_next_task=0.0):
@@ -118,7 +118,7 @@ class TaskManager(object):
         self._wait_interrupt_event.clear()
         self._task_interrupt_event.clear()
         self._running_event.set()
-        #logger.info("Task queue resumed with timeshift {}".format(self._timeshift))
+        logger.info("Task queue resumed with timeshift {}".format(self._timeshift))
         #print("Task queue resumed with timeshift {}".format(self._timeshift))
 
     def reset(self, interrupt_next_task=True):
@@ -128,11 +128,13 @@ class TaskManager(object):
             self._reset_event.set()
 
     def execute_task(self):
+        delta = 0.1
+
         with self._task_queue_lock:
             try:
                 start_time, priority, count, task = self.task_queue[0]
             except Exception as e:
-                #print("Task queue checking exception: {}".format(e))
+                logger.debug("Task queue checking exception: {}".format(e))
                 self._timeshift = 0.0
                 self._wait_interrupt_event.clear()
                 self._task_interrupt_event.clear()
@@ -140,24 +142,32 @@ class TaskManager(object):
                 return
 
         task_start_time = start_time + self._timeshift
-        #logger.info("Waiting util task execution time:{}".format(task_start_time))
+        logger.info("Executing task {}".format(task.func.__name__))
+        logger.debug("Waiting util task execution time:{}".format(task_start_time))
         #print("Waiting until task execution time:{}".format(task_start_time))
         wait(task_start_time, self._wait_interrupt_event)
 
+        if task_start_time - time.time() > 0.01:
+            logger.error("Need for waiting more")
+            self._wait_interrupt_event.clear()
+            return
+
         if not self._wait_interrupt_event.is_set():
             #logger.info("Executing task {}".format(task))
             #print("{} Executing task {}".format(time.time(),task))
             #print("Interrupter is set: {}".format(self._task_interrupt_event.is_set()))
             try:
                 task.func(*task.args, interrupter=self._task_interrupt_event, **task.kwargs)
+            
             except Exception as e:
                 logger.error("Error '{}' occurred in task {}".format(e, task))
                 #print("Error '{}' occurred in task {}".format(e, task))
                 if str(e) == 'STOP':
                     self.reset()
+                    logger.error("Return after STOP exception, can't arm!")
                     return
         else:
-            #logger.warning("Task interrupted before execution")
+            logger.error("Task interrupted before execution")
             #print("Task interrupted before execution")
             self._wait_interrupt_event.clear()
             return
@@ -166,14 +176,17 @@ class TaskManager(object):
             try:
                 start_time_n, priority_n, count_n, task_n = self.task_queue[0]
             except Exception as e:
-                #print("Timeout checking exception: {}".format(e))
+                logger.warning("Timeout checking exception: {}".format(e))
                 self._timeshift = 0.0
                 self._wait_interrupt_event.clear()
                 self._task_interrupt_event.clear()
                 self._running_event.clear()
                 return
             if (task_n == task) and (start_time_n == start_time):
-                self.pop_task()
+                try:
+                    self.pop_task()
+                except KeyError as e:
+                    logger.error(str(e))
                 #try:
                     #print("Pop {} function!".format(task.func.__name__))
                 #except Exception as e:
@@ -182,11 +195,11 @@ class TaskManager(object):
         if self._task_interrupt_event.is_set():
             self._task_interrupt_event.clear()     
 
-        #logger.info("Execution done")
+        logger.info("Execution done")
         #print("Execution done")
 
     def _task_processor(self):
-        #logger.info("Tasking thread started")
+        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()

README.md

@@ -1,4 +1,5 @@
 # clever-show
+
 [Русская версия](README_RU.md)
 
 Software for making the drone show controlled by Raspberry Pi and COEX [Clever](https://github.com/CopterExpress/clever) package. 
@@ -6,16 +7,16 @@ Software for making the drone show controlled by Raspberry Pi and COEX [Clever](
 [![Build Status](https://travis-ci.org/CopterExpress/clever-show.svg?branch=master)](https://travis-ci.org/CopterExpress/clever-show)
 
 ## This software includes
+
 * [Drone side](https://github.com/CopterExpress/clever-show/tree/master/Drone) with autonomous flight module, animation player module and client application for remote synchronized control of drones
 * [Server side](https://github.com/CopterExpress/clever-show/tree/master/Server) for making the drone show with ability of tuning drones, animation and music
 * [Blender 2.8 addon](https://github.com/CopterExpress/clever-show/tree/master/blender-addon) for animation export to drone paths
 * [Raspberry Pi image](https://github.com/CopterExpress/clever-show/releases/latest) for quick launch software on the drones
 
 ## Documentation
+
 > Documentation is available only in Russian for now.
 
 Start tutorial is located [here](docs/ru/start-tutorial.md).
 
 Detailed documentation is located in the [docs](https://github.com/CopterExpress/clever-show/tree/master/docs) folder.
-
-

Server/copter_table_models.py

@@ -1,5 +1,6 @@
 import sys
 import re
+import math
 import collections
 import indexed
 from server import ConfigOption
@@ -13,11 +14,10 @@ ModelStateRole = 999
 
 
 class CopterData:
-    class_basic_attrs = indexed.IndexedOrderedDict([('copter_id', None), ('anim_id', None),
-                                                    ('batt_v', None), ('batt_p', None),
-                                                    ('sys_status', None), ('cal_status', None), ('selfcheck', None),
-                                                    ('position', None), ("time_delta", None),
-                                                    ("client", None), ])
+    class_basic_attrs = indexed.IndexedOrderedDict([('copter_id', None), ('git_ver', None), ('anim_id', None),
+                                                    ('battery', None), ('sys_status', None), ('cal_status', None),
+                                                    ('mode', None), ('selfcheck', None), ('position', None), 
+                                                    ('start_pos', None), ('time_delta', None), ('client', None)])
 
     def __init__(self, **kwargs):
         self.attrs_dict = self.class_basic_attrs.copy()
@@ -63,8 +63,18 @@ class StatedCopterData(CopterData):
 
 class Checks:
     all_checks = {}
-    takeoff_checklist = (2, 3, 4, 5, 6)
+    takeoff_checklist = (3, 4, 6, 7, 8)
+    current_position = 'NO_POS'
+    start_position = 'NO_POS'
 
+    @classmethod
+    def get_pos_delta(self):
+        if self.current_position != 'NO_POS' and self.start_position != 'NO_POS':
+            delta_squared = 0
+            for i in range(3):
+                delta_squared += (self.current_position[i]-self.start_position[i])**2
+            return math.sqrt(delta_squared)
+        return 'NO_POS'
 
 class CopterDataModel(QtCore.QAbstractTableModel):
     selected_ready_signal = QtCore.pyqtSignal(bool)
@@ -75,8 +85,8 @@ class CopterDataModel(QtCore.QAbstractTableModel):
 
     def __init__(self, parent=None):
         super(CopterDataModel, self).__init__(parent)
-        self.headers = ('copter ID', '  animation ID  ', 'batt V', 'batt %', '  system  ',
-                        'calibration', 'selfcheck', 'current x y z yaw frame_id', 'time delta')
+        self.headers = ('copter ID', 'version', ' animation ID ', '  battery  ', '  system  ', 'sensors', 
+                        '  mode  ', 'checks', 'current x y z yaw frame_id', '    start x y z    ', 'dt')
         self.data_contents = []
 
         self.on_id_changed = None
@@ -86,7 +96,6 @@ class CopterDataModel(QtCore.QAbstractTableModel):
     def insertRows(self, contents, position='last', parent=QtCore.QModelIndex()):
         rows = len(contents)
         position = len(self.data_contents) if position == 'last' else position
-
         self.beginInsertRows(parent, position, position + rows - 1)
         self.data_contents[position:position] = contents
 
@@ -263,25 +272,25 @@ def col_check(col):
 
 
 @col_check(1)
+def check_ver(item):
+    if not item:
+        return None
+    return True
+
+@col_check(2)
 def check_anim(item):
     if not item:
         return None
     return str(item) != 'No animation'
 
-
-@col_check(2)
-def check_bat_v(item):
-    if not item:
-        return None
-    return float(item) > 3.2
-
-
 @col_check(3)
-def check_bat_p(item):
+def check_bat(item):
     if not item:
         return None
-    return float(item) > 30
-
+    if item == "NO_INFO":
+        return False
+    else:
+        return float(item.split(' ')[1][:-1]) > 50
 
 @col_check(4)
 def check_sys_status(item):
@@ -289,29 +298,57 @@ def check_sys_status(item):
         return None
     return item == "STANDBY"
 
-
 @col_check(5)
 def check_cal_status(item):
     if not item:
         return None
     return item == "OK"
 
-
 @col_check(6)
+def check_mode(item):
+    if not item:
+        return None
+    return (item != "NO_FCU") and not ("CMODE" in item)
+
+
+@col_check(7)
 def check_selfcheck(item):
     if not item:
         return None
     return item == "OK"
 
 
-@col_check(7)
-def check_cal_status(item):
+@col_check(8)
+def check_pos_status(item):
     if not item:
         return None
-    return True
+    str_pos = item.split(' ')
+    if str_pos[0] != 'nan' and str_pos[0] != 'NO_POS':
+        Checks.current_position = []
+        for i in range(3):
+            Checks.current_position.append(float(str_pos[i]))
+        return True
+    Checks.current_position = 'NO_POS'
+    return False
+
+@col_check(9)
+def check_start_pos_status(item):
+    if not item:
+        return None
+    str_start_pos = item.split(' ')
+    if str_start_pos[0] != 'nan' and str_start_pos[0] != 'NO_POS':
+        Checks.start_position = []
+        for i in range(3):
+            Checks.start_position.append(float(str_start_pos[i]))
+        delta = Checks.get_pos_delta()
+        if delta == 'NO_POS':
+            return False
+        else:
+            return delta < 1.    
+    return False
 
 
-@col_check(8)
+@col_check(10)
 def check_time_delta(item):
     if not item:
         return None
@@ -334,7 +371,7 @@ def takeoff_checks(copter_item):
 
 def flip_checks(copter_item):
     for col in Checks.takeoff_checklist:
-        if col != 4:
+        if col != 4 or col != 7:
             if not Checks.all_checks[col](copter_item[col]):
                 return False
         else:

Server/server_config.ini

@@ -1,7 +1,7 @@
 [SERVER]
 port = 25000
 buffer_size = 1024
-remove_disconnected = True
+remove_disconnected = False
 
 [BROADCAST]
 use_broadcast = True
@@ -11,4 +11,4 @@ broadcast_delay = 5
 [NTP]
 use_ntp = False
 host = ntp1.stratum2.ru
-port = 123
+port = 123

Server/server_gui.py

@@ -13,7 +13,7 @@ from PyQt5 import QtCore, QtGui, QtWidgets
 class Ui_MainWindow(object):
     def setupUi(self, MainWindow):
         MainWindow.setObjectName("MainWindow")
-        MainWindow.resize(1220, 750)
+        MainWindow.resize(1360, 761)
         self.centralwidget = QtWidgets.QWidget(MainWindow)
         self.centralwidget.setEnabled(True)
         self.centralwidget.setObjectName("centralwidget")
@@ -35,9 +35,10 @@ class Ui_MainWindow(object):
         self.tableView.setWordWrap(True)
         self.tableView.setObjectName("tableView")
         self.tableView.horizontalHeader().setCascadingSectionResizes(False)
-        self.tableView.horizontalHeader().setDefaultSectionSize(125)
+        self.tableView.horizontalHeader().setDefaultSectionSize(50)
         self.tableView.horizontalHeader().setMinimumSectionSize(50)
         self.tableView.horizontalHeader().setStretchLastSection(True)
+        self.tableView.verticalHeader().setVisible(False)
         self.horizontalLayout.addWidget(self.tableView)
         self.verticalLayout = QtWidgets.QVBoxLayout()
         self.verticalLayout.setSizeConstraint(QtWidgets.QLayout.SetMaximumSize)
@@ -186,7 +187,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, 1220, 25))
+        self.menubar.setGeometry(QtCore.QRect(0, 0, 1360, 25))
         self.menubar.setObjectName("menubar")
         self.menuOptions = QtWidgets.QMenu(self.menubar)
         self.menuOptions.setObjectName("menuOptions")
@@ -269,6 +270,7 @@ class Ui_MainWindow(object):
         self.menuDrone.addAction(self.action_remove_row)
         self.menuDrone.addSeparator()
         self.menuDrone.addAction(self.menuDeveloper_mode_2.menuAction())
+        self.menuDrone.addAction(self.action_remove_row)
         self.menuMusic.addAction(self.action_select_music_file)
         self.menuMusic.addAction(self.action_play_music)
         self.menuMusic.addAction(self.action_stop_music)
@@ -316,7 +318,7 @@ class Ui_MainWindow(object):
         self.action_send_Aruco_map.setText(_translate("MainWindow", "Send aruco map"))
         self.action_update_client_repo.setText(_translate("MainWindow", "Update clever-show git"))
         self.actionSend_launch_file_for_clever.setText(_translate("MainWindow", "Send launch file for clever"))
-        self.action_send_launch_file.setText(_translate("MainWindow", "Send launch file to clever"))
+        self.action_send_launch_file.setText(_translate("MainWindow", "Send launch files"))
         self.action_restart_clever.setText(_translate("MainWindow", "Restart clever service"))
         self.action_restart_clever_show.setText(_translate("MainWindow", "Restart clever-show service"))
         self.action_select_all_rows.setText(_translate("MainWindow", "Select all drones"))

Server/server_gui.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>1220</width>
-    <height>750</height>
+    <width>1360</width>
+    <height>761</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -50,7 +50,7 @@
          <bool>false</bool>
         </attribute>
         <attribute name="horizontalHeaderDefaultSectionSize">
-         <number>125</number>
+         <number>50</number>
         </attribute>
         <attribute name="horizontalHeaderMinimumSectionSize">
          <number>50</number>
@@ -58,6 +58,9 @@
         <attribute name="horizontalHeaderStretchLastSection">
          <bool>true</bool>
         </attribute>
+        <attribute name="verticalHeaderVisible">
+         <bool>false</bool>
+        </attribute>
        </widget>
       </item>
       <item>
@@ -375,7 +378,7 @@
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>1220</width>
+     <width>1360</width>
      <height>25</height>
     </rect>
    </property>
@@ -425,6 +428,7 @@
     <addaction name="action_remove_row"/>
     <addaction name="separator"/>
     <addaction name="menuDeveloper_mode_2"/>
+    <addaction name="action_remove_row"/>
    </widget>
    <widget class="QMenu" name="menuMusic">
     <property name="title">
@@ -466,7 +470,7 @@
   </action>
   <action name="action_send_launch_file">
    <property name="text">
-    <string>Send launch file to clever</string>
+    <string>Send launch files</string>
    </property>
   </action>
   <action name="action_restart_clever">

Server/server_qt.py

@@ -16,6 +16,7 @@ from quamash import QEventLoop, QThreadExecutor
 from server_gui import Ui_MainWindow
 
 from server import *
+import messaging_lib as messaging
 from copter_table_models import *
 from emergency import *
 
@@ -54,7 +55,6 @@ def confirmation_required(text="Are you sure?", label="Confirm operation?"):
 
     return inner
 
-
 # noinspection PyArgumentList,PyCallByClass
 class MainWindow(QtWidgets.QMainWindow):
     def __init__(self):
@@ -113,14 +113,17 @@ class MainWindow(QtWidgets.QMainWindow):
         self.signals.add_client_signal.emit(StatedCopterData(copter_id=client.copter_id, client=client))
 
     def client_connection_changed(self, client: Client):
+        logging.debug("Start remove {}".format(client.copter_id))
         row_data = self.model.get_row_by_attr("client", client)
         row_num = self.model.get_row_index(row_data)
+        logging.debug("Removing {}".format(client.copter_id))
         if row_num is not None:
             if Server().remove_disconnected and (not client.connected):
                 client.remove()
                 self.signals.remove_client_signal.emit(row_num)
             else:
                 self.signals.update_data_signal.emit(row_num, 0, client.connected, ModelStateRole)
+        logging.debug("{} removed".format(client.copter_id))
 
     def init_ui(self):
         # Connecting
@@ -170,59 +173,40 @@ class MainWindow(QtWidgets.QMainWindow):
     def selfcheck_selected(self):
         for copter_data_row in self.model.user_selected():
             client = copter_data_row.client
+            client.get_response("telemetry", self.update_table_data)
 
-            client.get_response("anim_id", self.set_copter_data, callback_args=(1, copter_data_row))
-            client.get_response("batt_voltage", self.set_copter_data, callback_args=(2, copter_data_row))
-            client.get_response("cell_voltage", self.set_copter_data, callback_args=(3, copter_data_row))
-            client.get_response("sys_status", self.set_copter_data, callback_args=(4, copter_data_row))
-            client.get_response("cal_status", self.set_copter_data, callback_args=(5, copter_data_row))
-            client.get_response("selfcheck", self.set_copter_data, callback_args=(6, copter_data_row))
-            client.get_response("position", self.set_copter_data, callback_args=(7, copter_data_row))
-            client.get_response("time", self.set_copter_data, callback_args=(8, copter_data_row))
-
-    def set_copter_data(self, value, col, copter_data_row):
-        row = self.model.get_row_index(copter_data_row)
-        if row is None:
-            logging.error("No such client!")
-            return
-
-        if col == 1:
-            data = value
-        elif col == 2:
-            data = "{}".format(round(float(value), 3))
-        elif col == 3:
-            batt_percent = ((float(value) - 3.2) / (4.2 - 3.2)) * 100  # TODO config
-            data = "{}".format(round(batt_percent, 3))
-        elif col == 4:
-            data = str(value)
-        elif col == 5:
-            data = str(value)
-        elif col == 6:
-            data = value
-        elif col == 7:
-            data = str(value)
-        elif col == 8:
-            data = "{}".format(round(float(value) - time.time(), 3))
-            if abs(float(data)) > 1:
-                copter_data_row.client.send_message("repair_chrony")
+    @pyqtSlot(str)
+    def update_table_data(self, message):
+        fields = message.split('`')
+        # copter_id git_version animation_id battery_v battery_p system_status calibration_status mode selfcheck current_position start_position copter_time
+        copter_id = fields[0]
+        git_version = fields[1]
+        animation_id = fields[2]
+        battery_v = fields[3]
+        battery_p = fields[4]
+        if battery_v == 'nan' or battery_p == 'nan':
+            battery_info = "NO_INFO"
         else:
-            logging.error("No column matched for response")
-            return
-
-        self.signals.update_data_signal.emit(row, col, data, ModelDataRole)
-
-
-    #def set_copter_id(self, value, copter_data_row):
-    #    col = 0
-    #    row = self.model.get_row_index(copter_data_row)
-    #    if row is None:
-    #        logging.error("No such client!")
-    #        return
-    #    logging.info("SET COPTER ID TO {}".format(value))
-    #
-    #    copter_data_row.client.copter_id = value
-    #    self.signals.update_data_signal.emit(row, col, value, ModelDataRole)
-    #    self.signals.update_data_signal.emit(row, col, True, ModelStateRole)
+            battery_info = "{}V {}%".format(battery_v, battery_p)
+        sys_status = fields[5]
+        cal_status = fields[6]
+        mode = fields[7]
+        selfcheck = fields[8]
+        current_pos = fields[9]
+        start_pos = fields[10]
+        copter_time = fields[11]
+        time_delta = "{}".format(round(float(copter_time) - time.time(), 3))
+        row = self.model.get_row_index(self.model.get_row_by_attr('copter_id', copter_id))
+        self.signals.update_data_signal.emit(row, 1, git_version, ModelDataRole)
+        self.signals.update_data_signal.emit(row, 2, animation_id, ModelDataRole)
+        self.signals.update_data_signal.emit(row, 3, battery_info, ModelDataRole)
+        self.signals.update_data_signal.emit(row, 4, sys_status, ModelDataRole)
+        self.signals.update_data_signal.emit(row, 5, cal_status, ModelDataRole)
+        self.signals.update_data_signal.emit(row, 6, mode, ModelDataRole)
+        self.signals.update_data_signal.emit(row, 7, selfcheck, ModelDataRole)
+        self.signals.update_data_signal.emit(row, 8, current_pos, ModelDataRole)
+        self.signals.update_data_signal.emit(row, 9, start_pos, ModelDataRole)
+        self.signals.update_data_signal.emit(row, 10, time_delta, ModelDataRole)
 
     @pyqtSlot(QtCore.QModelIndex)
     def selfcheck_info_dialog(self, index):
@@ -248,10 +232,16 @@ class MainWindow(QtWidgets.QMainWindow):
     @pyqtSlot()
     def remove_selected(self):
         for copter in self.model.user_selected():
-            row_num = self.model.data_contents.index(copter)
-            copter.client.remove()
-            self.signals.remove_client_signal.emit(row_num)
-            logging.info("Client removed from table!")
+            row_num = self.model.get_row_index(copter)
+            if row_num is not None:
+                copter.client.remove()
+
+                if not Server().remove_disconnected:
+                    self.signals.remove_client_signal.emit(row_num)
+                
+                logging.info("Client removed from table!")
+            else:
+                logging.error("Client is not in table!")
 
     @pyqtSlot()
     @confirmation_required("This operation will takeoff selected copters with delay and start animation. Proceed?")
@@ -263,7 +253,7 @@ class MainWindow(QtWidgets.QMainWindow):
             music_dt = self.ui.music_delay_spin.value()
             asyncio.ensure_future(self.play_music_at_time(music_dt+time_now), loop=loop)
             logging.info('Wait {} seconds to play music'.format(music_dt))
-        self.selfcheck_selected()
+        # self.selfcheck_selected()
         for copter in self.model.user_selected():
             if all_checks(copter):
                 server.send_starttime(copter.client, dt+time_now)
@@ -366,13 +356,12 @@ class MainWindow(QtWidgets.QMainWindow):
             print("Selected directory:", path)
             files = [file for file in glob.glob(path + '/*.csv')]
             names = [os.path.basename(file).split(".")[0] for file in files]
-            # print(files)
             for file, name in zip(files, names):
                 for copter in self.model.user_selected():
                     if name == copter.copter_id:
                         copter.client.send_file(file, "animation.csv")  # TODO config
                 else:
-                    print("Filename has no matches with any drone selected")
+                    logging.info("Filename has no matches with any drone selected")
 
     @pyqtSlot()
     def send_calibrations(self):
@@ -388,7 +377,7 @@ class MainWindow(QtWidgets.QMainWindow):
                     if name == copter.copter_id:
                         copter.client.send_file(file, "/home/pi/catkin_ws/src/clever/clever/camera_info/calibration.yaml")
                 else:
-                    print("Filename has no matches with any drone selected")
+                    logging.info("Filename has no matches with any drone selected")
 
     @pyqtSlot()
     def send_configurations(self):
@@ -401,7 +390,7 @@ class MainWindow(QtWidgets.QMainWindow):
             for section in sendable_config.sections():
                 for option in dict(sendable_config.items(section)):
                     value = sendable_config[section][option]
-                    logging.debug("Got item from config:".format(section, option, value))
+                    logging.debug("Got item from config: {} {} {}".format(section, option, value))
                     options.append(ConfigOption(section, option, value))
 
             for copter in self.model.user_selected():
@@ -419,12 +408,14 @@ class MainWindow(QtWidgets.QMainWindow):
 
     @pyqtSlot()
     def send_launch(self):
-        path = QFileDialog.getOpenFileName(self, "Select launch file for clever", filter="Launch files (*.launch)")[0]
+        path = str(QFileDialog.getExistingDirectory(self, "Select directory with launch files"))
         if path:
-            filename = os.path.basename(path)
-            print("Selected file:", path, filename)
+            print("Selected directory:", path)
+            files = [file for file in glob.glob(path + '/*.launch')]
             for copter in self.model.user_selected():
-                copter.client.send_file(path, "/home/pi/catkin_ws/src/clever/clever/launch/{}".format(filename))
+                for file in files:
+                    filename = os.path.basename(file)
+                    copter.client.send_file(file, "/home/pi/catkin_ws/src/clever/clever/launch/{}".format(filename))
                 # copter.client.send_message("service_restart", {"name": "clever"})
     
     @pyqtSlot()
@@ -563,6 +554,11 @@ class MainWindow(QtWidgets.QMainWindow):
                 self.model.data_contents[row_num].client \
                     .send_message("disarm")
 
+@messaging.message_callback("telem")
+def get_telem_data(*args, **kwargs):
+    message = kwargs.get("message", None)
+    window.update_table_data(message)
+
 
 if __name__ == "__main__":
     app = QtWidgets.QApplication(sys.argv)

builder/assets/clever-show.service

@@ -1,6 +1,8 @@
 [Unit]
 Description=Clever Show Client Service
-After=clever.service
+Requires=clever.service
+Requires=network.target
+After=network.target
 
 [Service]
 WorkingDirectory=/home/pi/clever-show/Drone

messaging_lib.py

@@ -5,6 +5,7 @@ import json
 import socket
 import struct
 import random
+import inspect
 import logging
 import threading
 import collections
@@ -20,6 +21,7 @@ except ImportError:
 
 PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value",  # "expires_on",
                                                            "callback", "callback_args", "callback_kwargs",
+                                                           "request_args", "resend",
                                                            ])
 logger = logging.getLogger(__name__)
 
@@ -33,7 +35,7 @@ def get_ip_address():
             ip_socket.connect(("8.8.8.8", 80))
             return ip_socket.getsockname()[0]
     except OSError:
-        logging.warning("No network connection detected, using localhost")
+        logger.warning("No network connection detected, using localhost")
         return "localhost"
 
 
@@ -193,7 +195,7 @@ class ConnectionManager(object):
     messages_callbacks = {}
     requests_callbacks = {}
 
-    def __init__(self):
+    def __init__(self, whoami = "computer"):
         self.selector = None
         self.socket = None
         self.addr = None
@@ -203,6 +205,8 @@ class ConnectionManager(object):
         self._recv_buffer = b""
         self._send_buffer = b""
 
+        self.whoami = whoami
+
         self._send_queue = collections.deque()
         self._received_queue = collections.deque()
         self._request_queue = collections.OrderedDict()
@@ -213,6 +217,7 @@ class ConnectionManager(object):
 
         self.BUFFER_SIZE = 1024
         self.resume_queue = False
+        self.resend_requests = True
 
     def _set_selector_events_mask(self, mode):
         """Set selector to listen for events: mode is 'r', 'w', 'rw'."""
@@ -226,7 +231,7 @@ class ConnectionManager(object):
             raise ValueError("Invalid events mask mode {}.".format(mode))
 
         key = self.selector.modify(self.socket, events, data=self)
-        logging.debug("Switched selector of {} to mode {}".format(self.addr, key.events))
+        logger.debug("Switched selector of {} to mode {}".format(self.addr, key.events))
         return key
 
     def connect(self, client_selector, client_socket, client_addr):
@@ -235,6 +240,8 @@ class ConnectionManager(object):
         self.addr = client_addr
 
         self._set_selector_events_mask('r')
+        if self.resend_requests:
+            self._resend_requests()
 
     def close(self):
         with self._close_lock:
@@ -245,6 +252,12 @@ class ConnectionManager(object):
 
     def _close(self):
         logger.info("Closing connection to {}".format(self.addr))
+
+        if not self.resume_queue:
+            self._recv_buffer = b''
+            self._send_buffer = b''
+            self._received_queue.clear()  #
+
         try:
             logger.info("Unregistering selector of {}".format(self.addr))
             self.selector.unregister(self.socket)
@@ -298,7 +311,7 @@ class ConnectionManager(object):
                 self._received_queue[0].income_raw = b''
 
             if self._received_queue:
-                if self._received_queue[-1].content:
+                if self._received_queue[0].content:
                     self.process_received(self._received_queue.popleft())
 
     def _read(self):
@@ -313,8 +326,6 @@ class ConnectionManager(object):
                 logger.debug("Received {} from {}".format(data, self.addr))
             else:
                 logger.warning("Connection to {} lost!".format(self.addr))
-                if not self.resume_queue:
-                    self._recv_buffer = b''
 
                 raise RuntimeError("Peer closed.")
 
@@ -357,21 +368,20 @@ class ConnectionManager(object):
 
     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():  # TODO as try []
-                if (key == request_id) and (value.requested_value == requested_value):
-                    request = self._request_queue.pop(key)
-                    value = message.content["value"]
-                    logger.debug(
-                        "Request {} successfully closed with value {}".format(request, message.content["value"])
-                    )
+            request = self._request_queue.pop(request_id, None)
 
-                    f = request.callback
-                    f(value, *request.callback_args, **request.callback_kwargs)
+        if (request is not None) and (request.requested_value == requested_value):
+            value = message.content["value"]
+            logger.debug(
+                "Request {} successfully closed with value {}".format(request, message.content["value"])
+            )
 
-                    break
-            else:
-                logger.warning("Unexpected  response!")
+            f = request.callback
+            f(value, *request.callback_args, **request.callback_kwargs)
+        else:
+            logger.warning("Unexpected  response!")
 
     def _process_filetransfer(self, message):  # TODO path?
         if message.jsonheader["content-type"] == "binary":
@@ -383,7 +393,9 @@ class ConnectionManager(object):
                 logger.error("File {} can not be written due error: {}".format(filepath, error))
             else:
                 logger.info("File {} successfully received ".format(filepath))
-                os.system("chown -R pi:pi /home/pi/clever-show")
+                if self.whoami == "pi":
+                    logger.info("Return rights to pi:pi after file transfer")
+                    os.system("chown pi:pi {}".format(filepath))
 
     def write(self):
         with self._send_lock:
@@ -418,7 +430,7 @@ class ConnectionManager(object):
             self._send_queue.append(data)
 
         if self.selector.get_key(self.socket).events != selectors.EVENT_WRITE:
-            self._set_selector_events_mask('w')
+            self._set_selector_events_mask('rw')
             NotifierSock().notify()
 
     def get_response(self, requested_value, callback, request_args=None,  # timeout=30,
@@ -437,9 +449,22 @@ class ConnectionManager(object):
                 callback=callback,
                 callback_args=callback_args,
                 callback_kwargs=callback_kwargs,
+                request_args=request_args,
+                resend=True,
             )
         self._send(MessageManager.create_request(requested_value, request_id, request_args))
 
+    def _resend_requests(self):
+        with self._request_lock:
+            for request_id, request in self._request_queue.items():
+                if request.resend:
+                    self._send(MessageManager.create_request(
+                        request.requested_value, request_id, request.request_args.update(resend=request.resend))
+                    )
+                    #request.resend = False
+
+            # self._request_queue.clear()
+
     def send_message(self, command, args=None):
         self._send(MessageManager.create_simple_message(command, args))
 
@@ -459,7 +484,7 @@ class ConnectionManager(object):
             ))
 
 
-class NotifierSock(Singleton):
+class NotifierSock(Singleton):  #TODO remake as connecting ONLY to self socket and selector
     def __init__(self):
         self.receive_socket = None
         self.addr = None

tools/cut.py

@@ -0,0 +1,100 @@
+import argparse
+import os
+import csv
+import glob
+import copy
+import logging
+
+def cut_file(filename, _from, _to):
+    imported_frames = []
+    anim_id = ""
+
+    try:
+        animation_file = open(filename)
+    except IOError:
+        logging.error("File {} can't be opened".format(filepath))
+    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]
+                logging.debug("Got animation_id: {}".format(anim_id))
+            else:
+                logging.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': float(x),
+                    'y': float(y),
+                    'z': float(z),
+                    'yaw': float(yaw),
+                    'red': int(red),
+                    'green': int(green),
+                    'blue': int(blue),
+                })
+            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),
+                    'y': float(y),
+                    'z': float(z),
+                    'yaw': float(yaw),
+                    'red': int(red),
+                    'green': int(green),
+                    'blue': int(blue),
+                })
+
+        if _to == 0 or _to >= len(imported_frames):
+            _to = len(imported_frames)-1
+
+        path = '{}/cut_{}_{}'.format(os.path.dirname(filename),_from,_to)
+        print('Path is {}'.format(path))
+        
+        csv_file = open(path+'/'+os.path.basename(filename), mode='w+')
+        with csv_file:
+            csv_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
+            if anim_id != "":
+                csv_writer.writerow([anim_id])
+            for i in range(_from, _to+1):
+                csv_writer.writerow([imported_frames[i]['number'], imported_frames[i]['x'], imported_frames[i]['y'], imported_frames[i]['z'],
+                                    imported_frames[i]['yaw'], imported_frames[i]['red'], imported_frames[i]['green'], imported_frames[i]['blue']])
+
+        print("Successfully created file {}".format(path+'/'+filename))
+
+if __name__ == "__main__":
+    
+    parser = argparse.ArgumentParser(description="cut animation")
+    parser.add_argument('directory', nargs='?', default='.',
+                        help="Directory with animation csv files. Default is '.'")
+    parser.add_argument('-f','--frm', type=int, default=0,
+                        help="Cut from this frame, default is 0 (from the beginning)")
+    parser.add_argument('-t','--to', type=int, default=0,
+                        help="Cut to this frame (including this one), default is 0 (to the end)")  
+    args = parser.parse_args()
+
+    _from = args.frm
+    _to = args.to
+
+    path = '{}/cut_{}_{}'.format(args.directory,_from,_to)
+
+    try:
+        os.mkdir(path)
+    except OSError:
+        print("Creation of the directory %s failed" % path)
+        files = [f for f in glob.glob(args.directory + '/*.csv')]
+        for f in files:
+            cut_file(f, _from, _to)
+    else:
+        print("Successfully created the directory %s " % path)
+
+        files = [f for f in glob.glob(args.directory + '/*.csv')]
+        for f in files:
+            cut_file(f, _from, _to)
+
+
+
+