clever-show/Drone/copter_client.py

import os
import sys
import time
import math
import rospy
import numpy
from clever import srv
import datetime
import logging
import threading
import psutil
import subprocess
from collections import namedtuple

from FlightLib import FlightLib
from FlightLib import LedLib

import client

import messaging_lib as messaging
import tasking_lib as tasking
import animation_lib as animation

from mavros_mavlink import *

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

static_bloadcaster = 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('animation_lib')
animation_logger.setLevel(logging.INFO)
animation_logger.addHandler(handler)

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 __init__(self, config_path="config/client.ini"):
        super(CopterClient, self).__init__(config_path)
        self.load_config()
        self.frames = {}

    def load_config(self):
        super(CopterClient, self).load_config()
        #print(self.config)
        # self.FLOOR_FRAME_EXISTS = False
        # self.TELEM_FREQ = self.config.getfloat('TELEMETRY', 'frequency')
        # self.TELEM_TRANSMIT = self.config.getboolean('TELEMETRY', 'transmit')
        # self.LOG_CPU_AND_MEMORY = self.config.getboolean('TELEMETRY', 'log_cpu_and_memory')
        # self.LAND_POS_DELTA = self.config.getfloat('TELEMETRY', 'land_if_pos_delta_bigger_than')
        # self.FRAME_ID = self.config.get('COPTERS', 'frame_id')
        # self.FRAME_FLIPPED_HEIGHT = 0.
        # self.TAKEOFF_HEIGHT = self.config.getfloat('COPTERS', 'takeoff_height')
        # self.TAKEOFF_TIME = self.config.getfloat('COPTERS', 'takeoff_time')
        # self.SAFE_TAKEOFF = self.config.getboolean('COPTERS', 'safe_takeoff')
        # self.RFP_TIME = self.config.getfloat('COPTERS', 'reach_first_point_time')
        # self.LAND_TIME = self.config.getfloat('COPTERS', 'land_time')
        # self.LAND_TIMEOUT = self.config.getfloat('COPTERS', 'land_timeout')
        # self.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common')
        # self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common')
        # self.Z0_COMMON = self.config.getfloat('COPTERS', 'z0_common')
        # self.YAW = self.config.get('COPTERS', 'yaw')
        # self.TAKEOFF_CHECK = self.config.getboolean('ANIMATION', 'takeoff_animation_check')
        # self.LAND_CHECK = self.config.getboolean('ANIMATION', 'land_animation_check')
        # self.FRAME_DELAY = self.config.getfloat('ANIMATION', 'frame_delay')
        # self.X_RATIO = self.config.getfloat('ANIMATION', 'x_ratio')
        # self.Y_RATIO = self.config.getfloat('ANIMATION', 'y_ratio')
        # self.Z_RATIO = self.config.getfloat('ANIMATION', 'z_ratio')
        # self.X0 = self.config.getfloat('PRIVATE', 'x0')
        # self.Y0 = self.config.getfloat('PRIVATE', 'y0')
        # 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_AFTER_RENAME = self.config.getboolean('PRIVATE', 'restart_after_rename')

    def on_broadcast_bind(self):
        configure_chrony_ip(self.config.server_host)
        restart_service("chrony")

    def start(self, task_manager_instance):
        rospy.loginfo("Init ROS node")
        rospy.init_node('clever_show_client')
        if self.config.led_use:
            LedLib.init_led(self.config.led_pin)
        task_manager_instance.start()  # TODO move to self
        if self.config.copter_frame_id == "floor":
            if self.config.floor_frame_enabled:
                self.start_floor_frame_broadcast()
            else:
                rospy.logerror("Can't make floor frame!")
        start_subscriber()

        telemetry.start_loop()
        super(CopterClient, self).start()

    def start_floor_frame_broadcast(self):
        trans = TransformStamped()
        trans.transform.translation.x = self.config.floor_frame_transtation[0]
        trans.transform.translation.y = self.config.floor_frame_transtation[1]
        trans.transform.translation.z = self.config.floor_frame_transtation[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.copter_frame_id
        static_bloadcaster.sendTransform(trans)


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


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


def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):
    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!")
        return False

    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")
def _response_id(*args, **kwargs):
    new_id = kwargs.get("new_id", None)
    if new_id is not None:
        old_id = client.active_client.client_id
        if new_id != old_id:
            client.active_client.config.set('PRIVATE', 'id', new_id, write=True)
            if new_id != '/hostname':
                if client.active_client.system_restart_after_rename:
                    hostname = client.active_client.client_id
                    configure_hostname(hostname)
                    configure_hosts(hostname)
                    configure_bashrc(hostname)
                    configure_motd(hostname)
                    execute_command("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 check_state_topic(wait_new_status=True):
        check = FlightLib.selfcheck()
        return check if check else "OK"
    else:
        stop_subscriber()
        return "NOT_CONNECTED_TO_FCU"


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


@messaging.request_callback("anim_id")
def _response_animation_id(*args, **kwargs):
    # Load animation
    result = animation.get_id()
    if result != 'No animation':
        logger.debug("Saving corrected animation")
        offset = numpy.array(client.active_client.config.private_offset) + numpy.array(client.active_client.config.copter_common_offset)
        frames = animation.load_animation(os.path.abspath("animation.csv"), offset[0], offset[1], offset[2], *client.active_client.config.animation_ratio)
        # Correct start and land frames in animation
        corrected_frames, start_action, start_delay = animation.correct_animation(frames,
                                                        check_takeoff=client.active_client.config.animation_takeoff_detection,
                                                        check_land=client.active_client.config.animation_land_detection,
                                                        )
        logger.debug("Start action: {}".format(start_action))
        # Save corrected animation
        animation.save_corrected_animation(corrected_frames)
    return result


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


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


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


@messaging.request_callback("cal_status")
def _response_cal_status(*args, **kwargs):
    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_locked(client.active_client.config.copter_frame_id)
    return "{:.2f} {:.2f} {:.2f} {:.1f} {}".format(
        telem.x, telem.y, telem.z, math.degrees(telem.yaw), client.active_client.config.copter_frame_id)


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


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


@messaging.request_callback("load_params")
def _load_params(*args, **kwargs):
    result = 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("move_start")
def _command_move_start_to_current_position(*args, **kwargs):
    x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"),
                                            *client.active_client.config.animation_ratio)
    logger.debug("x_start = {}, y_start = {}".format(x_start, y_start))
    if not math.isnan(x_start):
        telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
        logger.debug("x_telem = {}, y_telem = {}".format(telem.x, telem.y))
        if not math.isnan(telem.x):
            client.active_client.config.set('PRIVATE', 'offset', 
                                            [telem.x - x_start, telem.y - y_start, client.active_client.config.private_offset[2]],
                                            write=True)
            logger.info("Set start delta: {:.2f} {:.2f}".format(client.active_client.config.private_offset[0], 
                                                                client.active_client.config.private_offset[1]))
        else:
            logger.debug("Wrong telemetry")
    else:
        logger.debug("Wrong animation file")


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


@messaging.message_callback("set_z_to_ground")
def _command_set_z(*args, **kwargs):
    telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
    client.active_client.config.set('PRIVATE', 'offset', 
                                    [client.active_client.config.private_offset[0], client.active_client.config.private_offset[1], telem.z],
                                    write=True)
    logger.info("Set z offset to {:.2f}".format(client.active_client.config.private_offset[2]))


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


@messaging.message_callback("update_repo")
def _command_update_repo(*args, **kwargs):
    os.system("mv /home/pi/clever-show/Drone/client_config.ini /home/pi/clever-show/Drone/client_config_tmp.ini")
    os.system("git reset --hard HEAD")
    os.system("git checkout master")
    os.system("git fetch")
    os.system("git pull --rebase")
    os.system("mv /home/pi/clever-show/Drone/client_config_tmp.ini /home/pi/clever-show/Drone/client_config.ini")
    os.system("chown -R pi:pi /home/pi/clever-show")


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


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


@messaging.message_callback("service_restart")
def _command_service_restart(*args, **kwargs):
    service = kwargs["name"]
    restart_service(service)


@messaging.message_callback("repair_chrony")
def _command_chrony_repair(*args, **kwargs):
    configure_chrony_ip(client.active_client.config.server_host)
    restart_service("chrony")


@messaging.message_callback("led_test")
def _command_led_test(*args, **kwargs):
    LedLib.chase(255, 255, 255)
    time.sleep(2)
    LedLib.off()


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

    LedLib.fill(r, g, b)


@messaging.message_callback("flip")
def _copter_flip(*args, **kwargs):
    FlightLib.flip(frame_id=client.active_client.config.copter_frame_id)


@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": client.active_client.config.copter_takeoff_height,
                              "timeout": client.active_client.config.copter_takeoff_time,
                              "safe_takeoff": client.active_client.config.copter_safe_takeoff,
                              "use_leds": client.active_client.config.led_use,
                          }
                          )


@messaging.message_callback("takeoff_z")
def _command_takeoff_z(*args, **kwargs):
    z_str = kwargs.get("z", None)
    if z_str is not None:
        telem = FlightLib.get_telemetry_locked(client.active_client.config.copter_frame_id)
        logger.info("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now()))
        task_manager.add_task(0, 0, FlightLib.reach_point,
                              task_kwargs={
                                  "x": telem.x,
                                  "y": telem.y,
                                  "z": float(z_str),
                                  "frame_id": client.active_client.config.copter_frame_id,
                                  "timeout": client.active_client.config.copter_takeoff_time,
                                  "auto_arm": True,
                              }
                              )


@messaging.message_callback("land")
def _command_land(*args, **kwargs):
    task_manager.reset()
    task_manager.add_task(0, 0, animation.land,
                          task_kwargs={
                              "z": client.active_client.config.copter_takeoff_height,
                              "timeout": client.active_client.config.copter_takeoff_time,
                              "frame_id": client.active_client.config.copter_frame_id,
                              "use_leds": client.active_client.config.led_use,
                          }
                          )


@messaging.message_callback("emergency_land")
def _emergency_land(*args, **kwargs):
    logger.info(FlightLib.emergency_land().message)


@messaging.message_callback("disarm")
def _command_disarm(*args, **kwargs):
    task_manager.reset()
    task_manager.add_task(-5, 0, FlightLib.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):
    start_time = float(kwargs["time"])
    # Check if animation file is available
    if animation.get_id() == 'No animation':
        logger.error("Can't start animation without animation file!")
        return

    task_manager.reset(interrupt_next_task=False)

    logger.info("Start time = {}, wait for {} seconds".format(start_time, start_time - time.time()))
    # Load animation
    offset = numpy.array(client.active_client.config.private_offset) + numpy.array(client.active_client.config.copter_common_offset)
    frames = animation.load_animation(os.path.abspath("animation.csv"), offset[0], offset[1], offset[2], *client.active_client.config.animation_ratio)
    # Correct start and land frames in animation
    corrected_frames, start_action, start_delay = animation.correct_animation(frames,
                                                        check_takeoff=client.active_client.config.animation_takeoff_detection,
                                                        check_land=client.active_client.config.animation_land_detection,
                                                        )
    # Choose start action
    if start_action == 'takeoff':
        # Takeoff first
        task_manager.add_task(start_time, 0, animation.takeoff,
                              task_kwargs={
                                  "z": client.active_client.config.copter_takeoff_height,
                                  "timeout": client.active_client.config.copter_takeoff_time,
                                  "safe_takeoff": client.active_client.config.copter_safe_takeoff,
                                  # "frame_id": client.active_client.config.copter_frame_id,
                                  "use_leds": client.active_client.config.led_use,
                              }
                              )
        # Fly to first point
        rfp_time = start_time + client.active_client.config.copter_takeoff_time
        task_manager.add_task(rfp_time, 0, animation.execute_frame,
                              task_kwargs={
                                  "point": animation.convert_frame(corrected_frames[0])[0],
                                  "color": animation.convert_frame(corrected_frames[0])[1],
                                  "frame_id": client.active_client.config.copter_frame_id,
                                  "use_leds": client.active_client.config.led_use,
                                  "flight_func": FlightLib.reach_point,
                              }
                              )
        # Calculate first frame start time
        frame_time = rfp_time + client.active_client.config.copter_reach_first_point_time

    elif start_action == 'arm':
        # Calculate start time
        start_time += start_delay
        # Arm
        # task_manager.add_task(start_time, 0, FlightLib.arming_wrapper,
        #                    task_kwargs={
        #                        "state": True
        #                    }
        #                    )
        frame_time = start_time  # + 1.0
        point, color, yaw = animation.convert_frame(corrected_frames[0])
        task_manager.add_task(frame_time, 0, animation.execute_frame,
                              task_kwargs={
                                  "point": point,
                                  "color": color,
                                  "frame_id": client.active_client.config.copter_frame_id,
                                  "use_leds": client.active_client.config.led_use,
                                  "flight_func": FlightLib.navto,
                                  "auto_arm": True,
                              }
                              )
        # Calculate first frame start time
        frame_time += client.active_client.config.animation_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)
        if client.active_client.config.animation_yaw == "animation":
            yaw = frame["yaw"]
        else:
            yaw = math.radians(float(client.active_client.config.animation_yaw))
        task_manager.add_task(frame_time, 0, animation.execute_frame,
                              task_kwargs={
                                  "point": point,
                                  "color": color,
                                  "yaw": yaw,
                                  "frame_id": client.active_client.config.copter_frame_id,
                                  "use_leds": client.active_client.config.led_use,
                                  "flight_func": FlightLib.navto,
                              }
                              )
        frame_time += frame["delay"]

    # Calculate land_time
    land_time = frame_time + client.active_client.config.copter_land_time
    # Land
    task_manager.add_task(land_time, 0, animation.land,
                          task_kwargs={
                              "timeout": client.active_client.config.copter_land_timeout,
                              "frame_id": client.active_client.config.copter_frame_id,
                              "use_leds": client.active_client.config.led_use,
                          },
                          )


class Telemetry:
    params_default_dict = {
        "git_version": None,
        "animation_id": None,
        "battery": None,
        "armed": False,
        "fcu_status": None,
        "cal_status": None,
        "mode": None,
        "selfcheck": None,
        "current_position": None,
        "start_position": None,
        "task": None,
        "time": 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_start_position(cls):
        x_start, y_start = animation.get_start_xy(os.path.abspath("animation.csv"),
                                                  *client.active_client.config.animation_ratio)
        offset = numpy.array(client.active_client.config.private_offset) + numpy.array(client.active_client.config.copter_common_offset)
        x = x_start + offset[0]
        y = y_start + offset[1]
        z = offset[2]
        if not FlightLib._check_nans(x, y, z):
            return x, y, z
        return 'NO_POS'

    @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 = 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

            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 = FlightLib.selfcheck()
        if not check:
            check = "OK"
        return check

    @classmethod
    def get_position(cls, ros_telemetry):
        x, y, z = ros_telemetry.x, ros_telemetry.y, ros_telemetry.z
        if not math.isnan(x):
            return x, y, z, math.degrees(ros_telemetry.yaw), client.active_client.config.copter_frame_id
        return 'NO_POS'

    def update_telemetry_fast(self):
        self.start_position = self.get_start_position()
        self.task = task_manager.get_current_task()
        try:
            self.ros_telemetry = FlightLib.get_telemetry_locked(client.active_client.config.copter_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 = time.time()
        self.round_telemetry()

    def update_telemetry_slow(self):
        self.animation_id = animation.get_id()
        self.git_version = self.get_git_version()
        try:
            self.cal_status = get_calibration_status()
            self.fcu_status = 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.cal_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()
                FlightLib.reset_delta()
                self._tasks_cleared = True
                self._interruption_counter = 0
        else:
            self._tasks_cleared = False
        self._last_state = state

    def transmit_message(self):
        try:
            client.active_client.server_connection.send_message('telemetry', args={'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 client.active_client.config.telemetry_transmit and client.active_client.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 client.active_client.config.telemetry_log_resources:
                self.log_cpu_and_memory()
            rate.sleep()

    def start_loop(self):
        if client.active_client.config.telemetry_frequency > 0:
            telemetry_thread = threading.Thread(target=self._update_loop, name="Telemetry getting thread",
                                                args=(client.active_client.config.telemetry_frequency,))  # TODO MOVE? Daemon?
            slow_telemetry_thread = threading.Thread(target=self._slow_update_loop,
                                                     name="Slow telemetry getting thread")
            slow_telemetry_thread.start()
            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


if __name__ == "__main__":
    telemetry = Telemetry()
    copter_client = CopterClient()
    task_manager = tasking.TaskManager()
    rospy.Subscriber('/emergency', Bool, emergency_callback)
    copter_client.start(task_manager)