clever-show/Drone/client.py

from __future__ import print_function
import sys
import socket
import struct
import random
import time
import errno
import threading
import ConfigParser
from contextlib import closing

import rospy

from FlightLib.FlightLib import FlightLib
from FlightLib.FlightLib import LedLib

import play_animation

random.seed()

NTP_PACKET_FORMAT = "!12I"
NTP_DELTA = 2208988800L # 1970-01-01 00:00:00
NTP_QUERY = '\x1b' + 47 * '\0'  


def get_ntp_time(host, port):
    with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as s:
        s.sendto(NTP_QUERY, (host, port))
        msg, address = s.recvfrom(1024)
    unpacked = struct.unpack(NTP_PACKET_FORMAT, msg[0:struct.calcsize(NTP_PACKET_FORMAT)])
    return unpacked[10] + float(unpacked[11]) / 2**32 - NTP_DELTA


def reconnect(t=2):
    global clientSocket, host, port
    print("Trying to connect to", host, ":", port, "...")
    connected = False
    attempt_count = 0
    while not connected:
        print("Waiting for connection, attempt", attempt_count)
        try:
            clientSocket = socket.socket()
            # clientSocket.settimeout(3)
            clientSocket.connect((host, port))
            connected = True
            print("Connection successful")
        except socket.error as e:
            print("Waiting for connection:", e)
            time.sleep(t)
        attempt_count +=1
        if attempt_count >= 15:
            print("Too many attempts. Trying to get new server IP")
            broadcst_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            broadcst_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
            broadcst_client.bind(("", 8181))
            while True:
                data, addr = broadcst_client.recvfrom(1024)
                print("Recieved broadcast message %s from %s"%(data, addr))
                if parse_command(data.decode("UTF-8"))[0] == "server_ip":
                    print("Binding to new IP: ", addr)
                    host, port = addr
                    broadcst_client.close()
                    break


def send_all(msg):
        clientSocket.sendall(struct.pack('>I', len(msg)) + msg)


def recive_all(n):
    data = b''
    while len(data) < n:
        packet = clientSocket.recv(min(n - len(data), BUFFER_SIZE))
        if not packet:
            return None
        data += packet
    return data


def recive_message():
    raw_msglen = recive_all(4)
    if not raw_msglen:
        return None
    msglen = struct.unpack('>I', raw_msglen)[0]
    msg = recive_all(msglen)
    return msg


def form_command(command, args=()):
        return " ".join([command, args])


def parse_command(command_input):
    args = command_input.split()
    command = args.pop(0)
    return command, args


def recive_file(filename):
    print("Reciving file:", filename)
    with open(filename, 'wb') as file:  # TODO add directory
        while True:
            data = recive_message() #clientSocket.recv(BUFFER_SIZE)
            if data:
                print(data)
                if parse_command(data.decode("UTF-8"))[0] == "/endoffile":
                    print("File recived")
                    break
                file.write(data)


def write_to_config(section, option, value):
    config.set(section, option, value)
    with open(CONFIG_PATH, 'w') as file:
        config.write(file)


def animation_player(running_event, stop_event):
    print("Animation thread activated")
    frames = play_animation.read_animation_file()
    rate = rospy.Rate(1000 / 125)
    play_animation.takeoff()
    play_animation.animate_frame(frames[0]) #Reach first point at the same time with others
    rospy.sleep(5)
    for frame in frames:
        running_event.wait()
        if stop_event.is_set():
            break

        play_animation.animate_frame(frame)
        rate.sleep()
    else:
        play_animation.land()
        print("Animation ended")
    print("Animation thread closed")


stop_animation_event = threading.Event()
running_animation_event = threading.Event()


def start_animation(*args, **kwargs):
    animation_thread = threading.Thread(target=animation_player, args=(running_animation_event, stop_animation_event))
    play_animation.read_animation_file(animation_file)
    print("Starting animation!")
    running_animation_event.set()
    stop_animation_event.clear()
    animation_thread.start()


def resume_animation():
    print("Resuming animation")
    running_animation_event.set()


def pause_animation():
    print("Pausing animation")
    running_animation_event.clear()


def stop_animation():
    stop_animation_event.set()
    running_animation_event.clear()
    print("Stopping animation")
#    animation_thread.join()



CONFIG_PATH = "client_config.ini"
config = ConfigParser.ConfigParser()
config.read(CONFIG_PATH)


port = int(config.get('SERVER', 'port'))
host = config.get('SERVER', 'host')
BUFFER_SIZE = int(config.get('SERVER', 'buffer_size'))
NTP_HOST = config.get('NTP', 'host')
NTP_PORT = int(config.get('NTP', 'port'))

files_directory = config.get('FILETRANSFER', 'files_directory')
animation_file = config.get('COPTER', 'animation_file')

COPTER_ID = config.get('COPTER', 'id')
if COPTER_ID == 'default':
    COPTER_ID = 'copter' + str(random.randrange(9999)).zfill(4)
    write_to_config('COPTER', 'id', COPTER_ID)

TAKEOFF_HEIGHT = float(config.get('COPTER', 'takeoff_height'))

USE_LEDS = config.getboolean('COPTER', 'use_leds')
play_animation.USE_LEDS = USE_LEDS

rospy.init_node('Swarm_client', anonymous=True)
if USE_LEDS:
    LedLib.init_led()

print("Client started on copter:", COPTER_ID)
print("NTP time:", time.ctime(get_ntp_time(NTP_HOST, NTP_PORT)))
print("System time", time.ctime(time.time()))

reconnect()

print("Connected to server")

try:
    while True:
        try:
            message = recive_message()
            if message:
                message = message.decode("UTF-8")
                command, args = parse_command(message)
                print("Command from server:", command, args)
                if command == "writefile":
                    recive_file(args[0])
                elif command == "starttime":
                    starttime = float(args[0])
                    print("Starting on:", time.ctime(starttime))
                    dt = starttime - get_ntp_time(NTP_HOST, NTP_PORT)
                    print("Until start:", dt)
                    rospy.Timer(rospy.Duration(dt), start_animation, oneshot=True)
                elif command == 'takeoff':
                    play_animation.takeoff()
                elif command == 'pause'
                    pause_animation()
                elif command == 'resume'
                    resume_animation()
                elif command == 'stop':
                    stop_animation()
                    #FlightLib.reach(5, 5, 2)
                elif command == 'land':
                    FlightLib.land1()  # TODO dont forget change back to land
                elif command == 'disarm':
                    FlightLib.arming(False)
                elif command == 'request':
                    request_target = args[0]
                    print("Got request for:", request_target)
                    response = ""
                    if request_target == 'test':
                        response = "test_succsess"
                    elif request_target == 'id':
                        response = COPTER_ID
                    send_all(bytes(form_command("response", response)))
                    print("Request responded with:",  response)
        except socket.error as e:
            if e.errno != errno.EINTR:
                print("Connection lost due error:", e)
                print("Reconnecting...")
                reconnect()
                print("Re-connection successful")
            else:
                print("Interrupted")
                raise KeyboardInterrupt
except KeyboardInterrupt:
    print("Shutdown on keyboard interrupt")
finally:
    clientSocket.close()