clever-show/drone/client.py

import os
import sys
import time
import math
import numpy
import psutil
import logging
import datetime
import threading
import subprocess
from collections import namedtuple
from watchdog.observers import Observer
from watchdog.events import FileSystemEventHandler

# Import rospy
try:
    import rospy
except ImportError:
    print("Can't import rospy! Please check your ROS installation.")
    exit()

import rospkg

# Import clever or clover package
try:
    from clever import srv
except ImportError:
    try:
        from clover import srv
    except ImportError:
        print("Can't import clever or clover! Please check installation of clover ROS package.")
        exit()

# Import flight control
try:
    import modules.flight as flight
except ImportError:
    print("Can't import flight control module!")

# Import led control
try:
    import modules.led as led
except ImportError:
    print("Can't import led control module!")

# Add parent dir to PATH to import messaging_lib and config_lib
current_dir = (os.path.dirname(os.path.realpath(__file__)))
lib_dir = os.path.realpath(os.path.join(current_dir, '../lib'))
sys.path.insert(0, lib_dir)

import messaging
import modules.client_core as client_core
import modules.animation as animation
import modules.mavros_wrapper as mavros
import modules.tasking as tasking

from std_msgs.msg import Bool
from geometry_msgs.msg import Point, Quaternion, TransformStamped
from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_multiply
import tf2_ros
from geographiclib.geodesic import Geodesic

Earth = Geodesic.WGS84

def dist(x, y):
    return math.sqrt(x**2+y**2)

def azi(x, y):
    return 90 - math.atan2(y,x)*180/math.pi

def get_xy(dist, azi):
    return dist*math.sin(math.radians(azi)), dist*math.cos(math.radians(azi))

def valid(pos):
    for coord in pos:
        if math.isnan(coord):
            return False
    return True

static_broadcaster = tf2_ros.StaticTransformBroadcaster()

emergency = False

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('modules.animation')
animation_logger.setLevel(logging.INFO)
animation_logger.addHandler(handler)

client_logger = logging.getLogger('modules.client_core')
client_logger.setLevel(logging.DEBUG)
client_logger.addHandler(handler)

messaging_logger = logging.getLogger('messaging')
messaging_logger.setLevel(logging.INFO)
messaging_logger.addHandler(handler)

tasking_logger = logging.getLogger('modules.tasking')
tasking_logger.setLevel(logging.INFO)
tasking_logger.addHandler(handler)

flight_logger = logging.getLogger('modules.flight')
flight_logger.setLevel(logging.INFO)
flight_logger.addHandler(handler)

mavros_mavlink_logger = logging.getLogger('modules.mavros_wrapper')
mavros_mavlink_logger.setLevel(logging.INFO)
mavros_mavlink_logger.addHandler(handler)


class CopterClient(client_core.Client):
    def __init__(self, config_path="config/client.ini"):
        super(CopterClient, self).__init__(config_path)
        self.load_config()
        if self.config.clover_dir == 'auto':
            self.check_clover_dir()
        self.telemetry = None
        self.animation = animation.Animation(self.config, "animation.csv")

    def load_config(self):
        super(CopterClient, self).load_config()

    def check_clover_dir(self):
        rospack = rospkg.RosPack()
        try:
            path = rospack.get_path('clever')
        except rospkg.common.ResourceNotFound:
            try:
                path = rospack.get_path('clover')
            except rospkg.common.ResourceNotFound:
                path = 'error'
        self.config.set('', 'clover_dir', path, write=True)
        if path.count("/pi/"):
            self.server_connection.whoami = "pi"

    def on_broadcast_bind(self):
        repair_chrony(self.config.server_host)

    def start(self, task_manager_instance):
        rospy.loginfo("Init ROS node")
        rospy.init_node('clever_show_client', anonymous=True)
        task_manager_instance.start()
        mavros.start_subscriber()
        self.telemetry = Telemetry()
        self.telemetry.start_loop()
        if self.config.flight_frame_id == "floor":
            self.start_floor_frame_broadcast()
        elif self.config.flight_frame_id == "gps":
            self.start_gps_frame_broadcast()
        client_thread = threading.Thread(target=super(CopterClient, self).start, name="Client thread")
        client_thread.daemon = True
        client_thread.start()

    def start_floor_frame_broadcast(self):
        if self.config.floor_frame_parent == "gps":
            self.start_gps_frame_broadcast()
        trans = TransformStamped()
        trans.transform.translation.x = self.config.floor_frame_translation[0]
        trans.transform.translation.y = self.config.floor_frame_translation[1]
        trans.transform.translation.z = self.config.floor_frame_translation[2]
        trans.transform.rotation = Quaternion(*quaternion_from_euler(math.radians(self.config.floor_frame_rotation[0]),
                                                                     math.radians(self.config.floor_frame_rotation[1]),
                                                                     math.radians(self.config.floor_frame_rotation[2])))
        trans.header.frame_id = self.config.floor_frame_parent
        trans.child_frame_id = self.config.flight_frame_id
        static_broadcaster.sendTransform(trans)

    def start_gps_frame_broadcast(self):
        trans = TransformStamped()
        trans.transform.translation.x = 0
        trans.transform.translation.y = 0
        trans.transform.translation.z = 0
        trans.transform.rotation = Quaternion(*quaternion_from_euler(0,0,0))
        trans.header.frame_id = "map"
        trans.child_frame_id = "gps"
        static_broadcaster.sendTransform(trans)
        gps_frame_thread = threading.Thread(target=self.gps_frame_broadcast_loop, name="GPS frame broadcast thread")
        gps_frame_thread.daemon = True
        gps_frame_thread.start()

    def gps_frame_broadcast_loop(self):
        rate = rospy.Rate(1)
        while not rospy.is_shutdown():
            telem = flight.get_telemetry_locked(frame_id = "map")
            if telem is not None:
                if math.isnan(telem.lat) or math.isnan(telem.lon) or math.isnan(telem.x) or math.isnan(telem.y):
                    logger.info("Can't get position from telemetry")
                else:
                    lat = float(self.config.gps_frame_lat)
                    lon = float(self.config.gps_frame_lon)
                    geo_delta = Earth.Inverse(telem.lat, telem.lon, lat, lon)
                    #logger.info("dist: {} | azi: {}".format(geo_delta['s12'], geo_delta['azi1']))
                    dx, dy = get_xy(geo_delta['s12'], geo_delta['azi1'])
                    gps_dx = telem.x + dx
                    gps_dy = telem.y + dy
                    #logger.info("GPS frame dx: {} | dy: {}".format(gps_dx, gps_dy))
                    trans = TransformStamped()
                    trans.transform.translation.x = gps_dx
                    trans.transform.translation.y = gps_dy
                    trans.transform.translation.z = 0
                    trans.transform.rotation = Quaternion(*quaternion_from_euler(0,0,
                                                            math.radians(self.config.gps_frame_yaw)))
                    trans.header.frame_id = "map"
                    trans.child_frame_id = "gps"
                    static_broadcaster.sendTransform(trans)

            rate.sleep()


def restart_service(name):
    os.system("systemctl restart {}".format(name))

def repair_chrony(ip):
    logger.info("Configure chrony ip to {}".format(ip))
    configure_chrony_ip(ip)
    restart_service("chrony")

def execute_command(command):
    os.system(command)


def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):  # TODO simplify
    try:
        with open(path, 'r') as f:
            raw_content = f.read()
    except IOError as e:
        logger.error("Reading error {}".format(e))
        return False

    content = raw_content.split(" ")

    try:
        current_ip = content[ip_index]
    except IndexError:
        logger.error("Something wrong with config")
        return False

    if "." not in current_ip:
        logger.debug("That's not ip!")

    if current_ip != ip:
        content[ip_index] = ip

        try:
            with open(path, 'w') as f:
                f.write(" ".join(content))
        except IOError:
            logger.error("Error writing")
            return False

    return True


def configure_hostname(hostname):
    path = "/etc/hostname"
    try:
        with open(path, 'r') as f:
            raw_content = f.read()
    except IOError as e:
        logger.error("Reading error {}".format(e))
        return False

    current_hostname = str(raw_content)

    if current_hostname != hostname:
        content = hostname + '\n'
        try:
            with open(path, 'w') as f:
                f.write(content)
        except IOError:
            logger.error("Error writing")
            return False

    return True


def configure_hosts(hostname):
    path = "/etc/hosts"
    try:
        with open(path, 'r') as f:
            raw_content = f.read()
    except IOError as e:
        logger.error("Reading error {}".format(e))
        return False

    index_start = raw_content.find("127.0.1.1", )
    index_stop = raw_content.find("\n", index_start)

    hosts_array = raw_content[index_start:index_stop].split()
    _ip = hosts_array[0]
    current_hostname = hosts_array[1]
    if current_hostname != hostname:
        content = raw_content[:index_start] + "{}       {} {}.local".format(_ip, hostname, hostname) + raw_content[
                                                                                                       index_stop:]
        try:
            with open(path, 'w') as f:
                f.write(content)
        except IOError:
            logger.error("Error writing")
            return False

    return True


def configure_motd(hostname):
    with open("/etc/motd", "w") as f:
        f.write("\r\n{}\r\n\r\n".format(hostname))


def configure_bashrc(hostname):
    path = "/home/pi/.bashrc"
    try:
        with open(path, 'r') as f:
            raw_content = f.read()
    except IOError as e:
        logger.error("Reading error {}".format(e))
        return False

    index_start = raw_content.find("ROS_HOSTNAME='", ) + 14
    index_stop = raw_content.find("'", index_start)

    current_hostname = raw_content[index_start:index_stop]
    if current_hostname != hostname:
        content = raw_content[:index_start] + hostname + raw_content[index_stop:]
        try:
            with open(path, 'w') as f:
                f.write(content)
        except IOError:
            logger.error("Error writing")
            return False

    return True


@messaging.message_callback("execute")
def _execute(*args, **kwargs):
    command = kwargs.get("command", None)
    if command is not None:
        logger.info("Executing command: {}".format(command))
        execute_command(command)
        logger.info("Executing done")


@messaging.message_callback("id")  # TODO redo
def _response_id(*args, **kwargs):
    new_id = kwargs.get("new_id", None)
    if new_id is not None:
        old_id = copter.client_id
        if new_id != old_id:
            copter.config.set('', 'id', new_id, write=True)
            copter.client_id = new_id
            if new_id != '/hostname':
                if copter.config.system_restart_after_rename:
                    hostname = copter.client_id
                    configure_hostname(hostname)
                    configure_hosts(hostname)
                    configure_bashrc(hostname)
                    configure_motd(hostname)
                    execute_command("systemctl stop clever-show & reboot")
                    # execute_command("hostname {}".format(hostname))
                    # restart_service("dhcpcd")
                    # restart_service("avahi-daemon")
                    # restart_service("smbd")
                    # restart_service("roscore")
                    # restart_service("clever")
                restart_service("clever-show")


@messaging.request_callback("selfcheck")
def _response_selfcheck(*args, **kwargs):
    if mavros.check_state_topic(wait_new_status=True):
        check = flight.selfcheck()
        return check if check else "OK"
    else:
        mavros.stop_subscriber()
        return "NOT_CONNECTED_TO_FCU"


@messaging.request_callback("telemetry")
def _response_telemetry(*args, **kwargs):
    copter.telemetry.update()
    return copter.telemetry.create_msg_contents()


@messaging.request_callback("anim_id")
def _response_animation_id(*args, **kwargs):
    # Load animation
    return copter.animation.id


@messaging.request_callback("batt_voltage")
def _response_batt(*args, **kwargs):
    if mavros.check_state_topic(wait_new_status=True):
        return flight.get_telemetry_locked('body').voltage
    else:
        mavros.stop_subscriber()
        return float('nan')


@messaging.request_callback("cell_voltage")
def _response_cell(*args, **kwargs):
    if mavros.check_state_topic(wait_new_status=True):
        return flight.get_telemetry_locked('body').cell_voltage
    else:
        mavros.stop_subscriber()
        return float('nan')


@messaging.request_callback("sys_status")
def _response_sys_status(*args, **kwargs):
    return mavros.get_sys_status()


@messaging.request_callback("cal_status")
def _response_cal_status(*args, **kwargs):
    if mavros.check_state_topic(wait_new_status=True):
        return mavros.get_calibration_status()
    else:
        mavros.stop_subscriber()
        return "NOT_CONNECTED_TO_FCU"


@messaging.request_callback("position")
def _response_position(*args, **kwargs):
    telem = copter.telemetry.ros_telemetry
    return "{:.2f} {:.2f} {:.2f} {:.1f} {}".format(
        telem.x, telem.y, telem.z, math.degrees(telem.yaw), copter.config.flight_frame_id)


@messaging.request_callback("calibrate_gyro")
def _calibrate_gyro(*args, **kwargs):
    mavros.calibrate('gyro')
    return mavros.get_calibration_status()


@messaging.request_callback("calibrate_level")
def _calibrate_level(*args, **kwargs):
    mavros.calibrate('level')
    return mavros.get_calibration_status()


@messaging.request_callback("load_params")
def _load_params(*args, **kwargs):
    result = mavros.load_param_file('temp.params')
    logger.info("Load parameters to FCU success: {}".format(result))
    return result


@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("update_animation")
def _command_update_animation(*args, **kwargs):
    copter.animation.update_frames(copter.config, "animation.csv")


@messaging.message_callback("move_start")
def _command_move_start_to_current_position(*args, **kwargs):
    offset = numpy.array(copter.config.animation_private_offset) + numpy.array(copter.config.animation_common_offset)
    try:
        xs, ys, zs = copter.animation.get_start_point(copter.config.ratio, offset)
    except ValueError:
        logger.error("Can't get start point. Check animation file!")
    else:
        logger.debug("start x = {}, y = {}".format(xs, ys))
        telem = copter.telemetry.ros_telemetry
        logger.debug("telemetry x = {}, y = {}".format(telem.x, telem.y))
        if valid([telem.x, telem.y, telem.z]):
            copter.config.set('animation', 'private_offset',
                [telem.x - xs, telem.y - ys, copter.config.animation_private_offset[2]], write=True)
            logger.info("Set start delta: {:.2f} {:.2f}".format(copter.config.animation_private_offset[0],
                                                                copter.config.animation_private_offset[1]))
        else:
            logger.error("Wrong telemetry")


@messaging.message_callback("reset_start")
def _command_reset_start(*args, **kwargs):
    copter.config.set('animation', 'private_offset', [0, 0, copter.config.animation_private_offset[2]], write=True)
    logger.info("Reset start to {:.2f} {:.2f}".format(copter.config.animation_private_offset[0], copter.config.animation_private_offset[1]))


@messaging.message_callback("set_z_to_ground")
def _command_set_z(*args, **kwargs):
    telem = copter.telemetry.ros_telemetry
    if valid([telem.x, telem.y, telem.z]):
        copter.config.set('animation', 'private_offset',
            [copter.config.animation_private_offset[0], copter.config.animation_private_offset[1], telem.z], write=True)
        logger.info("Set z offset to {:.2f}".format(copter.config.animation_private_offset[2]))
    else:
        logger.error("Wrong telemetry")


@messaging.message_callback("reset_z_offset")
def _command_reset_z(*args, **kwargs):
    copter.config.set('animation', 'private_offset',
        [copter.config.animation_private_offset[0], copter.config.animation_private_offset[1], 0], write=True)
    logger.info("Reset z offset to {:.2f}".format(copter.config.animation_private_offset[2]))


@messaging.message_callback("update_repo")
def _command_update_repo(*args, **kwargs):
    os.system("git fetch")
    os.system("git pull --rebase")
    os.system("chown -R pi:pi /home/pi/clever-show")


@messaging.message_callback("reboot_all")
def _command_reboot_all(*args, **kwargs):
    mavros.reboot_fcu()
    execute_command("reboot")


@messaging.message_callback("reboot_fcu")
def _command_reboot(*args, **kwargs):
    mavros.reboot_fcu()


@messaging.message_callback("service_restart")
def _command_service_restart(*args, **kwargs):
    service = kwargs["name"]
    if service=="clover":
        restart_service("clever")
    if service=="clever-show":
        restart_service("clever-show@{}".format(copter.client_id))
    restart_service(service)


@messaging.message_callback("repair_chrony")
def _command_chrony_repair(*args, **kwargs):
    repair_chrony(copter.config.server_host)


@messaging.message_callback("led_test")
def _command_led_test(*args, **kwargs):
    led.set_effect(effect='flash', r=255, g=255, b=255)


@messaging.message_callback("led_fill")
def _command_led_fill(*args, **kwargs):
    red = kwargs.get("red", 0)
    green = kwargs.get("green", 0)
    blue = kwargs.get("blue", 0)
    led.set_effect(r=red, g=green, b=blue)


@messaging.message_callback("flip")
def _copter_flip(*args, **kwargs):
    flight.flip(frame_id=copter.config.flight_frame_id)


@messaging.message_callback("takeoff")
def _command_takeoff(*args, **kwargs):
    logger.info("Takeoff at {}".format(datetime.datetime.now()))
    task_manager.add_task(0, 0, animation.takeoff,
                          task_kwargs={
                              "z": copter.config.flight_takeoff_height,
                              "timeout": copter.config.flight_takeoff_time,
                              "safe_takeoff": False,
                              "use_leds": copter.config.led_use & copter.config.led_takeoff_indication,
                          })


@messaging.message_callback("takeoff_z")
def _command_takeoff_z(*args, **kwargs):
    try:
        z = float(kwargs.get("z", None))
    except TypeError:
        logger.error("takeoff_z: No z argument!")
    except ValueError:
        logger.error("takeoff_z: Wrong z argument!")
    else:
        telem = flight.get_telemetry_locked(copter.config.flight_frame_id)
        if valid([telem.x, telem.y, telem.z]):
            logger.info("Takeoff to z = {} at {}".format(z, datetime.datetime.now()))
            task_manager.add_task(0, 0, flight.reach_point,
                                task_kwargs={
                                    "x": telem.x,
                                    "y": telem.y,
                                    "z": z,
                                    "frame_id": copter.config.flight_frame_id,
                                    "timeout": copter.config.flight_takeoff_time,
                                    "auto_arm": True,
                                })
        else:
            logger.error("Wrong telemetry!")


@messaging.message_callback("land")
def _command_land(*args, **kwargs):
    task_manager.reset()
    task_manager.add_task(0, 0, animation.land,
                          task_kwargs={
                              "z": copter.config.flight_takeoff_height,
                              "timeout": copter.config.flight_land_timeout,
                              "frame_id": copter.config.flight_frame_id,
                              "use_leds": copter.config.led_use & copter.config.led_land_indication,
                          })


@messaging.message_callback("emergency_land")
def _emergency_land(*args, **kwargs):
    try:
        result = flight.emergency_land()
        logger.info(result.message)
    except rospy.ServiceException:
        logger.error("Can't execute emergency land: service is unavailable!")


@messaging.message_callback("disarm")
def _command_disarm(*args, **kwargs):
    task_manager.reset()
    task_manager.add_task(-5, 0, flight.arming_wrapper,
                          task_kwargs={
                              "state": False
                          })


@messaging.message_callback("stop")
def _command_stop(*args, **kwargs):
    task_manager.reset()


@messaging.message_callback("pause")
def _command_pause(*args, **kwargs):
    task_manager.pause()


@messaging.message_callback("resume")
def _command_resume(*args, **kwargs):
    task_manager.resume(time_to_start_next_task=kwargs.get("time", 0))


@messaging.message_callback("start")
def _play_animation(*args, **kwargs):

    # Validate start_time
    try:
        start_time = float(kwargs["time"])
    except ValueError:
        logger.error("start: Wrong time argument!")
        return
    except KeyError:
        logger.error("start: No time argument!")
        return

    # Check animation state
    if copter.animation.state is not "OK":
        logger.error("start: Bad animation state")
        return

    # Get output frames
    offset = numpy.array(copter.config.animation_private_offset) + numpy.array(copter.config.animation_common_offset)
    frames = copter.animation.get_scaled_output(copter.config.animation_ratio, offset)
    if not frames:
        logger.error("start: No frames in animation!")
        return

    # Get current telemetry
    telem = copter.telemetry.ros_telemetry
    if not valid([telem.x, telem.y, telem.z]):
        logger.error("start: Position is not valid!")
        return

    # Get start action and delay
    try:
        start_action, start_delay = copter.telemetry.start_position[-2:]
    except ValueError:
        logger.error("start: Can't get animation start position and delay")
        return
    # Reset task manager
    task_manager.reset(interrupt_next_task=False)

    # Set animation logic
    if start_action == 'takeoff':
        # Takeoff first at start_time + start_delay_time
        takeoff_time = start_time + start_delay
        task_manager.add_task(takeoff_time, 0, animation.takeoff,
                              task_kwargs={
                                  "z": copter.config.flight_takeoff_height,
                                  "timeout": copter.config.flight_takeoff_time,
                                  "safe_takeoff": False,
                                  "use_leds": copter.config.led_use & copter.config.led_takeoff_indication,
                              })
        # Fly to first point
        rfp_time = takeoff_time + copter.config.flight_takeoff_time
        task_manager.add_task(rfp_time, 0, animation.execute_frame,
                              task_kwargs={
                                  "frame": frames[0],
                                  "frame_id": copter.config.flight_frame_id,
                                  "use_leds": copter.config.led_use,
                                  "flight_func": flight.reach_point,
                              })
        # Calculate first frame start time
        frame_time = rfp_time + copter.config.flight_reach_first_point_time

    elif start_action == 'fly':
        # Calculate start time
        arm_time = start_time + start_delay  # + 1.0
        task_manager.add_task(arm_time, 0, animation.execute_frame,
                              task_kwargs={
                                  "frame": frames[0],
                                  "frame_id": copter.config.flight_frame_id,
                                  "use_leds": copter.config.led_use,
                                  "flight_func": flight.navto,
                                  "auto_arm": True,
                              })
        # Calculate first frame start time
        frame_time = arm_time + copter.config.flight_arming_time
    logger.debug("Start queue {}".format(task_manager.task_queue))
    # Play animation file
    for frame in frames:
        task_manager.add_task(frame_time, 0, animation.execute_frame,
                              task_kwargs={
                                  "frame": frame,
                                  "frame_id": copter.config.flight_frame_id,
                                  "use_leds": copter.config.led_use,
                                  "flight_func": flight.navto,
                              })
        frame_time += frame.delay
    # Calculate land_time
    land_time = frame_time + copter.config.flight_land_delay
    # Land
    task_manager.add_task(land_time, 0, animation.land,
                          task_kwargs={
                              "timeout": copter.config.flight_land_timeout,
                              "frame_id": copter.config.flight_frame_id,
                              "use_leds": copter.config.led_use & copter.config.led_land_indication,
                          })


# noinspection PyAttributeOutsideInit
class Telemetry:
    params_default_dict = {
        "git_version": None,
        "animation_info": None,
        "battery": None,
        "armed": False,
        "fcu_status": None,
        "calibration_status": None,
        "mode": None,
        "selfcheck": None,
        "current_position": None,
        "start_position": None,
        "last_task": None,
        "time_delta": None,
        "config_version": None,
    }

    def __init__(self):
        self._lock = threading.Lock()
        self._last_state = []
        self._interruption_counter = 0
        self._max_interruptions = 2
        self._tasks_cleared = False
        self.ros_telemetry = None

        for key, value in self.params_default_dict.items():
            setattr(self, key, value)

    def __setattr__(self, key, value):
        if key in self.params_default_dict:
            with self.__dict__['_lock']:
                self.__dict__[key] = value
        else:
            self.__dict__[key] = value

    def __getattr__(self, item):
        if item in self.params_default_dict:
            with self.__dict__['_lock']:
                return self.__dict__[item]

        return self.__dict__[item]

    @classmethod
    def get_git_version(cls):
        return subprocess.check_output("git log --pretty=format:'%h' -n 1", shell=True)

    @classmethod
    def get_config_version(cls):
        return "{} V{}".format(copter.config.config_name, copter.config.config_version)

    def get_start_position(self):
        offset = numpy.array(copter.config.animation_private_offset) + numpy.array(copter.config.animation_common_offset)
        try:
            x, y, z = copter.animation.get_start_point(copter.config.animation_ratio, offset)
        except ValueError:
            return [float('nan'),float('nan'),float('nan'),float('nan'),'error: no start pos in animation',float('nan')]
        else:
            start_delay = copter.animation.start_delay
            yaw = copter.animation.get_start_yaw()
            if not self.ros_telemetry:
                start_action = 'error: no telemetry data'
            else:
                start_action = copter.animation.get_start_action(
                                copter.config.animation_start_action, self.ros_telemetry.z,
                                copter.config.animation_takeoff_level, copter.config.animation_ground_level,
                                copter.config.animation_ratio, offset, self.fcu_status)
            return [x,y,z,yaw,start_action,start_delay]

    @classmethod
    def get_battery(cls, ros_telemetry):
        if ros_telemetry is None:
            return float('nan'), float('nan')

        battery_v = ros_telemetry.voltage

        batt_empty_param = mavros.get_param('BAT_V_EMPTY')
        batt_charged_param = mavros.get_param('BAT_V_CHARGED')
        batt_cells_param = mavros.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

            battery_p = (ros_telemetry.voltage / batt_cells - batt_empty) / (batt_charged - batt_empty) * 1.
            battery_p = max(min(battery_p, 1.), 0.)
        else:
            battery_p = float('nan')

        return battery_v, battery_p

    @classmethod
    def get_selfcheck(cls):
        check = flight.selfcheck()
        if not check:
            check = "OK"
        return check

    @classmethod
    def get_position(cls, ros_telemetry):
        try:
            x, y, z = ros_telemetry.x, ros_telemetry.y, ros_telemetry.z
        except AttributeError:
            return 'NO_POS'
        if not math.isnan(x):
            return x, y, z, math.degrees(ros_telemetry.yaw), copter.config.flight_frame_id
        return 'NO_POS'

    def get_ros_telemetry(self):
        return self.ros_telemetry

    def update_telemetry_fast(self):
        self.last_task = task_manager.get_current_task()
        try:
            self.ros_telemetry = flight.get_telemetry_locked(copter.config.flight_frame_id)
            if self.ros_telemetry.connected:
                self.armed = self.ros_telemetry.armed
                self.mode = self.ros_telemetry.mode
                self.selfcheck = self.get_selfcheck()
                self.current_position = self.get_position(self.ros_telemetry)
            else:
                self.reset_telemetry_values()
        except rospy.ServiceException:
            rospy.logdebug("Some service is unavailable")
            self.selfcheck = ["WAIT_ROS"]
        except AttributeError as e:
            rospy.logdebug(e)
        except rospy.TransportException as e:
            rospy.logdebug(e)
        self.time_delta = time.time()
        self.round_telemetry()

    def update_telemetry_slow(self):
        self.animation_info = [copter.animation.id, copter.animation.state]
        self.git_version = self.get_git_version()
        self.config_version = self.get_config_version()
        self.start_position = self.get_start_position()
        try:
            self.calibration_status = mavros.get_calibration_status()
            self.fcu_status = mavros.get_sys_status()
            self.battery = self.get_battery(self.ros_telemetry)
        except rospy.ServiceException:
            rospy.logdebug("Some service is unavailable")
            self.selfcheck = ["WAIT_ROS"]
        except AttributeError as e:
            rospy.logdebug(e)
        except rospy.TransportException as e:
            rospy.logdebug(e)

    def update(self):
        self.update_telemetry_fast()
        self.update_telemetry_slow()

    def round_telemetry(self):
        round_list = ["battery", "start_position", "current_position"]
        for key in round_list:
            if self.__dict__[key] not in [None, 'NO_POS', 'NO_FCU']:
                self.__dict__[key] = [round(v, 2) if type(v) == float else v for v in self.__dict__[key]]

    def reset_telemetry_values(self):
        self.battery = float('nan'), float('nan')
        self.calibration_status = 'NO_FCU'
        self.fcu_status = 'NO_FCU'
        self.mode = 'NO_FCU'
        self.selfcheck = ['NO_FCU']
        self.current_position = 'NO_POS'

    def check_failsafe_and_interruption(self):
        global emergency
        # check current state
        state = [self.mode, self.armed, task_manager.get_last_task_name()]
        mode, armed, last_task = state
        # check external interruption
        external_interruption = (mode != "OFFBOARD" and armed == True and last_task not in [None, 'land'])
        log_msg = ''
        if emergency:
            log_msg += 'emergency and '
        if external_interruption:
            log_msg += 'external interruption and '
            # count interruptions to avoid px4 mode glitches
            if state == self._last_state:
                self._interruption_counter += 1
            else:
                self._interruption_counter = 0
            logger.info("Possible expernal interruption, state_counter = {}".format(self._interruption_counter))
        # delete last ' end ' from log message
        if len(log_msg) > 5:
            log_msg = log_msg[:-5]
        # clear task manager if emergency or external interruption
        if emergency or (external_interruption and self._interruption_counter >= self._max_interruptions):
            if not self._tasks_cleared:
                logger.info("Clear task manager because of {}".format(log_msg))
                logger.info("Mode: {} | armed: {} | last task: {} ".format(mode, armed, last_task))
                task_manager.reset()
                flight.reset_delta()
                self._tasks_cleared = True
                self._interruption_counter = 0
        else:
            self._tasks_cleared = False
        self._last_state = state

    def transmit_message(self):  # todo if connected
        try:
            copter.server_connection.send_message('telemetry', kwargs={'value': self.create_msg_contents()})
        except AttributeError as e:
            logger.debug(e)

    @classmethod
    def log_cpu_and_memory(cls):
        cpu_usage = psutil.cpu_percent(interval=None, percpu=True)
        mem_usage = psutil.virtual_memory().percent
        cpu_temp_info = psutil.sensors_temperatures()['cpu-thermal'][0]
        cpu_temp = cpu_temp_info.current
        # https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md
        throttled_hex = subprocess.check_output("vcgencmd get_throttled", shell=True).split('=')[1]
        under_voltage = bool(int(bin(int(throttled_hex, 16))[2:][-1]))
        power_state = 'normal' if not under_voltage else 'under voltage!'
        if cpu_temp_info.critical:
            cpu_temp_state = 'critical'
        elif cpu_temp_info.high:
            cpu_temp_state = 'high'
        else:
            cpu_temp_state = 'normal'
        logger.info("CPU usage: {} | Memory: {} % | T: {} ({}) | Power: {}".format(
            cpu_usage, mem_usage, cpu_temp, cpu_temp_state, power_state))

    def _update_loop(self, freq):  # TODO extract?
        rate = rospy.Rate(freq)
        while not rospy.is_shutdown():

            self.update_telemetry_fast()
            self.check_failsafe_and_interruption()

            if copter.config.telemetry_transmit and copter.connected:
                self.transmit_message()

            rate.sleep()

    def _slow_update_loop(self):
        rate = rospy.Rate(1)
        while not rospy.is_shutdown():
            self.update_telemetry_slow()
            if copter.config.telemetry_log_resources:
                self.log_cpu_and_memory()
            rate.sleep()

    def start_loop(self):
        if copter.config.telemetry_frequency > 0:
            telemetry_thread = threading.Thread(target=self._update_loop, name="Telemetry getting thread",
                                                args=(copter.config.telemetry_frequency,))
            telemetry_thread.daemon = True
            telemetry_thread.start()
            slow_telemetry_thread = threading.Thread(target=self._slow_update_loop,
                                                     name="Slow telemetry getting thread")
            slow_telemetry_thread.daemon = True
            slow_telemetry_thread.start()
        else:
            logger.info("Telemetry loop is not created because of zero or negative telemetry frequency")

    def create_msg_contents(self, keys=None):  # keys: set or list
        if keys is None:
            keys = self.params_default_dict.keys()
        # return only existing keys from 'keys'
        return {k: self.__dict__[k] for k in keys if k in self.params_default_dict}


def emergency_callback(data):
    global emergency
    emergency = data.data


class AnimationEventHandler(FileSystemEventHandler):
    def on_any_event(self, event):
        logger.info('{} is {}'.format(event.src_path, event.event_type))
        # logger.info(os.path.splitext(event.src_path))
        if (os.path.splitext(event.src_path)[-1] == '.csv' and event.event_type != "deleted") or event.src_path.split('/')[-1] == 'client.ini':
            if os.path.exists("animation.csv"):
                logger.info("Update frames from animation.csv")
                copter.animation.update_frames(copter.config, "animation.csv")


if __name__ == "__main__":
    copter = CopterClient()
    task_manager = tasking.TaskManager()
    rospy.Subscriber('/emergency', Bool, emergency_callback)
    event_handler = AnimationEventHandler()
    observer = Observer()
    observer.schedule(event_handler, ".", recursive=True)
    observer.daemon = True
    observer.start()
    copter.start(task_manager)
    while not rospy.is_shutdown():
        rospy.sleep(0.1)