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__)))
sys.path.insert(0, os.path.realpath(os.path.join(current_dir, os.pardir, os.pardir)))

import lib.messaging as 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

def contains_nan(array):
    for num in array:
        if math.isnan(num):
            return True
    return False

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("animation.csv", self.config)
        self.gps_thread_run = False
        self.gps_thread_is_running = False

    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 on_config_update(self):
        self.gps_thread_run = False
        self.load_config()
        self.animation.on_config_update(self.config)
        if self.config.flight_frame_id == "floor":
            self.start_floor_frame_broadcast()
        elif self.config.flight_frame_id == "gps":
            self.start_gps_frame_broadcast()

    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):
        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):
        while self.gps_thread_is_running:
            rospy.sleep(1)
            logger.info("Wait until previous gps thread stop")
        self.gps_thread_run = True
        self.gps_thread_is_running = True
        rate = rospy.Rate(1)
        while not rospy.is_shutdown() and self.gps_thread_run:
            telem = flight.get_telemetry_locked(frame_id = "map")
            amsl_height = mavros.get_amsl_altitude()
            compass_hdg = mavros.get_compass_hdg()
            if telem is not None:
                if contains_nan([telem.lat, telem.lon, telem.x, telem.y, telem.yaw, amsl_height, compass_hdg]):
                    logger.info("Can't get position data from GPS")
                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
                    gps_dz = 0
                    if self.config.gps_frame_amsl != "current":
                        try:
                            gps_dz = telem.z - amsl_height + float(self.config.gps_frame_amsl)
                        except ValueError:
                            logger.error("Wrong amsl height in GPS FRAME config!")
                            continue
                    #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 = gps_dz
                    yaw = math.radians(compass_hdg) + telem.yaw - math.radians(self.config.gps_frame_yaw)
                    trans.transform.rotation = Quaternion(*quaternion_from_euler(0,0,yaw))
                    trans.header.frame_id = "map"
                    trans.child_frame_id = "gps"
                    static_broadcaster.sendTransform(trans)
            rate.sleep()
        self.gps_thread_is_running = False


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):
    pass # should be updated via watchdog event
    #copter.animation.update_frames(copter.config, "animation.csv")


@messaging.message_callback("move_start")
def _command_move_start_to_current_position(*args, **kwargs):
    private_offset = copter.config.animation_private_offset
    offset = numpy.array(private_offset) + numpy.array(copter.config.animation_common_offset)
    try:
        xs, ys, zs = copter.animation.get_start_frame(copter.telemetry.start_action).get_pos()
    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',
                [private_offset[0] + telem.x - xs, private_offset[1] + telem.y - ys, 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
    frames = copter.animation.get_output_frames(copter.telemetry.start_action)
    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

    # Play animation!
    frame_time = start_time
    for frame in frames:
        task_manager.add_task(frame_time, 0, animation.execute_frame,
                              task_kwargs={
                                  "frame": frame,
                                  "config": copter.config,
                              })
        frame_time += frame.delay
    task_manager.add_task(frame_time, 0, animation.turn_off_led)

# 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
        self.start_action = 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):
        try:
            x, y, z = copter.animation.get_start_frame('fly').get_pos()
        except (ValueError, AttributeError):
            return [float('nan'),float('nan'),float('nan'),float('nan'),"error: can't get start pos in animation",float('nan')]
        else:
            start_delay = copter.animation.start_time
            yaw = copter.animation.get_start_frame('fly').yaw
            if not self.ros_telemetry:
                self.start_action = 'error: no telemetry data'
            else:
                self.start_action = copter.animation.get_start_action(self.ros_telemetry.z, self.fcu_status)
            return [x,y,z,yaw,self.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', 'stand'])
        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 event.src_path.split('/')[-1] == 'client.ini':
            if os.path.exists("animation.csv"):
                copter.on_config_update()
                logger.info("Config updated!")
        elif (os.path.splitext(event.src_path)[-1] == '.csv' and event.event_type != "deleted"):
            if os.path.exists("animation.csv"):
                copter.animation.on_animation_update("animation.csv")
                logger.info("Animation updated!")


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)