From 785fec1bbdbb2c267ee43a84323447db045fa53e Mon Sep 17 00:00:00 2001 From: artem30801 Date: Tue, 26 Mar 2019 02:57:23 +0000 Subject: [PATCH 01/47] parent folder --- Drone/client.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Drone/client.py b/Drone/client.py index f0f8412..be0f0fb 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -7,6 +7,10 @@ import logging import ConfigParser from contextlib import closing +import os,sys,inspect +current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parent_dir = os.path.dirname(current_dir) +sys.path.insert(0, parent_dir) from messaging_lib import Message random.seed() @@ -151,4 +155,4 @@ class Client: if __name__ == "__main__": client = Client() client.reconnect() - client.mainloop() \ No newline at end of file + client.mainloop() From ba071a239dfaab4620c9370b6af7a786647ca388 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Mon, 8 Apr 2019 18:20:02 +0300 Subject: [PATCH 02/47] New server-client release --- Drone/client.py | 458 ++++++++++++----------------------------- Drone/copter_client.py | 252 +++++++++++++++++++++++ Server/server.py | 456 +++++++++++++++++++++++----------------- Server/server_qt.py | 61 +++--- __init__.py | 0 messaging_lib.py | 108 ++++++++++ 6 files changed, 787 insertions(+), 548 deletions(-) create mode 100644 Drone/copter_client.py create mode 100644 __init__.py create mode 100644 messaging_lib.py diff --git a/Drone/client.py b/Drone/client.py index 920f089..f0f8412 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -1,358 +1,154 @@ -from __future__ import print_function -import os -import sys -import socket -import struct -import random import time import errno -import json +import random +import socket +import struct import logging -import threading import ConfigParser from contextlib import closing -import rospy -import pause - -from FlightLib import FlightLib -from FlightLib import LedLib - -import play_animation +from messaging_lib import Message random.seed() logging.basicConfig( # TODO all prints as logs - level=logging.INFO, + level=logging.DEBUG, # INFO format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s", handlers=[ logging.FileHandler("client_logs.log"), logging.StreamHandler() ]) -NTP_PACKET_FORMAT = "!12I" -NTP_DELTA = 2208988800L # 1970-01-01 00:00:00 -NTP_QUERY = '\x1b' + 47 * '\0' + +class Client: + def __init__(self, config_path="client_config.ini"): + self.client_socket = None + self.server_host = None + self.server_port = None + self.broadcast_port = None + + self.connected = False + self.client_id = None + + # Init configs + self.config_path = config_path + self.config = ConfigParser.ConfigParser() + self.load_config() + + def load_config(self): + self.config.read(self.config_path) + + self.broadcast_port = self.config.getint('SERVER', 'broadcast_port') + self.server_port = self.config.getint('SERVER', 'port') + self.server_host = self.config.get('SERVER', 'host') + self.BUFFER_SIZE = self.config.getint('SERVER', 'buffer_size') + self.USE_NTP = self.config.getboolean('NTP', 'use_ntp') + self.NTP_HOST = self.config.get('NTP', 'host') + self.NTP_PORT = self.config.getint('NTP', 'port') + + files_directory = self.config.get('FILETRANSFER', 'files_directory') + + #FRAME_ID = self.config.get('COPTERS', 'frame_id') # TODO in play_animation + #self.TAKEOFF_HEIGHT = self.config.getfloat('COPTERS', 'takeoff_height') + #self.TAKEOFF_TIME = self.config.getfloat('COPTERS', 'takeoff_time') + #self.RFP_TIME = self.config.getfloat('COPTERS', 'reach_first_point_time') + #self.SAFE_TAKEOFF = self.config.getboolean('COPTERS', 'safe_takeoff') + + #self.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common') + #self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common') + #self.X0 = self.config.getfloat('PRIVATE', 'x0') + #self.Y0 = self.config.getfloat('PRIVATE', 'y0') + + #self.USE_LEDS = config.getboolean('PRIVATE', 'use_leds') + #play_animation.USE_LEDS = USE_LEDS # TODO in copter_client + + self.client_id = self.config.get('PRIVATE', 'id') + if self.client_id == 'default': + client_id = 'copter' + str(random.randrange(9999)).zfill(4) + #write_to_config('PRIVATE', 'id', client_id) + elif self.client_id == '/hostname': + self.client_id = socket.gethostname() + + @staticmethod + def get_ntp_time(ntp_host, ntp_port): + NTP_PACKET_FORMAT = "!12I" + NTP_DELTA = 2208988800L # 1970-01-01 00:00:00 + NTP_QUERY = '\x1b' + 47 * '\0' + + with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as s: + s.sendto(bytes(NTP_QUERY), (ntp_host, ntp_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 get_ntp_time(ntp_host, ntp_port): - with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as s: - s.sendto(NTP_QUERY, (ntp_host, ntp_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(timeout=2, attempt_limit=5): - 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(timeout) - clientSocket.connect((host, port)) - connected = True - print("Connection successful") - clientSocket.settimeout(None) - except socket.error as e: - if e.errno != errno.EINTR: - print("Waiting for connection, can not connect:", e) - time.sleep(timeout) - else: - print("Shutting down on keyboard interrupt") - raise KeyboardInterrupt - attempt_count += 1 - - if attempt_count >= attempt_limit: - print("Too many attempts. Trying to get new server IP") - broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - broadcast_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) - broadcast_client.bind(("", broadcast_port)) - while True: - data, addr = broadcast_client.recvfrom(1024) - print("Received broadcast message %s from %s" % (data, addr)) - command, args = parse_message(data.decode("UTF-8")) - if command == "server_ip": - host, port = args["host"], int(args["port"]) - print("Binding to new IP: ", host, port) - broadcast_client.close() - write_to_config("SERVER", "port", port) - write_to_config("SERVER", "host", host) - attempt_count = 0 - 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)) - print("Receiving packet {}; full data is {}".format(packet, data)) - if not packet: - return None - data += packet - return data - - -def recive_message(): - raw_msglen = recive_all(4) - if not raw_msglen: - print("No valid msg") - return None - msglen = struct.unpack('>I', raw_msglen)[0] - msg = recive_all(msglen) - return msg - - -def form_message(str_command, dict_arguments): - msg_dict = {str_command: dict_arguments} - msg = json.dumps(msg_dict) - return msg - - -def parse_message(msg): - try: - j_message = json.loads(msg) - except ValueError: - print("Json string not in correct format") - return None, None - str_command = list(j_message.keys())[0] - dict_arguments = list(j_message.values())[0] - - return str_command, dict_arguments - - -def recive_file(filename): - print("Receiving 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_message(data.decode("UTF-8"))[0] == "/endoffile": - print("File received") - break - file.write(data) + def reconnect(self, timeout=2, attempt_limit=5): + logging.info("Trying to connect to {}:{} ...".format(self.server_host, self.server_port)) + attempt_count = 0 + while not self.connected: + logging.info("Waiting for connection, attempt {}".format(attempt_count)) + try: + self.client_socket = socket.socket() + self.client_socket.settimeout(timeout) + self.client_socket.connect((self.server_host, self.server_port)) + except socket.error as error: + if error.errno != errno.EINTR: + logging.warning("Can not connect due error: {}".format(error)) + attempt_count += 1 + time.sleep(timeout) + else: + logging.critical("Shutting down on keyboard interrupt") + raise KeyboardInterrupt else: + self.connected = True + #self.client_socket.settimeout(None) + self.client_socket.setblocking(False) + logging.info("Connection to server successful!") break - -def animation_player(running_event, stop_event): - print("Animation thread activated") - frames = play_animation.read_animation_file() - if not frames: - logging.error("Animation is empty, shutting down animation player") - return - - delay_time = 0.125 - - print("Takeoff") - play_animation.takeoff(z=TAKEOFF_HEIGHT, safe_takeoff=SAFE_TAKEOFF) - takeoff_time = starttime + TAKEOFF_TIME - dt = takeoff_time - time.time() - print("Wait until takeoff " + str(dt) + "s: " + time.ctime(takeoff_time)) - pause.until(takeoff_time) - - print("Reach first point") - play_animation.reach_frame(frames[0], x0=X0+X0_COMMON, y0=Y0+Y0_COMMON) #Reach first point at the same time with others - rfp_time = takeoff_time + RFP_TIME - dt = rfp_time - time.time() - print("Wait reaching first point " + str(dt) + "s: " + time.ctime(rfp_time)) - pause.until(rfp_time) - - next_frame_time = rfp_time - print("Start animation at " + str(time.time())) - for frame in frames: - #running_event.wait() - play_animation.animate_frame(frame, x0=X0+X0_COMMON, y0=Y0+Y0_COMMON) - next_frame_time += delay_time - if stop_event.is_set(): - running_animation_event.clear() - break - #rate.sleep() - pause.until(next_frame_time) - else: - play_animation.land() - print("Animation ended") - print("Animation thread closed") + if attempt_count >= attempt_limit: + logging.info("Too many attempts. Trying to get new server IP") + self.broadcast_bind() + attempt_count = 0 -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)) - 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() - print("Stopping animation") -# animation_thread.join() - - -def selfcheck(): - return FlightLib.selfcheck() - - -def write_to_config(section, option, value): - config.set(section, option, value) - with open(CONFIG_PATH, 'w') as file: # TODO as separate function - config.write(file) - - -def load_config(): - global config, CONFIG_PATH - global broadcast_port, port, host, BUFFER_SIZE - global USE_NTP, NTP_HOST, NTP_PORT - global files_directory, animation_file - global FRAME_ID, TAKEOFF_HEIGHT, TAKEOFF_TIME, SAFE_TAKEOFF, RFP_TIME - global USE_LEDS, COPTER_ID - global X0, X0_COMMON, Y0, Y0_COMMON - CONFIG_PATH = "client_config.ini" - config = ConfigParser.ConfigParser() - config.read(CONFIG_PATH) - - broadcast_port = config.getint('SERVER', 'broadcast_port') - port = config.getint('SERVER', 'port') - host = config.get('SERVER', 'host') - BUFFER_SIZE = config.getint('SERVER', 'buffer_size') - USE_NTP = config.getboolean('NTP', 'use_ntp') - NTP_HOST = config.get('NTP', 'host') - NTP_PORT = config.getint('NTP', 'port') - - files_directory = config.get('FILETRANSFER', 'files_directory') - animation_file = config.get('FILETRANSFER', 'animation_file') - - FRAME_ID = config.get('COPTERS', 'frame_id') # TODO in play_animation - TAKEOFF_HEIGHT = config.getfloat('COPTERS', 'takeoff_height') - TAKEOFF_TIME = config.getfloat('COPTERS', 'takeoff_time') - RFP_TIME = config.getfloat('COPTERS', 'reach_first_point_time') - SAFE_TAKEOFF = config.getboolean('COPTERS', 'safe_takeoff') - - X0_COMMON = config.getfloat('COPTERS', 'x0_common') - Y0_COMMON = config.getfloat('COPTERS', 'y0_common') - X0 = config.getfloat('PRIVATE', 'x0') - Y0 = config.getfloat('PRIVATE', 'y0') - - USE_LEDS = config.getboolean('PRIVATE', 'use_leds') - play_animation.USE_LEDS = USE_LEDS - - COPTER_ID = config.get('PRIVATE', 'id') - if COPTER_ID == 'default': - COPTER_ID = 'copter' + str(random.randrange(9999)).zfill(4) - write_to_config('PRIVATE', 'id', COPTER_ID) - elif COPTER_ID == '/hostname': - COPTER_ID = socket.gethostname() - -load_config() - -rospy.init_node('Swarm_client', anonymous=True) -if USE_LEDS: - LedLib.init_led() - -print("Client started on copter:", COPTER_ID) -if USE_NTP: - 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: + def broadcast_bind(self): + broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + broadcast_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + broadcast_client.bind(("", self.broadcast_port)) try: - message = recive_message() - if message: - message = message.decode("UTF-8") - command, args = parse_message(message) - print("Command from server:", command, args) - if command == "writefile": - recive_file(args['filename']) - if bool(args['clever_restart']): - os.system("systemctl restart clever") - elif command == 'config_write': - write_to_config(args['section'], args['option'], args['value']) - elif command == 'config_reload': - load_config() - elif command == "starttime": - global starttime - starttime = float(args['time']) - print("Starting on:", time.ctime(starttime)) - dt = starttime - time.time() - if USE_NTP: - 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(safe_takeoff=SAFE_TAKEOFF) - elif command == 'pause': - pause_animation() - elif command == 'resume': - resume_animation() - elif command == 'stop': - stop_animation() - FlightLib.interrupt() - elif command == 'land': - play_animation.land() - elif command == 'disarm': - FlightLib.arming(False) - elif command == 'led_test': - LedLib.fill(255, 255, 255) - time.sleep(2) - LedLib.off() + while True: + data, addr = broadcast_client.recvfrom(1024) + message = Message() + message.income_raw = data + message.process_message() + if message.content: + logging.info("Received broadcast message {} from {}".format(message.content, addr)) + if message.content["command"] == "server_ip": + args = message.content["args"] + self.server_host = args["host"] + self.server_port = int(args["port"]) + logging.info("Binding to new IP: {}:{}".format(self.server_host, self.server_port)) + #write_to_config("SERVER", "port", port) + #write_to_config("SERVER", "host", host) # TODO + break + finally: + broadcast_client.close() - elif command == 'request': - request_target = args['value'] - print("Got request for:", request_target) - response = "" - if request_target == 'test': - response = "test_success" - elif request_target == 'id': - response = COPTER_ID - elif request_target == 'selfcheck': - check = FlightLib.selfcheck() - response = check if check else "OK" - elif request_target == 'batt_voltage': - response = FlightLib.get_telemetry('body').voltage - elif request_target == 'cell_voltage': - response = FlightLib.get_telemetry('body').cell_voltage + def mainloop(self): + while True: + #''' + message = Message() + print("Recieving message:") + while not message.content: + message.income_raw += self.client_socket.recv(1) + print(message.income_raw) + message.process_message() + print(message.content) + # ''' - send_all(bytes(form_message("response", - {"status": "ok", "value": response, "value_name": str(request_target)}))) - 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() +if __name__ == "__main__": + client = Client() + client.reconnect() + client.mainloop() \ No newline at end of file diff --git a/Drone/copter_client.py b/Drone/copter_client.py new file mode 100644 index 0000000..d7c8fd6 --- /dev/null +++ b/Drone/copter_client.py @@ -0,0 +1,252 @@ +from __future__ import print_function +import os +import sys +import socket +import struct +import random +import time +import errno +import json +import logging +import threading +import ConfigParser + +import rospy +import pause + +from .. import messaging_lib + +from FlightLib import FlightLib +from FlightLib import LedLib + +import play_animation + +def recive_file(filename): + print("Receiving 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_message(data.decode("UTF-8"))[0] == "/endoffile": + print("File received") + break + file.write(data) + else: + break + + +def animation_player(running_event, stop_event): + print("Animation thread activated") + frames = play_animation.read_animation_file() + if not frames: + logging.error("Animation is empty, shutting down animation player") + return + + delay_time = 0.125 + + print("Takeoff") + play_animation.takeoff(z=TAKEOFF_HEIGHT, safe_takeoff=SAFE_TAKEOFF) + takeoff_time = starttime + TAKEOFF_TIME + dt = takeoff_time - time.time() + print("Wait until takeoff " + str(dt) + "s: " + time.ctime(takeoff_time)) + pause.until(takeoff_time) + + print("Reach first point") + play_animation.reach_frame(frames[0], x0=X0+X0_COMMON, y0=Y0+Y0_COMMON) #Reach first point at the same time with others + rfp_time = takeoff_time + RFP_TIME + dt = rfp_time - time.time() + print("Wait reaching first point " + str(dt) + "s: " + time.ctime(rfp_time)) + pause.until(rfp_time) + + next_frame_time = rfp_time + print("Start animation at " + str(time.time())) + for frame in frames: + running_event.wait() + play_animation.animate_frame(frame, x0=X0+X0_COMMON, y0=Y0+Y0_COMMON) + next_frame_time += delay_time + if stop_event.is_set(): + running_animation_event.clear() + break + #rate.sleep() + pause.until(next_frame_time) + 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)) + 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() + print("Stopping animation") +# animation_thread.join() + + +def selfcheck(): + return FlightLib.selfcheck() + + +def write_to_config(section, option, value): + config.set(section, option, value) + with open(CONFIG_PATH, 'w') as file: # TODO as separate function + config.write(file) + + +def load_config(): + global config, CONFIG_PATH + global broadcast_port, port, host, BUFFER_SIZE + global USE_NTP, NTP_HOST, NTP_PORT + global files_directory, animation_file + global FRAME_ID, TAKEOFF_HEIGHT, TAKEOFF_TIME, SAFE_TAKEOFF, RFP_TIME + global USE_LEDS, COPTER_ID + global X0, X0_COMMON, Y0, Y0_COMMON + CONFIG_PATH = "client_config.ini" + config = ConfigParser.ConfigParser() + config.read(CONFIG_PATH) + + broadcast_port = config.getint('SERVER', 'broadcast_port') + port = config.getint('SERVER', 'port') + host = config.get('SERVER', 'host') + BUFFER_SIZE = config.getint('SERVER', 'buffer_size') + USE_NTP = config.getboolean('NTP', 'use_ntp') + NTP_HOST = config.get('NTP', 'host') + NTP_PORT = config.getint('NTP', 'port') + + files_directory = config.get('FILETRANSFER', 'files_directory') + animation_file = config.get('FILETRANSFER', 'animation_file') + + FRAME_ID = config.get('COPTERS', 'frame_id') # TODO in play_animation + TAKEOFF_HEIGHT = config.getfloat('COPTERS', 'takeoff_height') + TAKEOFF_TIME = config.getfloat('COPTERS', 'takeoff_time') + RFP_TIME = config.getfloat('COPTERS', 'reach_first_point_time') + SAFE_TAKEOFF = config.getboolean('COPTERS', 'safe_takeoff') + + X0_COMMON = config.getfloat('COPTERS', 'x0_common') + Y0_COMMON = config.getfloat('COPTERS', 'y0_common') + X0 = config.getfloat('PRIVATE', 'x0') + Y0 = config.getfloat('PRIVATE', 'y0') + + USE_LEDS = config.getboolean('PRIVATE', 'use_leds') + play_animation.USE_LEDS = USE_LEDS + + COPTER_ID = config.get('PRIVATE', 'id') + if COPTER_ID == 'default': + COPTER_ID = 'copter' + str(random.randrange(9999)).zfill(4) + write_to_config('PRIVATE', 'id', COPTER_ID) + elif COPTER_ID == '/hostname': + COPTER_ID = socket.gethostname() + +load_config() + +rospy.init_node('Swarm_client', anonymous=True) +if USE_LEDS: + LedLib.init_led() + +print("Client started on copter:", COPTER_ID) +if USE_NTP: + 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_message(message) + print("Command from server:", command, args) + if command == "writefile": + recive_file(args['filename']) + if bool(args['clever_restart']): + os.system("systemctl restart clever") + elif command == 'config_write': + write_to_config(args['section'], args['option'], args['value']) + elif command == 'config_reload': + load_config() + elif command == "starttime": + global starttime + starttime = float(args['time']) + print("Starting on:", time.ctime(starttime)) + dt = starttime - time.time() + if USE_NTP: + 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(safe_takeoff=SAFE_TAKEOFF) + elif command == 'pause': + pause_animation() + elif command == 'resume': + resume_animation() + elif command == 'stop': + stop_animation() + FlightLib.interrupt() + elif command == 'land': + play_animation.land() + elif command == 'disarm': + FlightLib.arming(False) + elif command == 'led_test': + LedLib.fill(255, 255, 255) + time.sleep(2) + LedLib.off() + + elif command == 'request': + request_target = args['value'] + print("Got request for:", request_target) + response = "" + if request_target == 'test': + response = "test_success" + elif request_target == 'id': + response = COPTER_ID + elif request_target == 'selfcheck': + check = FlightLib.selfcheck() + response = check if check else "OK" + elif request_target == 'batt_voltage': + response = FlightLib.get_telemetry('body').voltage + elif request_target == 'cell_voltage': + response = FlightLib.get_telemetry('body').cell_voltage + + send_all(bytes(form_message("response", + {"status": "ok", "value": response, "value_name": str(request_target)}))) + 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() + diff --git a/Server/server.py b/Server/server.py index 69ed0d6..52bcfdf 100644 --- a/Server/server.py +++ b/Server/server.py @@ -1,44 +1,41 @@ -import os import sys -import math import time -import json -import struct import socket import random import logging import threading +import selectors import collections import configparser +from messaging_lib import Message + # All imports sorted in pyramid just because random.seed() logging.basicConfig( # TODO all prints as logs - level=logging.INFO, + level=logging.DEBUG, format="%(asctime)s [%(name)-7.7s] [%(threadName)-19.19s] [%(levelname)-7.7s] %(message)s", handlers=[ logging.FileHandler("server_logs.log"), logging.StreamHandler() ]) - -class ConfigOption: - def __init__(self, section, option, value): - self.section = section - self.option = option - self.value = value +ConfigOption = collections.namedtuple("ConfigOption", ["section", "option", "value"]) +PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on", + "callback", "callback_args", "callback_kwargs" + ]) class Server: BUFFER_SIZE = 1024 def __init__(self, server_id=None, config_path="server_config.ini"): - self.id = server_id if server_id else str(random.randint(0, 9999)).zfill(4) # Init socket + self.sel = selectors.DefaultSelector() self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.host = socket.gethostname() @@ -47,13 +44,12 @@ class Server: # Init configs self.config_path = config_path self.config = configparser.ConfigParser() - self.config.read(self.config_path) self.load_config() # Init threads - self.autoconnect_thread = threading.Thread(target=self._auto_connect, daemon=True, - name='Client auto-connect') - self.autoconnect_thread_running = threading.Event() # Can be used for manual thread killing + self.autoconnect_thread = threading.Thread(target=self._client_processor, daemon=True, + name='Client processor') + self.client_processor_thread_running = threading.Event() # Can be used for manual thread killing self.broadcast_thread = threading.Thread(target=self._ip_broadcast, daemon=True, name='IP broadcast sender') @@ -64,7 +60,8 @@ class Server: self.listener_thread_running = threading.Event() def load_config(self): - self.port = int(self.config['SERVER']['port']) + self.config.read(self.config_path) + self.port = int(self.config['SERVER']['port']) # TODO try, init def self.broadcast_port = int(self.config['SERVER']['broadcast_port']) self.BROADCAST_DELAY = int(self.config['SERVER']['broadcast_delay']) Server.BUFFER_SIZE = int(self.config['SERVER']['buffer_size']) @@ -74,12 +71,12 @@ class Server: self.NTP_PORT = int(self.config['NTP']['port']) def start(self): # do_auto_connect=True, do_ip_broadcast=True, do_listen_broadcast=False - logging.info("Starting server with id: {} on {} !".format(self.id, self.ip)) + logging.info("Starting server with id: {} on {}:{} !".format(self.id, self.ip, self.port)) logging.info("Starting server socket!") self.server_socket.bind((self.ip, self.port)) - logging.info("Starting client autoconnect thread!") - self.autoconnect_thread_running.set() + logging.info("Starting client processor thread!") + self.client_processor_thread_running.set() self.autoconnect_thread.start() logging.info("Starting broadcast sender thread!") @@ -92,10 +89,11 @@ class Server: def stop(self): logging.info("Stopping server") - self.autoconnect_thread_running.clear() + self.client_processor_thread_running.clear() self.broadcast_thread_running.clear() self.listener_thread_running.clear() self.server_socket.close() + self.sel.close() logging.info("Server stopped") @staticmethod @@ -113,36 +111,59 @@ class Server: msg, _ = ntp_socket.recvfrom(1024) return int.from_bytes(msg[-8:], 'big') / 2 ** 32 - NTP_DELTA - def _auto_connect(self): - logging.info("Client autoconnect thread started!") - while self.autoconnect_thread_running.is_set(): - self.server_socket.listen(1) - c, addr = self.server_socket.accept() - logging.info("Got connection from: {}".format(str(addr))) - if not any(client_addr == addr[0] for client_addr in Client.clients.keys()): - client = Client(addr[0]) - logging.info("New client") - else: - logging.info("Reconnected client") - Client.clients[addr[0]].connect(c, addr) + # noinspection PyArgumentList + def _client_processor(self): + logging.info("Client processor (selector) thread started!") + self.server_socket.listen() + self.server_socket.setblocking(False) + self.sel.register(self.server_socket, selectors.EVENT_READ, data=None) + + while self.client_processor_thread_running.is_set(): + events = self.sel.select(timeout=None) + for key, mask in events: + if key.data is None: + self._connect_client(key.fileobj) + else: + client = key.data + try: + client.process_events(mask) + except Exception as error: + logging.error("Exception {} occurred for {}! Resetting connection!".format(error, client.addr)) + client.close() + logging.info("Client autoconnect thread stopped!") + def _connect_client(self, sock): + conn, addr = sock.accept() + logging.info("Got connection from: {}".format(str(addr))) + conn.setblocking(False) + + if not any(client_addr == addr[0] for client_addr in Client.clients.keys()): + client = Client(addr[0]) + logging.info("New client") + else: + client = Client.clients[addr[0]] + logging.info("Reconnected client") + self.sel.register(conn, selectors.EVENT_READ, data=client) + client.connect(self.sel, conn, addr) + def _ip_broadcast(self): logging.info("Broadcast sender thread started!") - msg = bytes(Client.form_message( - "server_ip", {"host": self.ip, "port": str(self.port), "id": self.id} - ), "UTF-8") + msg = Message.create_simple_message( + "server_ip", {"host": self.ip, "port": str(self.port), "id": self.id}) broadcast_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) broadcast_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) broadcast_sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) logging.info("Formed broadcast message: {}".format(msg)) - while self.broadcast_thread_running.is_set(): - time.sleep(self.BROADCAST_DELAY) - broadcast_sock.sendto(msg, ('255.255.255.255', self.broadcast_port)) - logging.debug("Broadcast sent") - broadcast_sock.close() - logging.info("Broadcast sender thread stopped, socked closed!") + try: + while self.broadcast_thread_running.is_set(): + time.sleep(self.BROADCAST_DELAY) + broadcast_sock.sendto(msg, ('255.255.255.255', self.broadcast_port)) + logging.debug("Broadcast sent") + finally: + broadcast_sock.close() + logging.info("Broadcast sender thread stopped, socked closed!") def _broadcast_listen(self): logging.info("Broadcast listener thread started!") @@ -154,23 +175,35 @@ class Server: logging.critical("Another server is running on this computer, shutting down!") sys.exit() - while self.listener_thread_running.is_set(): - data, addr = broadcast_client.recvfrom(1024) - command, args = Client.parse_message(data.decode("UTF-8")) - if command == "server_ip": - if args["id"] != self.id: - logging.critical("Another server detected on network, shutting down") - sys.exit() - broadcast_client.close() - logging.info("Broadcast listener thread stopped, socked closed!") + try: + while self.listener_thread_running.is_set(): + data, addr = broadcast_client.recvfrom(1024) + message = Message() + message.income_raw = data + message.process_message() + if message.content: + if message.content["command"] == "server_ip": + if message.content["args"]["id"] != self.id: + logging.critical("Another server detected on network, shutting down") + sys.exit() + else: + logging.warning("Got wrong broadcast message from {}".format(addr)) + finally: + broadcast_client.close() + logging.info("Broadcast listener thread stopped, socked closed!") - def send_starttime(self, copter, dt=0): + def time_now(self): if self.USE_NTP: timenow = Server.get_ntp_time(self.NTP_HOST, self.NTP_PORT) else: timenow = time.time() + return timenow + + def send_starttime(self, copter, dt=0): + timenow = self.time_now() print('Now:', time.ctime(timenow), "+ dt =", dt) - copter.send(Client.form_message("starttime", {"time": str(timenow + dt)})) + copter.send(Message.create_simple_message("starttime", {"time": str(timenow + dt)})) # TODO change start_on + # TODO all commands as tasks with timestamp and priority def requires_connect(f): @@ -192,7 +225,7 @@ def requires_any_connected(f): class Client: - resume_quee = True + resume_queue = True clients = {} @@ -201,9 +234,13 @@ class Client: on_disconnect = None def __init__(self, ip): + self.selector = None self.socket = None self.addr = None + self._recv_buffer = b"" + self._send_buffer = b"" + self._send_queue = collections.deque() self._received_queue = collections.deque() self._request_queue = collections.OrderedDict() @@ -212,167 +249,174 @@ class Client: self._request_lock = threading.Lock() self.copter_id = None - self.selected = False # Use to select copters for certain purposes DEPRECATED - Client.clients[ip] = self + self.clients[ip] = self self.connected = False - def connect(self, client_socket, client_addr): - print("Client connected") - if not Client.resume_quee: + def _set_selector_events_mask(self, mode): + """Set selector to listen for events: mode is 'r', 'w', 'rw'.""" + if mode == "r": + events = selectors.EVENT_READ + elif mode == "w": + events = selectors.EVENT_WRITE + elif mode == "rw": + events = selectors.EVENT_READ | selectors.EVENT_WRITE + else: + raise ValueError("Invalid events mask mode {}.".format(mode)) + self.selector.modify(self.socket, events, data=self) + + def connect(self, client_selector, client_socket, client_addr): + logging.info("Client connected") + if not Client.resume_queue: self._send_queue = collections.deque() + self.selector = client_selector self.socket = client_socket self.addr = client_addr - self.socket.setblocking(0) self.connected = True - client_thread = threading.Thread(target=self._run, name="Client {} thread".format(self.addr)) - client_thread.start() + if self.copter_id is None: - self.copter_id = self.get_response("id") - print("Got copter id:", self.copter_id) - if Client.on_first_connect: - Client.on_first_connect(self) + self.get_response("id", self._got_id) - if Client.on_connect: - Client.on_connect(self) + if self.on_connect: + self.on_connect(self) - def _send_all(self, msg): - self.socket.sendall(struct.pack('>I', len(msg)) + msg) + def _got_id(self): + logging.info("Got copter id: {} for client {}".format(self.copter_id, self.addr)) + if Client.on_first_connect: + Client.on_first_connect(self) - def _receive_all(self, n): - data = b'' - while len(data) < n: - packet = self.socket.recv(min(n - len(data), Server.BUFFER_SIZE)) - print("Receiving packet {}; full data is {}".format(packet, data)) - if not packet: - return None - data += packet - return data + def close(self): + logging.info("Closing connection to {}".format(self.addr)) + try: + self.selector.unregister(self.socket) + except AttributeError: + pass + except Exception as error: + logging.error("{}: Error during selector unregistering: {}".format(self.addr, error)) + finally: + self.selector = None - def _receive_message(self): - raw_msglen = self._receive_all(4) - if not raw_msglen: - print("No valid msg") - return None - msglen = struct.unpack('>I', raw_msglen)[0] - msg = self._receive_all(msglen) - return msg + try: + self.socket.close() + except AttributeError: + pass + except OSError as error: + logging.error("{}: Error during socket closing: {}".format(self.addr, error)) + finally: + self.socket = None - def _run(self): - while self.connected: - try: - if self._send_queue: - with self._send_lock: - msg = self._send_queue.popleft() - try: - self._send_all(msg) - print("Send", msg, "to", self.addr) - except socket.error as e: - logging.warning("Attempt to send failed: {}".format(e)) - with self._send_lock: - self._send_queue.appendleft(msg) - raise e + def process_events(self, mask): + print(self.socket, self.selector, mask) + if mask & selectors.EVENT_READ: + self.read() + if mask & selectors.EVENT_WRITE: + self.write() - try: # check if data in buffer - check = self.socket.recv(Server.BUFFER_SIZE, socket.MSG_PEEK) - if check: - received = self._receive_message() - if received: - received = received.decode("UTF-8") - print("Received", received, "from", self.addr) - command, args = Client.parse_message(received) - if command == "response": - with self._request_lock: - for key, value in self._request_queue.items(): - if not value and key == args["value_name"]: - self._request_queue[key] = args['value'] - print("Request successfully closed") - break - else: - print("Unexpected request") - else: - self._received_queue.appendleft(received) - except socket.error: + with self._send_lock: + if (not self._send_buffer) and self._send_queue: + message = self._send_queue.popleft() + self._send_buffer += message + self._set_selector_events_mask('rw') + + def process_received(self): + if self._received_queue: + if self._received_queue[-1].content: + message = self._received_queue.pop() + message_type = message.jsonheader["message-type"] + logging.debug("Received message! Header: {}, content: {}".format(message.jsonheader, message.content)) + if message_type == "message": + pass + elif message_type == "response": + request_id, requested_value = message.content["requst_id"], message.content["requested_value"] + with self._request_lock: + for key, value in self._request_queue.items(): + if (key == request_id) and (value.requested_value == requested_value): + request = self._request_queue.pop(key) + request.value = message.content["value"] + logging.debug( + "Request successfully closed with value {}".format(message.content["value"]) + ) + request.callback(request.value, *request.callback_args, **request.callback_kwargs) + break + else: + logging.warning("Unexpected request response!") + elif message_type == "request": pass - except socket.error as e: - logging.warning("Client error: {}, disconnected".format(e)) + def read(self): + self._read() + if self._recv_buffer: + if not self._received_queue or (self._received_queue[0].content is not None): + self._received_queue.appendleft(Message()) + + self._received_queue[0].income_raw += self._recv_buffer + self._received_queue[0].process_message() + + if self._received_queue[0].content and self._received_queue[0].income_raw: + self._recv_buffer = self._received_queue[0].income_raw + self._recv_buffer + self._received_queue[0].income_raw = b'' + + self.process_received() + + def write(self): + self._write() + if not (self._send_buffer and self._send_queue): + self._set_selector_events_mask("r") + + def _read(self): + try: + data = self.socket.recv(Server.BUFFER_SIZE) + except BlockingIOError: + # Resource temporarily unavailable (errno EWOULDBLOCK) + pass + else: + if data: + self._recv_buffer += data + logging.debug("Received {} from {}".format(data, self.addr)) + else: + logging.warning("Connection to {} lost!".format(self.addr)) self.connected = False - self.socket.close() + if not Client.resume_queue: + self._recv_buffer = b'' + if Client.on_disconnect: Client.on_disconnect(self) - break - # time.sleep(0.05) - @staticmethod - def form_message(command: str, dict_arguments: dict = None): - if dict_arguments is None: - dict_arguments = {} - msg_dict = {command: dict_arguments} - msg = json.dumps(msg_dict) - return msg + raise RuntimeError("Peer closed.") - @staticmethod - def parse_message(msg): - try: - j_message = json.loads(msg) - except ValueError: - print("Json string not in correct format") - return None, None - str_command = list(j_message.keys())[0] - dict_arguments = list(j_message.values())[0] + def _write(self): + if self._send_buffer: + try: + sent = self.socket.send(self._send_buffer) + except BlockingIOError: + # Resource temporarily unavailable (errno EWOULDBLOCK) + pass + except socket.error as error: + logging.warning("Attempt to send message {} to {} failed due error: {}".format( + self._send_buffer, self.addr, error)) - return str_command, dict_arguments + if not Client.resume_queue: + self._send_buffer = b'' + + raise error + else: + print(sent) + logging.debug("Sent {} to {}".format(self._send_buffer[:sent], self.addr)) + self._send_buffer = self._send_buffer[sent:] + print(self._send_buffer) + time.sleep(0.1) @requires_connect def send(self, *messages): for message in messages: with self._send_lock: - self._send_queue.append(bytes(message, "UTF-8")) + self._send_queue.append(message) - @requires_connect - def get_response(self, requested_value): - with self._request_lock: - self._request_queue[requested_value] = "" - self.send(Client.form_message("request", {"value": requested_value})) - - while not self._request_queue[requested_value]: - pass - - with self._request_lock: - return self._request_queue.pop(requested_value) - - @requires_connect - def send_file(self, filepath, dest_filename, clever_restart = False): - print("Sending file ", dest_filename) - chunk_count = math.ceil(os.path.getsize(filepath) / Server.BUFFER_SIZE) - self.send(Client.form_message("writefile", {"filesize": chunk_count, "filename": dest_filename, "clever_restart": clever_restart})) - with open(filepath, 'rb') as file: - chunk = file.read(Server.BUFFER_SIZE) - while chunk: - with self._send_lock: - self._send_queue.append(chunk) - chunk = file.read(Server.BUFFER_SIZE) - - self.send(Client.form_message("/endoffile")) # TODO mb remove - print("File sent") - - @staticmethod - @requires_any_connected - def send_to_selected(message): # DEPRECATED - for client in Client.clients.values(): - if client.connected and client.selected: - client.send(message) - - @staticmethod - @requires_any_connected - def request_to_selected(requested_value): # DEPRECATED - for client in Client.clients.values(): - if client.connected and client.selected: - client.get_response(requested_value) + def send_message(self, command, args=None): + self.send(Message.create_simple_message(command, args)) @staticmethod @requires_any_connected @@ -381,12 +425,48 @@ class Client: if client.connected or force_all: client.send(message) + @classmethod + @requires_any_connected + def broadcast_message(cls, command, args=None, force_all=False): + cls.broadcast(Message.create_simple_message(command, args), force_all) + + def get_response(self, requested_value, callback, request_args=None, # timeout=30, + callback_args=(), callback_kwargs: dict=None): + if request_args is None: + request_args = {} + if callback_kwargs is None: + callback_kwargs = {} + + request_id = str(random.randint(0, 9999)).zfill(4) + with self._request_lock: + self._request_queue[request_id] = PendingRequest( + requested_value=requested_value, + value=None, + # expires_on=Server.time_now()+timeout, + callback=callback, + callback_args=callback_args, + callback_kwargs=callback_kwargs, + ) + self.send(Message.create_request(requested_value, request_id, request_args)) + + def send_file(self, filepath, dest_filepath): # clever_restart=False + try: + with open(filepath, 'rb') as file: + data = file.read() + except OSError as error: + logging.warning("File can not be opened due error: ".format(error)) + else: + logging.info("Sending file {} to {} (as: {})".format(filepath, self.addr, dest_filepath)) + self.send(Message.create_message(data, "binary", "filetransfer", "binary", {"path": dest_filepath})) + def send_config_options(self, *options: ConfigOption): + logging.info("Sending config options: {} to {}".format(options, self.addr)) for option in options: - self.send( - Client.form_message('config_write', - {'section': option.section, 'option': option.option, 'value': option.value})) - self.send(Client.form_message("config_reload")) + self.send_message( + 'config_write', + {'section': option.section, 'option': option.option, 'value': option.value} + ) + self.send_message("config_reload") @staticmethod def get_by_id(copter_id): diff --git a/Server/server_qt.py b/Server/server_qt.py index 161a34b..e6acb1a 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -1,8 +1,8 @@ +import os import glob from PyQt5 import QtWidgets from PyQt5.QtGui import QStandardItemModel, QStandardItem -#from PyQt5.QtCore import QModelIndex from PyQt5.QtCore import Qt, pyqtSlot from PyQt5.QtWidgets import QFileDialog, QMessageBox @@ -11,19 +11,19 @@ from PyQt5.QtWidgets import QFileDialog, QMessageBox from server_gui import Ui_MainWindow from server import * -class CopterView(QStandardItemModel): - pass + +# noinspection PyArgumentList,PyCallByClass class MainWindow(QtWidgets.QMainWindow): def __init__(self): super(MainWindow, self).__init__() self.ui = Ui_MainWindow() self.ui.setupUi(self) - self.initUI() + self.init_ui() self.show() - def initUI(self): - #Connecting + def init_ui(self): + # Connecting self.ui.check_button.clicked.connect(self.check_selected) self.ui.start_button.clicked.connect(self.send_starttime) self.ui.pause_button.clicked.connect(self.pause_all) @@ -38,7 +38,7 @@ class MainWindow(QtWidgets.QMainWindow): self.ui.action_send_configurations.triggered.connect(self.send_configurations) self.ui.action_send_Aruco_map.triggered.connect(self.send_aruco) - #Initing table and table model + # Initing table and table model self.ui.tableView.setModel(model) self.ui.tableView.horizontalHeader().setStretchLastSection(True) @@ -49,24 +49,25 @@ class MainWindow(QtWidgets.QMainWindow): if item.isCheckable() and item.checkState() == Qt.Checked: print("Copter {} checked".format(model.item(row_num, 0).text())) copter = Client.get_by_id(item.text()) - batt_total = float(copter.get_response("batt_voltage")) - batt_cell = float(copter.get_response("cell_voltage")) - selfcheck = copter.get_response("selfcheck") - - batt_percent = ((batt_cell-3.2)/(4.2-3.2))*100 - - model.setData(model.index(row_num, 2), "{} V.".format(round(batt_total, 3))) - model.setData(model.index(row_num, 3), "{} %".format(round(batt_percent, 3))) - if selfcheck != "OK": - print(selfcheck) - model.setData(model.index(row_num, 4), str(selfcheck)) - else: - print("Everything ok") - model.setData(model.index(row_num, 4), str(selfcheck)) + copter.get_response("batt_voltage", self._set_copter_data, callback_args=(row_num, 2)) + copter.get_response("cell_voltage", self._set_copter_data, callback_args=(row_num, 3)) + copter.get_response("selfcheck", self._set_copter_data, callback_args=(row_num, 4)) self.ui.start_button.setEnabled(True) self.ui.takeoff_button.setEnabled(True) + def _set_copter_data(self, value, row, col): + if col == 2: + model.setData(model.index(row, col), "{} V.".format(round(float(value), 3))) + elif col == 3: + batt_percent = ((float(value) - 3.2) / (4.2 - 3.2)) * 100 + model.setData(model.index(row, col), "{} %".format(round(batt_percent, 3))) + elif col == 4: + if value != "OK": + model.setData(model.index(row, col), str(value)) + else: + model.setData(model.index(row, col), str(value)) + @pyqtSlot() def send_starttime(self): dt = self.ui.start_delay_spin.value() @@ -88,15 +89,15 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def stop_all(self): - Client.broadcast(Client.form_message("stop")) + Client.broadcast_message("stop") @pyqtSlot() def pause_all(self): if self.ui.pause_button.text() == 'Pause': - Client.broadcast(Client.form_message('pause')) + Client.broadcast_message('pause') self.ui.pause_button.setText('Resume') else: - Client.broadcast(Client.form_message('resume')) + Client.broadcast_message('resume') self.ui.pause_button.setText('Pause') @pyqtSlot() @@ -106,7 +107,7 @@ class MainWindow(QtWidgets.QMainWindow): if item.isCheckable() and item.checkState() == Qt.Checked: if True: # TODO checks for batt/selfckeck here copter = Client.get_by_id(item.text()) - copter.send(Client.form_message("led_test")) + copter.send_message("led_test") @pyqtSlot() def takeoff_selected(self): @@ -122,17 +123,18 @@ class MainWindow(QtWidgets.QMainWindow): if item.isCheckable() and item.checkState() == Qt.Checked: if True: # TODO checks for batt/selfckeck here copter = Client.get_by_id(item.text()) - copter.send(Client.form_message("takeoff")) + copter.send_message("takeoff") else: print("Cancelled") + pass @pyqtSlot() def land_all(self): - Client.broadcast(Client.form_message("land")) + Client.broadcast_message("land") @pyqtSlot() def disarm_all(self): - Client.broadcast(Client.form_message("disarm")) + Client.broadcast_message("disarm") @pyqtSlot() def send_animations(self): @@ -181,7 +183,8 @@ class MainWindow(QtWidgets.QMainWindow): item = model.item(row_num, 0) if item.isCheckable() and item.checkState() == Qt.Checked: copter = Client.get_by_id(item.text()) - copter.send_file(path, "/home/pi/catkin_ws/src/clever/aruco_pose/map/animation_map.txt", clever_restart=True) + copter.send_file(path, "/home/pi/catkin_ws/src/clever/aruco_pose/map/animation_map.txt") + # clever_restart=True model = QStandardItemModel() diff --git a/__init__.py b/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/messaging_lib.py b/messaging_lib.py new file mode 100644 index 0000000..c3fb1f4 --- /dev/null +++ b/messaging_lib.py @@ -0,0 +1,108 @@ +import io +import sys +import json +import struct + + +class Message: + def __init__(self): + self.income_raw = b"" + self._jsonheader_len = None + self.jsonheader = None + self.content = None + + @staticmethod + def _json_encode(obj, encoding="utf-8"): + return json.dumps(obj, ensure_ascii=False).encode(encoding) + + @staticmethod + def _json_decode(json_bytes, encoding="utf-8"): + with io.TextIOWrapper(io.BytesIO(json_bytes), encoding=encoding, newline="") as tiow: + obj = json.load(tiow) + return obj + + @classmethod + def create_message(cls, content_bytes, content_type, message_type, content_encoding="utf-8", + additional_headers=None): + jsonheader = { + "byteorder": sys.byteorder, + "content-type": content_type, + "content-encoding": content_encoding, + "content-length": len(content_bytes), + "message-type": message_type, + } + if additional_headers: + jsonheader.update(additional_headers) + + jsonheader_bytes = cls._json_encode(jsonheader, "utf-8") + message_hdr = struct.pack(">H", len(jsonheader_bytes)) + message = message_hdr + jsonheader_bytes + content_bytes + return message + + @classmethod + def create_json_message(cls, contents): + message = cls.create_message(cls._json_encode(contents), "json", "message") + return message + + @classmethod + def create_simple_message(cls, command, args=None): + if args is None: + args = {} + message = cls.create_json_message({"command": command, "args": args}) + return message + + @classmethod + def create_request(cls, requested_value, request_id, args=None): + if args is None: + args = {} + contents = {"requested_value": requested_value, + "requst_id": request_id, + "args": args, + } + message = cls.create_message(cls._json_encode(contents), "json", "request") + return message + + def _process_protoheader(self): + header_len = 2 + if len(self.income_raw) >= header_len: + self._jsonheader_len = struct.unpack(">H", self.income_raw[:header_len])[0] + self.income_raw = self.income_raw[header_len:] + + def _process_jsonheader(self): + header_len = self._jsonheader_len + if len(self.income_raw) >= header_len: + self.jsonheader = self._json_decode(self.income_raw[:header_len], "utf-8") + self.income_raw = self.income_raw[header_len:] + for reqhdr in ( + "byteorder", + "content-length", + "content-type", + "content-encoding", + "message-type", + ): + if reqhdr not in self.jsonheader: + raise ValueError('Missing required header {}'.format(reqhdr)) + + def _process_content(self): + content_len = self.jsonheader["content-length"] + if not len(self.income_raw) >= content_len: + return + data = self.income_raw[:content_len] + self.income_raw = self.income_raw[content_len:] + if self.jsonheader["content-type"] == "json": + encoding = self.jsonheader["content-encoding"] + self.content = self._json_decode(data, encoding) + else: + self.content = data + + def process_message(self): + if self._jsonheader_len is None: + self._process_protoheader() + + if self._jsonheader_len is not None: + if self.jsonheader is None: + self._process_jsonheader() + + if self.jsonheader: + if self.content is None: + self._process_content() From 95d9405849903fe23fa9a24dec242800bfce8bae Mon Sep 17 00:00:00 2001 From: Artem Date: Tue, 9 Apr 2019 00:12:01 +0700 Subject: [PATCH 03/47] rw in server --- Server/server.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Server/server.py b/Server/server.py index 52bcfdf..40f4d82 100644 --- a/Server/server.py +++ b/Server/server.py @@ -116,7 +116,7 @@ class Server: logging.info("Client processor (selector) thread started!") self.server_socket.listen() self.server_socket.setblocking(False) - self.sel.register(self.server_socket, selectors.EVENT_READ, data=None) + self.sel.register(self.server_socket, selectors.EVENT_READ | selectors.EVENT_WRITE, data=None) while self.client_processor_thread_running.is_set(): events = self.sel.select(timeout=None) @@ -275,6 +275,8 @@ class Client: self.socket = client_socket self.addr = client_addr + self._set_selector_events_mask('rw') + self.connected = True if self.copter_id is None: @@ -319,7 +321,7 @@ class Client: if (not self._send_buffer) and self._send_queue: message = self._send_queue.popleft() self._send_buffer += message - self._set_selector_events_mask('rw') + # self._set_selector_events_mask('rw') def process_received(self): if self._received_queue: @@ -363,8 +365,8 @@ class Client: def write(self): self._write() - if not (self._send_buffer and self._send_queue): - self._set_selector_events_mask("r") + # if not (self._send_buffer and self._send_queue): + # self._set_selector_events_mask("r") def _read(self): try: From 8d1589fb47e9b3b58ea5e4f2e3b4de51a7780297 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Mon, 8 Apr 2019 20:28:24 +0300 Subject: [PATCH 04/47] First working client --- Drone/client.py | 47 +++++++++++++++++++++++++++++++++++------------ 1 file changed, 35 insertions(+), 12 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index f0f8412..0011241 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -4,6 +4,7 @@ import random import socket import struct import logging +import selectors2 as selectors import ConfigParser from contextlib import closing @@ -22,6 +23,7 @@ logging.basicConfig( # TODO all prints as logs class Client: def __init__(self, config_path="client_config.ini"): + self.selector = selectors.DefaultSelector() self.client_socket = None self.server_host = None self.server_port = None @@ -81,7 +83,6 @@ class Client: 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(self, timeout=2, attempt_limit=5): logging.info("Trying to connect to {}:{} ...".format(self.server_host, self.server_port)) attempt_count = 0 @@ -103,6 +104,8 @@ class Client: self.connected = True #self.client_socket.settimeout(None) self.client_socket.setblocking(False) + events = selectors.EVENT_READ | selectors.EVENT_WRITE + self.selector.register(self.client_socket, events, data=None) logging.info("Connection to server successful!") break @@ -111,7 +114,6 @@ class Client: self.broadcast_bind() attempt_count = 0 - def broadcast_bind(self): broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) broadcast_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) @@ -135,17 +137,38 @@ class Client: finally: broadcast_client.close() + def service_connection(self, key, mask): + sock = key.fileobj + data = key.data + if mask & selectors.EVENT_READ: + recv_data = sock.recv(1024) # Should be ready to read + if recv_data: + print("received", repr(recv_data), "from connection", ) + if not recv_data: + print("closing connection",) + self.selector.unregister(sock) + self.client_socket.close() + if mask & selectors.EVENT_READ: + pass + + def mainloop(self): - while True: - #''' - message = Message() - print("Recieving message:") - while not message.content: - message.income_raw += self.client_socket.recv(1) - print(message.income_raw) - message.process_message() - print(message.content) - # ''' + #self.client_socket.send("104771313759739") + try: + while True: + events = self.selector.select(timeout=1) + if events: + for key, mask in events: + self.service_connection(key, mask) + pass + + if not self.selector.get_map(): + logging.warning("No active connections left") + self.reconnect() + except KeyboardInterrupt: + print("caught keyboard interrupt, exiting") + finally: + self.selector.close() if __name__ == "__main__": From e35bb5241341f9b2501e294652cf8985c8ded211 Mon Sep 17 00:00:00 2001 From: Artem Date: Tue, 9 Apr 2019 00:29:21 +0700 Subject: [PATCH 05/47] gitignore --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 96305fc..2c2068e 100644 --- a/.gitignore +++ b/.gitignore @@ -109,3 +109,5 @@ Drone/test_animation/ Drone/animation.csv Drone/client_logs images/ + +\.idea/ From 1dd033530860421d4dbe625a912120e5df5fde97 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Tue, 9 Apr 2019 08:59:44 +0300 Subject: [PATCH 06/47] Client ad server sharing same connection handler now --- Drone/client.py | 51 ++++------ Server/server.py | 242 ++++++------------------------------------- messaging_lib.py | 259 ++++++++++++++++++++++++++++++++++++++++++++++- 3 files changed, 310 insertions(+), 242 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index 87614ac..241a0d3 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -4,6 +4,7 @@ import random import socket import struct import logging +import collections import selectors2 as selectors import ConfigParser from contextlib import closing @@ -13,7 +14,7 @@ current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentfra parent_dir = os.path.dirname(current_dir) sys.path.insert(0, parent_dir) -from messaging_lib import Message +import messaging_lib as messaging random.seed() logging.basicConfig( # TODO all prints as logs @@ -29,6 +30,9 @@ class Client: def __init__(self, config_path="client_config.ini"): self.selector = selectors.DefaultSelector() self.client_socket = None + + self.server_connection = messaging.ConnectionManager() + self.server_host = None self.server_port = None self.broadcast_port = None @@ -70,7 +74,7 @@ class Client: self.client_id = self.config.get('PRIVATE', 'id') if self.client_id == 'default': - client_id = 'copter' + str(random.randrange(9999)).zfill(4) + self.client_id = 'copter' + str(random.randrange(9999)).zfill(4) #write_to_config('PRIVATE', 'id', client_id) elif self.client_id == '/hostname': self.client_id = socket.gethostname() @@ -105,11 +109,6 @@ class Client: logging.critical("Shutting down on keyboard interrupt") raise KeyboardInterrupt else: - self.connected = True - #self.client_socket.settimeout(None) - self.client_socket.setblocking(False) - events = selectors.EVENT_READ | selectors.EVENT_WRITE - self.selector.register(self.client_socket, events, data=None) logging.info("Connection to server successful!") break @@ -118,14 +117,20 @@ class Client: self.broadcast_bind() attempt_count = 0 + def _connect(self): + self.connected = True + self.client_socket.setblocking(False) + events = selectors.EVENT_READ | selectors.EVENT_WRITE + self.selector.register(self.client_socket, events, data=None) + def broadcast_bind(self): broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) broadcast_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) broadcast_client.bind(("", self.broadcast_port)) try: while True: - data, addr = broadcast_client.recvfrom(1024) - message = Message() + data, addr = broadcast_client.recvfrom(self.BUFFER_SIZE) + message = messaging.MessageManager() message.income_raw = data message.process_message() if message.content: @@ -141,39 +146,25 @@ class Client: finally: broadcast_client.close() - def service_connection(self, key, mask): - sock = key.fileobj - data = key.data - if mask & selectors.EVENT_READ: - recv_data = sock.recv(1024) # Should be ready to read - if recv_data: - print("received", repr(recv_data), "from connection", ) - if not recv_data: - print("closing connection",) - self.selector.unregister(sock) - self.client_socket.close() - if mask & selectors.EVENT_READ: - pass - - def mainloop(self): - #self.client_socket.send("104771313759739") try: while True: events = self.selector.select(timeout=1) if events: for key, mask in events: - self.service_connection(key, mask) - pass + if key.data is None: + connection = key.data + connection.process_events(mask) if not self.selector.get_map(): - logging.warning("No active connections left") + logging.warning("No active connections left!") self.reconnect() - except KeyboardInterrupt: - print("caught keyboard interrupt, exiting") + except (KeyboardInterrupt, errno.EINTR): + logging.critical("Caught interrupt, exiting!") finally: self.selector.close() +# TODO class connection if __name__ == "__main__": client = Client() diff --git a/Server/server.py b/Server/server.py index 40f4d82..6dc972a 100644 --- a/Server/server.py +++ b/Server/server.py @@ -8,7 +8,7 @@ import selectors import collections import configparser -from messaging_lib import Message +import messaging_lib as messaging # All imports sorted in pyramid just because @@ -23,9 +23,6 @@ logging.basicConfig( # TODO all prints as logs ]) ConfigOption = collections.namedtuple("ConfigOption", ["section", "option", "value"]) -PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on", - "callback", "callback_args", "callback_kwargs" - ]) class Server: @@ -149,7 +146,7 @@ class Server: def _ip_broadcast(self): logging.info("Broadcast sender thread started!") - msg = Message.create_simple_message( + msg = messaging.MessageManager.create_simple_message( "server_ip", {"host": self.ip, "port": str(self.port), "id": self.id}) broadcast_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) broadcast_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) @@ -178,7 +175,7 @@ class Server: try: while self.listener_thread_running.is_set(): data, addr = broadcast_client.recvfrom(1024) - message = Message() + message = messaging.MessageManager() message.income_raw = data message.process_message() if message.content: @@ -202,7 +199,7 @@ class Server: def send_starttime(self, copter, dt=0): timenow = self.time_now() print('Now:', time.ctime(timenow), "+ dt =", dt) - copter.send(Message.create_simple_message("starttime", {"time": str(timenow + dt)})) # TODO change start_on + copter.send_message("starttime", {"time": str(timenow + dt)}) # TODO change start_on # TODO all commands as tasks with timestamp and priority @@ -224,9 +221,7 @@ def requires_any_connected(f): return wrapper -class Client: - resume_queue = True - +class Client(messaging.ConnectionManager): clients = {} on_connect = None # Use as callback functions @@ -234,48 +229,24 @@ class Client: on_disconnect = None def __init__(self, ip): - self.selector = None - self.socket = None - self.addr = None - - self._recv_buffer = b"" - self._send_buffer = b"" - - self._send_queue = collections.deque() - self._received_queue = collections.deque() - self._request_queue = collections.OrderedDict() - - self._send_lock = threading.Lock() - self._request_lock = threading.Lock() - + super(Client, self).__init__() self.copter_id = None + self.connected = False self.clients[ip] = self - self.connected = False - - def _set_selector_events_mask(self, mode): - """Set selector to listen for events: mode is 'r', 'w', 'rw'.""" - if mode == "r": - events = selectors.EVENT_READ - elif mode == "w": - events = selectors.EVENT_WRITE - elif mode == "rw": - events = selectors.EVENT_READ | selectors.EVENT_WRITE - else: - raise ValueError("Invalid events mask mode {}.".format(mode)) - self.selector.modify(self.socket, events, data=self) + @staticmethod + def get_by_id(copter_id): + for client in Client.clients.values(): + if client.copter_id == copter_id: + return client def connect(self, client_selector, client_socket, client_addr): logging.info("Client connected") - if not Client.resume_queue: + if not self.resume_queue: self._send_queue = collections.deque() - self.selector = client_selector - self.socket = client_socket - self.addr = client_addr - - self._set_selector_events_mask('rw') + super(Client, self).connect(client_selector, client_socket, client_addr) self.connected = True @@ -291,175 +262,18 @@ class Client: Client.on_first_connect(self) def close(self): - logging.info("Closing connection to {}".format(self.addr)) - try: - self.selector.unregister(self.socket) - except AttributeError: - pass - except Exception as error: - logging.error("{}: Error during selector unregistering: {}".format(self.addr, error)) - finally: - self.selector = None + self.connected = False - try: - self.socket.close() - except AttributeError: - pass - except OSError as error: - logging.error("{}: Error during socket closing: {}".format(self.addr, error)) - finally: - self.socket = None + if Client.on_disconnect: + Client.on_disconnect(self) - def process_events(self, mask): - print(self.socket, self.selector, mask) - if mask & selectors.EVENT_READ: - self.read() - if mask & selectors.EVENT_WRITE: - self.write() + super(Client, self).close() - with self._send_lock: - if (not self._send_buffer) and self._send_queue: - message = self._send_queue.popleft() - self._send_buffer += message - # self._set_selector_events_mask('rw') - - def process_received(self): - if self._received_queue: - if self._received_queue[-1].content: - message = self._received_queue.pop() - message_type = message.jsonheader["message-type"] - logging.debug("Received message! Header: {}, content: {}".format(message.jsonheader, message.content)) - if message_type == "message": - pass - elif message_type == "response": - request_id, requested_value = message.content["requst_id"], message.content["requested_value"] - with self._request_lock: - for key, value in self._request_queue.items(): - if (key == request_id) and (value.requested_value == requested_value): - request = self._request_queue.pop(key) - request.value = message.content["value"] - logging.debug( - "Request successfully closed with value {}".format(message.content["value"]) - ) - request.callback(request.value, *request.callback_args, **request.callback_kwargs) - break - else: - logging.warning("Unexpected request response!") - elif message_type == "request": - pass - - def read(self): - self._read() - if self._recv_buffer: - if not self._received_queue or (self._received_queue[0].content is not None): - self._received_queue.appendleft(Message()) - - self._received_queue[0].income_raw += self._recv_buffer - self._received_queue[0].process_message() - - if self._received_queue[0].content and self._received_queue[0].income_raw: - self._recv_buffer = self._received_queue[0].income_raw + self._recv_buffer - self._received_queue[0].income_raw = b'' - - self.process_received() - - def write(self): - self._write() - # if not (self._send_buffer and self._send_queue): - # self._set_selector_events_mask("r") - - def _read(self): - try: - data = self.socket.recv(Server.BUFFER_SIZE) - except BlockingIOError: - # Resource temporarily unavailable (errno EWOULDBLOCK) - pass - else: - if data: - self._recv_buffer += data - logging.debug("Received {} from {}".format(data, self.addr)) - else: - logging.warning("Connection to {} lost!".format(self.addr)) - self.connected = False - if not Client.resume_queue: - self._recv_buffer = b'' - - if Client.on_disconnect: - Client.on_disconnect(self) - - raise RuntimeError("Peer closed.") - - def _write(self): - if self._send_buffer: - try: - sent = self.socket.send(self._send_buffer) - except BlockingIOError: - # Resource temporarily unavailable (errno EWOULDBLOCK) - pass - except socket.error as error: - logging.warning("Attempt to send message {} to {} failed due error: {}".format( - self._send_buffer, self.addr, error)) - - if not Client.resume_queue: - self._send_buffer = b'' - - raise error - else: - print(sent) - logging.debug("Sent {} to {}".format(self._send_buffer[:sent], self.addr)) - self._send_buffer = self._send_buffer[sent:] - print(self._send_buffer) - time.sleep(0.1) @requires_connect - def send(self, *messages): - for message in messages: - with self._send_lock: - self._send_queue.append(message) - - def send_message(self, command, args=None): - self.send(Message.create_simple_message(command, args)) - - @staticmethod - @requires_any_connected - def broadcast(message, force_all=False): - for client in Client.clients.values(): - if client.connected or force_all: - client.send(message) - - @classmethod - @requires_any_connected - def broadcast_message(cls, command, args=None, force_all=False): - cls.broadcast(Message.create_simple_message(command, args), force_all) - - def get_response(self, requested_value, callback, request_args=None, # timeout=30, - callback_args=(), callback_kwargs: dict=None): - if request_args is None: - request_args = {} - if callback_kwargs is None: - callback_kwargs = {} - - request_id = str(random.randint(0, 9999)).zfill(4) - with self._request_lock: - self._request_queue[request_id] = PendingRequest( - requested_value=requested_value, - value=None, - # expires_on=Server.time_now()+timeout, - callback=callback, - callback_args=callback_args, - callback_kwargs=callback_kwargs, - ) - self.send(Message.create_request(requested_value, request_id, request_args)) - - def send_file(self, filepath, dest_filepath): # clever_restart=False - try: - with open(filepath, 'rb') as file: - data = file.read() - except OSError as error: - logging.warning("File can not be opened due error: ".format(error)) - else: - logging.info("Sending file {} to {} (as: {})".format(filepath, self.addr, dest_filepath)) - self.send(Message.create_message(data, "binary", "filetransfer", "binary", {"path": dest_filepath})) + def _send(self, data): + super(Client, self)._send(data) + logging.debug("Queued data to send: {}".format(data)) def send_config_options(self, *options: ConfigOption): logging.info("Sending config options: {} to {}".format(options, self.addr)) @@ -471,10 +285,16 @@ class Client: self.send_message("config_reload") @staticmethod - def get_by_id(copter_id): - for copter in Client.clients.values(): - if copter.copter_id == copter_id: - return copter + @requires_any_connected + def broadcast(message, force_all=False): + for client in Client.clients.values(): + if client.connected or force_all: + client.send(message) + + @classmethod + @requires_any_connected + def broadcast_message(cls, command, args=None, force_all=False): + cls.broadcast(messaging.MessageManager.create_simple_message(command, args), force_all) if __name__ == '__main__': diff --git a/messaging_lib.py b/messaging_lib.py index c3fb1f4..561b6ce 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -2,9 +2,22 @@ import io import sys import json import struct +import random +import logging +import threading +import collections + +try: + import selectors +except: + import selectors2 as selectors + +PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on", + "callback", "callback_args", "callback_kwargs" + ]) -class Message: +class MessageManager: def __init__(self): self.income_raw = b"" self._jsonheader_len = None @@ -62,6 +75,15 @@ class Message: message = cls.create_message(cls._json_encode(contents), "json", "request") return message + @classmethod + def create_response(cls, requested_value, request_id, value): + contents = {"requested_value": requested_value, + "requst_id": request_id, + "value": value, + } + message = cls.create_message(cls._json_encode(contents), "json", "response") + return message + def _process_protoheader(self): header_len = 2 if len(self.income_raw) >= header_len: @@ -106,3 +128,238 @@ class Message: if self.jsonheader: if self.content is None: self._process_content() + + +def message_callback(string_command): + def inner(f): + ConnectionManager.requests_callbacks.update({string_command: f}) + + def wrapper(*args, **kwargs): + return f(*args, **kwargs) + return wrapper + return inner + + +class ConnectionManager(object): + messages_callbacks = {} + requests_callbacks = {} + + def __init__(self): + self.selector = None + self.socket = None + self.addr = None + + self.selector = None + self.socket = None + self.addr = None + + self._recv_buffer = b"" + self._send_buffer = b"" + + self._send_queue = collections.deque() + self._received_queue = collections.deque() + self._request_queue = collections.OrderedDict() + + self._send_lock = threading.Lock() + self._request_lock = threading.Lock() + + self.BUFFER_SIZE = 1024 + self.resume_queue = True + + def _set_selector_events_mask(self, mode): + """Set selector to listen for events: mode is 'r', 'w', 'rw'.""" + if mode == "r": + events = selectors.EVENT_READ + elif mode == "w": + events = selectors.EVENT_WRITE + elif mode == "rw": + events = selectors.EVENT_READ | selectors.EVENT_WRITE + else: + raise ValueError("Invalid events mask mode {}.".format(mode)) + self.selector.modify(self.socket, events, data=self) + + def connect(self, client_selector, client_socket, client_addr): + self.selector = client_selector + self.socket = client_socket + self.addr = client_addr + + self._set_selector_events_mask('rw') + + def close(self): + logging.info("Closing connection to {}".format(self.addr)) + try: + self.selector.unregister(self.socket) + except AttributeError: + pass + except Exception as error: + logging.error("{}: Error during selector unregistering: {}".format(self.addr, error)) + finally: + self.selector = None + + try: + self.socket.close() + except AttributeError: + pass + except OSError as error: + logging.error("{}: Error during socket closing: {}".format(self.addr, error)) + finally: + self.socket = None + + def process_events(self, mask): + print(self.socket, self.selector, mask) + if mask & selectors.EVENT_READ: + self.read() + if mask & selectors.EVENT_WRITE: + self.write() + + def read(self): + self._read() + if self._recv_buffer: + if not self._received_queue or (self._received_queue[0].content is not None): + self._received_queue.appendleft(MessageManager()) + + self._received_queue[0].income_raw += self._recv_buffer + self._received_queue[0].process_message() + + if self._received_queue[0].content and self._received_queue[0].income_raw: + self._recv_buffer = self._received_queue[0].income_raw + self._recv_buffer + self._received_queue[0].income_raw = b'' + + self.process_received() + + def _read(self): + try: + data = self.socket.recv(self.BUFFER_SIZE) + except io.BlockingIOError: + # Resource temporarily unavailable (errno EWOULDBLOCK) + pass + else: + if data: + self._recv_buffer += data + logging.debug("Received {} from {}".format(data, self.addr)) + else: + logging.warning("Connection to {} lost!".format(self.addr)) + if not self.resume_queue: + self._recv_buffer = b'' + + raise RuntimeError("Peer closed.") + + def process_received(self): + if self._received_queue: + if self._received_queue[-1].content: + message = self._received_queue.pop() + message_type = message.jsonheader["message-type"] + logging.debug("Received message! Header: {}, content: {}".format(message.jsonheader, message.content)) + if message_type == "message": + self._process_message(message) + elif message_type == "response": + self._process_response(message) + elif message_type == "request": + self._process_request(message) + elif message_type == "filetransfer": + self._process_filetransfer(message) + + def _process_message(self, message): + command = message.content["command"] + args = message.content["args"] + self.messages_callbacks[command](**args) + + def _process_request(self, message): + command = message.content["requested_value"] + request_id = message.content["request_id"] + args = message.content["args"] + value = self.requests_callbacks[command](**args) + self._send_response(command, request_id, value) + + def _process_response(self, message): + request_id, requested_value = message.content["requst_id"], message.content["requested_value"] + with self._request_lock: + for key, value in self._request_queue.items(): + if (key == request_id) and (value.requested_value == requested_value): + request = self._request_queue.pop(key) + request.value = message.content["value"] + logging.debug( + "Request successfully closed with value {}".format(message.content["value"]) + ) + request.callback(request.value, *request.callback_args, **request.callback_kwargs) + break + else: + logging.warning("Unexpected response!") + + def _process_filetransfer(self, message): + if message.jsonheader["content-type"] == "binary": + filepath = message.jsonheader["filepath"] + try: + with open(filepath, 'wb') as f: + f.write(message.content) + except OSError as error: + logging.warning("File can not be written due error: ".format(error)) + else: + logging.info("File {} successfully received ".format(filepath)) + + def write(self): + with self._send_lock: + if (not self._send_buffer) and self._send_queue: + message = self._send_queue.popleft() + self._send_buffer += message + if self._send_buffer: + self._write() + + def _write(self): + try: + sent = self.socket.send(self._send_buffer) + except io.BlockingIOError: + # Resource temporarily unavailable (errno EWOULDBLOCK) + pass + except Exception as error: + logging.warning("Attempt to send message {} to {} failed due error: {}".format( + self._send_buffer, self.addr, error)) + + if not self.resume_queue: + self._send_buffer = b'' + + raise error + else: + logging.debug("Sent {} to {}".format(self._send_buffer[:sent], self.addr)) + self._send_buffer = self._send_buffer[sent:] + + def _send(self, data): + with self._send_lock: + self._send_queue.append(data) + + def get_response(self, requested_value, callback, request_args=None, # timeout=30, + callback_args=(), callback_kwargs=None): + if request_args is None: + request_args = {} + if callback_kwargs is None: + callback_kwargs = {} + + request_id = str(random.randint(0, 9999)).zfill(4) + with self._request_lock: + self._request_queue[request_id] = PendingRequest( + requested_value=requested_value, + value=None, + # expires_on=Server.time_now()+timeout, #TODO + callback=callback, + callback_args=callback_args, + callback_kwargs=callback_kwargs, + ) + self._send(MessageManager.create_request(requested_value, request_id, request_args)) + + def send_message(self, command, args=None): + self._send(MessageManager.create_simple_message(command, args)) + + def _send_response(self, requested_value, request_id, value): + self._send(MessageManager.create_response(requested_value, request_id, value)) + + def send_file(self, filepath, dest_filepath): # clever_restart=False + try: + with open(filepath, 'rb') as f: + data = f.read() + except OSError as error: + logging.warning("File can not be opened due error: ".format(error)) + else: + logging.info("Sending file {} to {} (as: {})".format(filepath, self.addr, dest_filepath)) + self._send(MessageManager.create_message( + data, "binary", "filetransfer", "binary", {"filepath": dest_filepath} + )) \ No newline at end of file From ab49fae6407a7ea62f5ccb58fd697bc5d8444047 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Tue, 9 Apr 2019 09:18:19 +0300 Subject: [PATCH 07/47] Fixes --- Drone/client.py | 9 +++++++-- messaging_lib.py | 4 ++-- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index 241a0d3..c8052b6 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -110,6 +110,7 @@ class Client: raise KeyboardInterrupt else: logging.info("Connection to server successful!") + self._connect() break if attempt_count >= attempt_limit: @@ -121,7 +122,9 @@ class Client: self.connected = True self.client_socket.setblocking(False) events = selectors.EVENT_READ | selectors.EVENT_WRITE - self.selector.register(self.client_socket, events, data=None) + self.selector.register(self.client_socket, events, data=self.server_connection) + self.server_connection.connect(self.selector, self.client_socket, (self.server_host, self.server_port)) + def broadcast_bind(self): broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) @@ -153,12 +156,14 @@ class Client: if events: for key, mask in events: if key.data is None: + pass + else: connection = key.data connection.process_events(mask) if not self.selector.get_map(): logging.warning("No active connections left!") - self.reconnect() + #self.reconnect() except (KeyboardInterrupt, errno.EINTR): logging.critical("Caught interrupt, exiting!") finally: diff --git a/messaging_lib.py b/messaging_lib.py index 561b6ce..0dc5daa 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -69,7 +69,7 @@ class MessageManager: if args is None: args = {} contents = {"requested_value": requested_value, - "requst_id": request_id, + "request_id": request_id, "args": args, } message = cls.create_message(cls._json_encode(contents), "json", "request") @@ -78,7 +78,7 @@ class MessageManager: @classmethod def create_response(cls, requested_value, request_id, value): contents = {"requested_value": requested_value, - "requst_id": request_id, + "request_id": request_id, "value": value, } message = cls.create_message(cls._json_encode(contents), "json", "response") From ec05257e4e31985cc3d5d4ac7ba5ca4484f729b8 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Tue, 9 Apr 2019 10:38:35 +0300 Subject: [PATCH 08/47] Fixing client reconnection --- Drone/client.py | 62 ++++++++++++++++++++++++++++++++---------------- messaging_lib.py | 20 ++++++++++++---- 2 files changed, 56 insertions(+), 26 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index c8052b6..4f9ab28 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -27,6 +27,7 @@ logging.basicConfig( # TODO all prints as logs class Client: + active_client = None def __init__(self, config_path="client_config.ini"): self.selector = selectors.DefaultSelector() self.client_socket = None @@ -45,6 +46,8 @@ class Client: self.config = ConfigParser.ConfigParser() self.load_config() + Client.active_client = self + def load_config(self): self.config.read(self.config_path) @@ -91,7 +94,17 @@ class Client: 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(self, timeout=2, attempt_limit=5): + def start(self): + try: + while True: + self._reconnect() + self._process_connections() + + except (KeyboardInterrupt, errno.EINTR): + logging.critical("Caught interrupt, exiting!") + self.selector.close() + + def _reconnect(self, timeout=2, attempt_limit=5): logging.info("Trying to connect to {}:{} ...".format(self.server_host, self.server_port)) attempt_count = 0 while not self.connected: @@ -118,12 +131,14 @@ class Client: self.broadcast_bind() attempt_count = 0 + def _connect(self): self.connected = True self.client_socket.setblocking(False) events = selectors.EVENT_READ | selectors.EVENT_WRITE self.selector.register(self.client_socket, events, data=self.server_connection) self.server_connection.connect(self.selector, self.client_socket, (self.server_host, self.server_port)) + self._process_connections() def broadcast_bind(self): @@ -149,29 +164,34 @@ class Client: finally: broadcast_client.close() - def mainloop(self): - try: - while True: - events = self.selector.select(timeout=1) - if events: - for key, mask in events: - if key.data is None: - pass - else: - connection = key.data + def _process_connections(self): + while True: + events = self.selector.select(timeout=1) + if events: + for key, mask in events: + if key.data is None: + pass + else: + connection = key.data + try: connection.process_events(mask) + except Exception as error: + logging.error( + "Exception {} occurred for {}! Resetting connection!".format(error, connection.addr) + ) + self.server_connection.close() + self.connected = False + break - if not self.selector.get_map(): - logging.warning("No active connections left!") - #self.reconnect() - except (KeyboardInterrupt, errno.EINTR): - logging.critical("Caught interrupt, exiting!") - finally: - self.selector.close() + if not self.selector.get_map(): + logging.warning("No active connections left!") + return + +@messaging.request_callback("id") +def response_id(): + return Client.active_client.client_id -# TODO class connection if __name__ == "__main__": client = Client() - client.reconnect() - client.mainloop() + client.start() diff --git a/messaging_lib.py b/messaging_lib.py index 0dc5daa..392c0f6 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -130,7 +130,7 @@ class MessageManager: self._process_content() -def message_callback(string_command): +def request_callback(string_command): def inner(f): ConnectionManager.requests_callbacks.update({string_command: f}) @@ -206,7 +206,6 @@ class ConnectionManager(object): self.socket = None def process_events(self, mask): - print(self.socket, self.selector, mask) if mask & selectors.EVENT_READ: self.read() if mask & selectors.EVENT_WRITE: @@ -262,14 +261,25 @@ class ConnectionManager(object): def _process_message(self, message): command = message.content["command"] args = message.content["args"] - self.messages_callbacks[command](**args) + try: + self.messages_callbacks[command](**args) + except KeyError: + logging.warning("Command {} does not exist!".format(command)) + except Exception as error: + logging.error("Error during command {} execution: {}".format(command, error)) def _process_request(self, message): command = message.content["requested_value"] request_id = message.content["request_id"] args = message.content["args"] - value = self.requests_callbacks[command](**args) - self._send_response(command, request_id, value) + try: + value = self.requests_callbacks[command](**args) + except KeyError: + logging.warning("Request {} does not exist!".format(command)) + except Exception as error: # TODO send response error\cancel + logging.error("Error during command {} execution: {}".format(command, error)) + else: + self._send_response(command, request_id, value) def _process_response(self, message): request_id, requested_value = message.content["requst_id"], message.content["requested_value"] From db04cc794a964ba2aab312aa1665a558f76de633 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Tue, 9 Apr 2019 10:41:32 +0300 Subject: [PATCH 09/47] fix "requst" --- messaging_lib.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/messaging_lib.py b/messaging_lib.py index 392c0f6..c43c592 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -282,7 +282,7 @@ class ConnectionManager(object): self._send_response(command, request_id, value) def _process_response(self, message): - request_id, requested_value = message.content["requst_id"], message.content["requested_value"] + request_id, requested_value = message.content["request_id"], message.content["requested_value"] with self._request_lock: for key, value in self._request_queue.items(): if (key == request_id) and (value.requested_value == requested_value): From b9008204eebf9deda14ddfe2fb15914c239075be Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Tue, 9 Apr 2019 10:49:18 +0300 Subject: [PATCH 10/47] Fix copter id getter --- Server/server.py | 3 ++- messaging_lib.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Server/server.py b/Server/server.py index 6dc972a..ec9e794 100644 --- a/Server/server.py +++ b/Server/server.py @@ -256,8 +256,9 @@ class Client(messaging.ConnectionManager): if self.on_connect: self.on_connect(self) - def _got_id(self): + def _got_id(self, value): logging.info("Got copter id: {} for client {}".format(self.copter_id, self.addr)) + self.copter_id = value if Client.on_first_connect: Client.on_first_connect(self) diff --git a/messaging_lib.py b/messaging_lib.py index c43c592..f4586c0 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -164,7 +164,7 @@ class ConnectionManager(object): self._request_lock = threading.Lock() self.BUFFER_SIZE = 1024 - self.resume_queue = True + self.resume_queue = False def _set_selector_events_mask(self, mode): """Set selector to listen for events: mode is 'r', 'w', 'rw'.""" From 733b4e84c134196d590b9dcfb589aee391388472 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Tue, 9 Apr 2019 23:11:45 +0300 Subject: [PATCH 11/47] Fix request --- Server/server.py | 4 ++-- messaging_lib.py | 29 +++++++++++++++++++++++++---- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/Server/server.py b/Server/server.py index ec9e794..9d7a562 100644 --- a/Server/server.py +++ b/Server/server.py @@ -257,7 +257,7 @@ class Client(messaging.ConnectionManager): self.on_connect(self) def _got_id(self, value): - logging.info("Got copter id: {} for client {}".format(self.copter_id, self.addr)) + logging.info("Got copter id: {} for client {}".format(value, self.addr)) self.copter_id = value if Client.on_first_connect: Client.on_first_connect(self) @@ -290,7 +290,7 @@ class Client(messaging.ConnectionManager): def broadcast(message, force_all=False): for client in Client.clients.values(): if client.connected or force_all: - client.send(message) + client._send(message) @classmethod @requires_any_connected diff --git a/messaging_lib.py b/messaging_lib.py index f4586c0..ab3f375 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -13,7 +13,8 @@ except: import selectors2 as selectors PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on", - "callback", "callback_args", "callback_kwargs" + "callback", "callback_args", "callback_kwargs", + #"obj", ]) @@ -286,12 +287,29 @@ class ConnectionManager(object): with self._request_lock: for key, value in self._request_queue.items(): if (key == request_id) and (value.requested_value == requested_value): + print(54) request = self._request_queue.pop(key) - request.value = message.content["value"] + print(request) + value = message.content["value"] + print(45) logging.debug( "Request successfully closed with value {}".format(message.content["value"]) ) - request.callback(request.value, *request.callback_args, **request.callback_kwargs) + ''' + if request.obj: + obj = request.obj + f = request.callback + + obj.f(request.value, *request.callback_args, **request.callback_kwargs) + + else: + f = request.callback + f(request.value, *request.callback_args, **request.callback_kwargs) + ''' + f = request.callback + print(f) + f(value, *request.callback_args, **request.callback_kwargs) + break else: logging.warning("Unexpected response!") @@ -311,6 +329,7 @@ class ConnectionManager(object): with self._send_lock: if (not self._send_buffer) and self._send_queue: message = self._send_queue.popleft() + print(self._send_queue) self._send_buffer += message if self._send_buffer: self._write() @@ -332,13 +351,14 @@ class ConnectionManager(object): else: logging.debug("Sent {} to {}".format(self._send_buffer[:sent], self.addr)) self._send_buffer = self._send_buffer[sent:] + print(self._send_buffer) def _send(self, data): with self._send_lock: self._send_queue.append(data) def get_response(self, requested_value, callback, request_args=None, # timeout=30, - callback_args=(), callback_kwargs=None): + callback_args=(), callback_kwargs=None, obj=None): if request_args is None: request_args = {} if callback_kwargs is None: @@ -353,6 +373,7 @@ class ConnectionManager(object): callback=callback, callback_args=callback_args, callback_kwargs=callback_kwargs, + #obj=obj ) self._send(MessageManager.create_request(requested_value, request_id, request_args)) From efa081efc32555025fddb6e35c7ea2bb6edc748f Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Tue, 16 Apr 2019 23:03:24 +0300 Subject: [PATCH 12/47] Minimal operateable server+copter client. Many improvements, including architecture improvements --- Drone/client.py | 77 ++++++----- Drone/copter_client.py | 282 +++++++--------------------------------- Drone/play_animation.py | 5 +- Server/server.py | 39 +++--- Server/server_qt.py | 4 +- messaging_lib.py | 84 ++++++------ 6 files changed, 165 insertions(+), 326 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index 4f9ab28..418ea07 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -5,17 +5,17 @@ import socket import struct import logging import collections -import selectors2 as selectors import ConfigParser +import selectors2 as selectors + from contextlib import closing -import os,sys,inspect +import os,sys,inspect # Add parent dir to PATH to import messaging_lib current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) parent_dir = os.path.dirname(current_dir) sys.path.insert(0, parent_dir) import messaging_lib as messaging -random.seed() logging.basicConfig( # TODO all prints as logs level=logging.DEBUG, # INFO @@ -25,9 +25,11 @@ logging.basicConfig( # TODO all prints as logs logging.StreamHandler() ]) +ConfigOption = collections.namedtuple("ConfigOption", ["section", "option", "value"]) -class Client: - active_client = None +active_client = None # maybe needs to be refactored + +class Client(object): def __init__(self, config_path="client_config.ini"): self.selector = selectors.DefaultSelector() self.client_socket = None @@ -46,7 +48,8 @@ class Client: self.config = ConfigParser.ConfigParser() self.load_config() - Client.active_client = self + global active_client + active_client = self def load_config(self): self.config.read(self.config_path) @@ -59,29 +62,27 @@ class Client: self.NTP_HOST = self.config.get('NTP', 'host') self.NTP_PORT = self.config.getint('NTP', 'port') - files_directory = self.config.get('FILETRANSFER', 'files_directory') - - #FRAME_ID = self.config.get('COPTERS', 'frame_id') # TODO in play_animation - #self.TAKEOFF_HEIGHT = self.config.getfloat('COPTERS', 'takeoff_height') - #self.TAKEOFF_TIME = self.config.getfloat('COPTERS', 'takeoff_time') - #self.RFP_TIME = self.config.getfloat('COPTERS', 'reach_first_point_time') - #self.SAFE_TAKEOFF = self.config.getboolean('COPTERS', 'safe_takeoff') - - #self.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common') - #self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common') - #self.X0 = self.config.getfloat('PRIVATE', 'x0') - #self.Y0 = self.config.getfloat('PRIVATE', 'y0') - - #self.USE_LEDS = config.getboolean('PRIVATE', 'use_leds') - #play_animation.USE_LEDS = USE_LEDS # TODO in copter_client + self.files_directory = self.config.get('FILETRANSFER', 'files_directory') self.client_id = self.config.get('PRIVATE', 'id') if self.client_id == 'default': self.client_id = 'copter' + str(random.randrange(9999)).zfill(4) - #write_to_config('PRIVATE', 'id', client_id) + self.write_config(False, 'PRIVATE', 'id', self.client_id) elif self.client_id == '/hostname': self.client_id = socket.gethostname() + def rewrite_config(self): + with open(self.config_path, 'w') as file: + self.config.write(file) + + def write_config(self, reload_config=True, *config_options): + for config_option in config_options: + self.config.set(config_option.section, config_option.option, config_option.value) + self.rewrite_config() + + if reload_config: + self.load_config() + @staticmethod def get_ntp_time(ntp_host, ntp_port): NTP_PACKET_FORMAT = "!12I" @@ -94,13 +95,20 @@ class Client: unpacked = struct.unpack(NTP_PACKET_FORMAT, msg[0:struct.calcsize(NTP_PACKET_FORMAT)]) return unpacked[10] + float(unpacked[11]) / 2 ** 32 - NTP_DELTA + def time_now(self): + if self.USE_NTP: + timenow = self.get_ntp_time(self.NTP_HOST, self.NTP_PORT) + else: + timenow = time.time() + return timenow + def start(self): try: while True: self._reconnect() self._process_connections() - except (KeyboardInterrupt, errno.EINTR): + except (KeyboardInterrupt, InterruptedError): logging.critical("Caught interrupt, exiting!") self.selector.close() @@ -155,11 +163,12 @@ class Client: logging.info("Received broadcast message {} from {}".format(message.content, addr)) if message.content["command"] == "server_ip": args = message.content["args"] - self.server_host = args["host"] self.server_port = int(args["port"]) + self.server_host = args["host"] + self.write_config(False, + ConfigOption("SERVER", "port", self.server_port), + ConfigOption("SERVER", "host", self.server_host)) logging.info("Binding to new IP: {}:{}".format(self.server_host, self.server_port)) - #write_to_config("SERVER", "port", port) - #write_to_config("SERVER", "host", host) # TODO break finally: broadcast_client.close() @@ -187,10 +196,20 @@ class Client: logging.warning("No active connections left!") return -@messaging.request_callback("id") -def response_id(): - return Client.active_client.client_id +@messaging.request_callback("id") +def _response_id(): + return active_client.client_id + +@messaging.request_callback("time") +def _response_time(): + return active_client.time_now() + +@messaging.message_callback("config_write") +def _command_config_write(*args, **kwargs): + options = [ConfigOption(**raw_option) for raw_option in kwargs["options"]] + logging.info("Writing config options: {}".format(options)) + active_client.write_config(kwargs["reload"], *options) if __name__ == "__main__": client = Client() diff --git a/Drone/copter_client.py b/Drone/copter_client.py index d7c8fd6..94533a7 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -1,252 +1,64 @@ -from __future__ import print_function import os -import sys -import socket -import struct -import random import time -import errno -import json -import logging -import threading -import ConfigParser - import rospy -import pause - -from .. import messaging_lib from FlightLib import FlightLib from FlightLib import LedLib +import client +import messaging_lib as messaging import play_animation -def recive_file(filename): - print("Receiving 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_message(data.decode("UTF-8"))[0] == "/endoffile": - print("File received") - break - file.write(data) - else: - break +def + +class CopterClient(client.Client): + def load_config(self): + super(CopterClient, self).load_config() + self.FRAME_ID = self.config.get('COPTERS', 'frame_id') # TODO in play_animation + self.TAKEOFF_HEIGHT = self.config.getfloat('COPTERS', 'takeoff_height') + self.TAKEOFF_TIME = self.config.getfloat('COPTERS', 'takeoff_time') + self.RFP_TIME = self.config.getfloat('COPTERS', 'reach_first_point_time') + self.SAFE_TAKEOFF = self.config.getboolean('COPTERS', 'safe_takeoff') + + self.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common') + self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common') + self.X0 = self.config.getfloat('PRIVATE', 'x0') + self.Y0 = self.config.getfloat('PRIVATE', 'y0') + + self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds') + play_animation.USE_LEDS = self.USE_LEDS + + def start(self): + super(CopterClient, self).start() + rospy.init_node('Swarm_client', anonymous=True) + if self.USE_LEDS: + LedLib.init_led() -def animation_player(running_event, stop_event): - print("Animation thread activated") - frames = play_animation.read_animation_file() - if not frames: - logging.error("Animation is empty, shutting down animation player") - return - - delay_time = 0.125 - - print("Takeoff") - play_animation.takeoff(z=TAKEOFF_HEIGHT, safe_takeoff=SAFE_TAKEOFF) - takeoff_time = starttime + TAKEOFF_TIME - dt = takeoff_time - time.time() - print("Wait until takeoff " + str(dt) + "s: " + time.ctime(takeoff_time)) - pause.until(takeoff_time) - - print("Reach first point") - play_animation.reach_frame(frames[0], x0=X0+X0_COMMON, y0=Y0+Y0_COMMON) #Reach first point at the same time with others - rfp_time = takeoff_time + RFP_TIME - dt = rfp_time - time.time() - print("Wait reaching first point " + str(dt) + "s: " + time.ctime(rfp_time)) - pause.until(rfp_time) - - next_frame_time = rfp_time - print("Start animation at " + str(time.time())) - for frame in frames: - running_event.wait() - play_animation.animate_frame(frame, x0=X0+X0_COMMON, y0=Y0+Y0_COMMON) - next_frame_time += delay_time - if stop_event.is_set(): - running_animation_event.clear() - break - #rate.sleep() - pause.until(next_frame_time) - 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)) - 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() - print("Stopping animation") -# animation_thread.join() - - -def selfcheck(): +@messaging.request_callback("selfcheck") +def _response_selfcheck(): return FlightLib.selfcheck() +@messaging.request_callback("batt_voltage") +def _response_batt(): + return FlightLib.get_telemetry('body').voltage -def write_to_config(section, option, value): - config.set(section, option, value) - with open(CONFIG_PATH, 'w') as file: # TODO as separate function - config.write(file) +@messaging.request_callback("cell_voltage") +def _response_cell(): + return FlightLib.get_telemetry('body').cell_voltage + +@messaging.message_callback("service_restart") +def _message_service_restart(*args, **kwargs): + os.system("systemctl restart"+kwargs["name"]) + +@messaging.message_callback("led_test") +def _command_config_write(*args, **kwargs): + LedLib.chase(255, 255, 255) + time.sleep(2) + LedLib.off() -def load_config(): - global config, CONFIG_PATH - global broadcast_port, port, host, BUFFER_SIZE - global USE_NTP, NTP_HOST, NTP_PORT - global files_directory, animation_file - global FRAME_ID, TAKEOFF_HEIGHT, TAKEOFF_TIME, SAFE_TAKEOFF, RFP_TIME - global USE_LEDS, COPTER_ID - global X0, X0_COMMON, Y0, Y0_COMMON - CONFIG_PATH = "client_config.ini" - config = ConfigParser.ConfigParser() - config.read(CONFIG_PATH) - - broadcast_port = config.getint('SERVER', 'broadcast_port') - port = config.getint('SERVER', 'port') - host = config.get('SERVER', 'host') - BUFFER_SIZE = config.getint('SERVER', 'buffer_size') - USE_NTP = config.getboolean('NTP', 'use_ntp') - NTP_HOST = config.get('NTP', 'host') - NTP_PORT = config.getint('NTP', 'port') - - files_directory = config.get('FILETRANSFER', 'files_directory') - animation_file = config.get('FILETRANSFER', 'animation_file') - - FRAME_ID = config.get('COPTERS', 'frame_id') # TODO in play_animation - TAKEOFF_HEIGHT = config.getfloat('COPTERS', 'takeoff_height') - TAKEOFF_TIME = config.getfloat('COPTERS', 'takeoff_time') - RFP_TIME = config.getfloat('COPTERS', 'reach_first_point_time') - SAFE_TAKEOFF = config.getboolean('COPTERS', 'safe_takeoff') - - X0_COMMON = config.getfloat('COPTERS', 'x0_common') - Y0_COMMON = config.getfloat('COPTERS', 'y0_common') - X0 = config.getfloat('PRIVATE', 'x0') - Y0 = config.getfloat('PRIVATE', 'y0') - - USE_LEDS = config.getboolean('PRIVATE', 'use_leds') - play_animation.USE_LEDS = USE_LEDS - - COPTER_ID = config.get('PRIVATE', 'id') - if COPTER_ID == 'default': - COPTER_ID = 'copter' + str(random.randrange(9999)).zfill(4) - write_to_config('PRIVATE', 'id', COPTER_ID) - elif COPTER_ID == '/hostname': - COPTER_ID = socket.gethostname() - -load_config() - -rospy.init_node('Swarm_client', anonymous=True) -if USE_LEDS: - LedLib.init_led() - -print("Client started on copter:", COPTER_ID) -if USE_NTP: - 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_message(message) - print("Command from server:", command, args) - if command == "writefile": - recive_file(args['filename']) - if bool(args['clever_restart']): - os.system("systemctl restart clever") - elif command == 'config_write': - write_to_config(args['section'], args['option'], args['value']) - elif command == 'config_reload': - load_config() - elif command == "starttime": - global starttime - starttime = float(args['time']) - print("Starting on:", time.ctime(starttime)) - dt = starttime - time.time() - if USE_NTP: - 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(safe_takeoff=SAFE_TAKEOFF) - elif command == 'pause': - pause_animation() - elif command == 'resume': - resume_animation() - elif command == 'stop': - stop_animation() - FlightLib.interrupt() - elif command == 'land': - play_animation.land() - elif command == 'disarm': - FlightLib.arming(False) - elif command == 'led_test': - LedLib.fill(255, 255, 255) - time.sleep(2) - LedLib.off() - - elif command == 'request': - request_target = args['value'] - print("Got request for:", request_target) - response = "" - if request_target == 'test': - response = "test_success" - elif request_target == 'id': - response = COPTER_ID - elif request_target == 'selfcheck': - check = FlightLib.selfcheck() - response = check if check else "OK" - elif request_target == 'batt_voltage': - response = FlightLib.get_telemetry('body').voltage - elif request_target == 'cell_voltage': - response = FlightLib.get_telemetry('body').cell_voltage - - send_all(bytes(form_message("response", - {"status": "ok", "value": response, "value_name": str(request_target)}))) - 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() +if __name__ == "__main__": + copter_client = CopterClient() + copter_client.start() diff --git a/Drone/play_animation.py b/Drone/play_animation.py index 5bd9a2e..2f554c2 100644 --- a/Drone/play_animation.py +++ b/Drone/play_animation.py @@ -16,9 +16,8 @@ def takeoff(z=1.5, safe_takeoff=True, timeout=5000): if USE_LEDS: LedLib.wipe_to(255, 0, 0) if safe_takeoff: - FlightLib.takeoff(z=z, wait=True, timeout_takeoff = timeout, emergency_land=True) # TODO dont forget change back to takeoff - else: - FlightLib.takeoff(z=z, wait=True, emergency_land=False) + FlightLib.takeoff(z=z, wait=True, timeout_takeoff=timeout, emergency_land=safe_takeoff) + LedLib.blink(0, 255, 0) def land(descend=False): diff --git a/Server/server.py b/Server/server.py index 9d7a562..9b04863 100644 --- a/Server/server.py +++ b/Server/server.py @@ -95,9 +95,13 @@ class Server: @staticmethod def get_ip_address(): - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as ip_socket: - ip_socket.connect(("8.8.8.8", 80)) - return ip_socket.getsockname()[0] + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as ip_socket: + ip_socket.connect(("8.8.8.8", 80)) + return ip_socket.getsockname()[0] + except OSError: + logging.warning("No network connection detected, starting on localhost") + return "localhost" @staticmethod def get_ntp_time(ntp_host, ntp_port): @@ -108,6 +112,13 @@ class Server: msg, _ = ntp_socket.recvfrom(1024) return int.from_bytes(msg[-8:], 'big') / 2 ** 32 - NTP_DELTA + def time_now(self): + if self.USE_NTP: + timenow = self.get_ntp_time(self.NTP_HOST, self.NTP_PORT) + else: + timenow = time.time() + return timenow + # noinspection PyArgumentList def _client_processor(self): logging.info("Client processor (selector) thread started!") @@ -189,13 +200,6 @@ class Server: broadcast_client.close() logging.info("Broadcast listener thread stopped, socked closed!") - def time_now(self): - if self.USE_NTP: - timenow = Server.get_ntp_time(self.NTP_HOST, self.NTP_PORT) - else: - timenow = time.time() - return timenow - def send_starttime(self, copter, dt=0): timenow = self.time_now() print('Now:', time.ctime(timenow), "+ dt =", dt) @@ -270,20 +274,19 @@ class Client(messaging.ConnectionManager): super(Client, self).close() - @requires_connect def _send(self, data): super(Client, self)._send(data) logging.debug("Queued data to send: {}".format(data)) - def send_config_options(self, *options: ConfigOption): + def send_config_options(self, *options: ConfigOption, reload_config=True): logging.info("Sending config options: {} to {}".format(options, self.addr)) - for option in options: - self.send_message( - 'config_write', - {'section': option.section, 'option': option.option, 'value': option.value} - ) - self.send_message("config_reload") + sending_options = [{'section': option.section, 'option': option.option, 'value': option.value} + for option in options] + print(sending_options) + self.send_message( + 'config_write', {"options": sending_options, "reload": reload_config} + ) @staticmethod @requires_any_connected diff --git a/Server/server_qt.py b/Server/server_qt.py index e6acb1a..53e480a 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -165,7 +165,7 @@ class MainWindow(QtWidgets.QMainWindow): for section in sendable_config.sections(): for option in dict(sendable_config.items(section)): value = sendable_config[section][option] - print("Got item from config:", section, option, value) + logging.debug("Got item from config:".format(section, option, value)) options.append(ConfigOption(section, option, value)) for row_num in range(model.rowCount()): item = model.item(row_num, 0) @@ -184,7 +184,7 @@ class MainWindow(QtWidgets.QMainWindow): if item.isCheckable() and item.checkState() == Qt.Checked: copter = Client.get_by_id(item.text()) copter.send_file(path, "/home/pi/catkin_ws/src/clever/aruco_pose/map/animation_map.txt") - # clever_restart=True + copter.send_message("service_restart", {"name": "clever"}) model = QStandardItemModel() diff --git a/messaging_lib.py b/messaging_lib.py index ab3f375..49d1160 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -9,12 +9,12 @@ import collections try: import selectors -except: +except ImportError: import selectors2 as selectors PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on", "callback", "callback_args", "callback_kwargs", - #"obj", + # "obj", ]) @@ -97,11 +97,11 @@ class MessageManager: self.jsonheader = self._json_decode(self.income_raw[:header_len], "utf-8") self.income_raw = self.income_raw[header_len:] for reqhdr in ( - "byteorder", - "content-length", - "content-type", - "content-encoding", - "message-type", + "byteorder", + "content-length", + "content-type", + "content-encoding", + "message-type", ): if reqhdr not in self.jsonheader: raise ValueError('Missing required header {}'.format(reqhdr)) @@ -131,13 +131,29 @@ class MessageManager: self._process_content() -def request_callback(string_command): +def message_callback(string_command): def inner(f): - ConnectionManager.requests_callbacks.update({string_command: f}) + ConnectionManager.messages_callbacks[string_command] = f + logging.debug("Registered message function {} for {}".format(f, string_command)) def wrapper(*args, **kwargs): return f(*args, **kwargs) + return wrapper + + return inner + + +def request_callback(string_command): + def inner(f): + ConnectionManager.requests_callbacks[string_command] = f + logging.debug("Registered callback function {} for {}".format(f, string_command)) + + def wrapper(*args, **kwargs): + return f(*args, **kwargs) + + return wrapper + return inner @@ -219,13 +235,17 @@ class ConnectionManager(object): self._received_queue.appendleft(MessageManager()) self._received_queue[0].income_raw += self._recv_buffer + self._recv_buffer = b'' self._received_queue[0].process_message() + # if something left after processing message - put it back if self._received_queue[0].content and self._received_queue[0].income_raw: self._recv_buffer = self._received_queue[0].income_raw + self._recv_buffer self._received_queue[0].income_raw = b'' - self.process_received() + if self._received_queue: + if self._received_queue[-1].content: + self.process_received() def _read(self): try: @@ -245,19 +265,19 @@ class ConnectionManager(object): raise RuntimeError("Peer closed.") def process_received(self): - if self._received_queue: - if self._received_queue[-1].content: - message = self._received_queue.pop() - message_type = message.jsonheader["message-type"] - logging.debug("Received message! Header: {}, content: {}".format(message.jsonheader, message.content)) - if message_type == "message": - self._process_message(message) - elif message_type == "response": - self._process_response(message) - elif message_type == "request": - self._process_request(message) - elif message_type == "filetransfer": - self._process_filetransfer(message) + income_message = self._received_queue.pop() + message_type = income_message.jsonheader["message-type"] + logging.debug("Received message! Header: {}, content: {}".format( + income_message.jsonheader, income_message.content)) + + if message_type == "message": + self._process_message(income_message) + elif message_type == "response": + self._process_response(income_message) + elif message_type == "request": + self._process_request(income_message) + elif message_type == "filetransfer": + self._process_filetransfer(income_message) def _process_message(self, message): command = message.content["command"] @@ -295,19 +315,8 @@ class ConnectionManager(object): logging.debug( "Request successfully closed with value {}".format(message.content["value"]) ) - ''' - if request.obj: - obj = request.obj - f = request.callback - - obj.f(request.value, *request.callback_args, **request.callback_kwargs) - else: - f = request.callback - f(request.value, *request.callback_args, **request.callback_kwargs) - ''' f = request.callback - print(f) f(value, *request.callback_args, **request.callback_kwargs) break @@ -329,7 +338,6 @@ class ConnectionManager(object): with self._send_lock: if (not self._send_buffer) and self._send_queue: message = self._send_queue.popleft() - print(self._send_queue) self._send_buffer += message if self._send_buffer: self._write() @@ -351,14 +359,13 @@ class ConnectionManager(object): else: logging.debug("Sent {} to {}".format(self._send_buffer[:sent], self.addr)) self._send_buffer = self._send_buffer[sent:] - print(self._send_buffer) def _send(self, data): with self._send_lock: self._send_queue.append(data) def get_response(self, requested_value, callback, request_args=None, # timeout=30, - callback_args=(), callback_kwargs=None, obj=None): + callback_args=(), callback_kwargs=None): if request_args is None: request_args = {} if callback_kwargs is None: @@ -373,7 +380,6 @@ class ConnectionManager(object): callback=callback, callback_args=callback_args, callback_kwargs=callback_kwargs, - #obj=obj ) self._send(MessageManager.create_request(requested_value, request_id, request_args)) @@ -393,4 +399,4 @@ class ConnectionManager(object): logging.info("Sending file {} to {} (as: {})".format(filepath, self.addr, dest_filepath)) self._send(MessageManager.create_message( data, "binary", "filetransfer", "binary", {"filepath": dest_filepath} - )) \ No newline at end of file + )) From fc5a15510ebc5c11c445fb55af4d71f1db6f7730 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Thu, 18 Apr 2019 19:59:41 +0300 Subject: [PATCH 13/47] Improvements in client-side --- Drone/client_config.ini | 3 ++- Drone/copter_client.py | 21 ++++++++++++++++----- Drone/play_animation.py | 14 ++++++++------ messaging_lib.py | 5 ++--- 4 files changed, 28 insertions(+), 15 deletions(-) diff --git a/Drone/client_config.ini b/Drone/client_config.ini index 1a63ad8..2fe7540 100644 --- a/Drone/client_config.ini +++ b/Drone/client_config.ini @@ -1,7 +1,7 @@ [SERVER] port = 25000 broadcast_port = 8181 -host = 192.168.1.104 +host = 192.168.43.168 buffer_size = 1024 [FILETRANSFER] @@ -27,3 +27,4 @@ id = /hostname use_leds = True x0 = 0 y0 = 0 + diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 94533a7..c732122 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -9,16 +9,17 @@ import client import messaging_lib as messaging import play_animation -def class CopterClient(client.Client): def load_config(self): super(CopterClient, self).load_config() - self.FRAME_ID = self.config.get('COPTERS', 'frame_id') # TODO in play_animation + self.FRAME_ID = self.config.get('COPTERS', 'frame_id') + play_animation.FRAME_ID = self.FRAME_ID self.TAKEOFF_HEIGHT = self.config.getfloat('COPTERS', 'takeoff_height') self.TAKEOFF_TIME = self.config.getfloat('COPTERS', 'takeoff_time') - self.RFP_TIME = self.config.getfloat('COPTERS', 'reach_first_point_time') self.SAFE_TAKEOFF = self.config.getboolean('COPTERS', 'safe_takeoff') + self.RFP_TIME = self.config.getfloat('COPTERS', 'reach_first_point_time') + self.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common') self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common') @@ -34,23 +35,26 @@ class CopterClient(client.Client): if self.USE_LEDS: LedLib.init_led() - @messaging.request_callback("selfcheck") def _response_selfcheck(): return FlightLib.selfcheck() + @messaging.request_callback("batt_voltage") def _response_batt(): return FlightLib.get_telemetry('body').voltage + @messaging.request_callback("cell_voltage") def _response_cell(): return FlightLib.get_telemetry('body').cell_voltage + @messaging.message_callback("service_restart") -def _message_service_restart(*args, **kwargs): +def _command_service_restart(*args, **kwargs): os.system("systemctl restart"+kwargs["name"]) + @messaging.message_callback("led_test") def _command_config_write(*args, **kwargs): LedLib.chase(255, 255, 255) @@ -58,6 +62,13 @@ def _command_config_write(*args, **kwargs): LedLib.off() +@messaging.message_callback("takeoff") +def _command_takeoff(*args, **kwargs): + play_animation.takeoff(z=client.active_client.TAKEOFF_HEIGHT, + timeout=client.active_client.TAKEOFF_TIME, + safe_takeoff=client.active_client.SAFE_TAKEOFF, + ) + if __name__ == "__main__": copter_client = CopterClient() diff --git a/Drone/play_animation.py b/Drone/play_animation.py index 2f554c2..99eee63 100644 --- a/Drone/play_animation.py +++ b/Drone/play_animation.py @@ -10,13 +10,13 @@ module_logger = logging.getLogger("Animation player") animation_file_path = 'animation.csv' USE_LEDS = True +FRAME_ID = 'aruco_map' def takeoff(z=1.5, safe_takeoff=True, timeout=5000): if USE_LEDS: LedLib.wipe_to(255, 0, 0) - if safe_takeoff: - FlightLib.takeoff(z=z, wait=True, timeout_takeoff=timeout, emergency_land=safe_takeoff) + FlightLib.takeoff(z=z, wait=True, timeout_takeoff=timeout, emergency_land=safe_takeoff,) LedLib.blink(0, 255, 0) @@ -28,14 +28,16 @@ def land(descend=False): LedLib.off() -def animate_frame(current_frame, x0=0.0, y0=0.0): - FlightLib.navto(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'], yaw=1.57) # TODO yaw +def animate_frame(current_frame, x0=0.0, y0=0.0, copter_frame_id=FRAME_ID): + FlightLib.navto(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'], + yaw=1.57, frame_id=copter_frame_id) # TODO yaw if USE_LEDS: LedLib.fill(current_frame['red'], current_frame['green'], current_frame['blue']) -def reach_frame(current_frame, x0=0.0, y0=0.0, timeout=5000): - FlightLib.reach_point(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'], yaw=1.57, timeout=timeout) # TODO yaw +def reach_frame(current_frame, x0=0.0, y0=0.0, timeout=5000, copter_frame_id=FRAME_ID): + FlightLib.reach_point(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'], + yaw=1.57, timeout=timeout, frame_id=copter_frame_id) # TODO yaw if USE_LEDS: LedLib.fill(current_frame['red'], current_frame['green'], current_frame['blue']) diff --git a/messaging_lib.py b/messaging_lib.py index 49d1160..79f2ae3 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -14,7 +14,6 @@ except ImportError: PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on", "callback", "callback_args", "callback_kwargs", - # "obj", ]) @@ -323,14 +322,14 @@ class ConnectionManager(object): else: logging.warning("Unexpected response!") - def _process_filetransfer(self, message): + def _process_filetransfer(self, message): # TODO path? if message.jsonheader["content-type"] == "binary": filepath = message.jsonheader["filepath"] try: with open(filepath, 'wb') as f: f.write(message.content) except OSError as error: - logging.warning("File can not be written due error: ".format(error)) + logging.error("File {} can not be written due error: {}".format(filepath, error)) else: logging.info("File {} successfully received ".format(filepath)) From 1ae3438c15c8270bbecb30c220ba38293a4b6170 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Thu, 18 Apr 2019 20:48:59 +0300 Subject: [PATCH 14/47] Fixes: receiving multiple messages at once, added rospy init --- Drone/copter_client.py | 4 +++- Server/server_qt.py | 2 +- messaging_lib.py | 11 +++++------ 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index c732122..8e975a2 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -37,7 +37,8 @@ class CopterClient(client.Client): @messaging.request_callback("selfcheck") def _response_selfcheck(): - return FlightLib.selfcheck() + check = FlightLib.selfcheck() + return check if check else "OK" @messaging.request_callback("batt_voltage") @@ -71,5 +72,6 @@ def _command_takeoff(*args, **kwargs): if __name__ == "__main__": + rospy.init_node('Swarm_client', anonymous=True) copter_client = CopterClient() copter_client.start() diff --git a/Server/server_qt.py b/Server/server_qt.py index 53e480a..6096b26 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -64,7 +64,7 @@ class MainWindow(QtWidgets.QMainWindow): model.setData(model.index(row, col), "{} %".format(round(batt_percent, 3))) elif col == 4: if value != "OK": - model.setData(model.index(row, col), str(value)) + model.setData(model.index(row, col), str(value)) # TODO different handling else: model.setData(model.index(row, col), str(value)) diff --git a/messaging_lib.py b/messaging_lib.py index 79f2ae3..79d36f9 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -229,7 +229,7 @@ class ConnectionManager(object): def read(self): self._read() - if self._recv_buffer: + while self._recv_buffer: if not self._received_queue or (self._received_queue[0].content is not None): self._received_queue.appendleft(MessageManager()) @@ -242,9 +242,9 @@ class ConnectionManager(object): self._recv_buffer = self._received_queue[0].income_raw + self._recv_buffer self._received_queue[0].income_raw = b'' - if self._received_queue: - if self._received_queue[-1].content: - self.process_received() + if self._received_queue: + if self._received_queue[-1].content: + self.process_received(self._received_queue.popleft()) def _read(self): try: @@ -263,8 +263,7 @@ class ConnectionManager(object): raise RuntimeError("Peer closed.") - def process_received(self): - income_message = self._received_queue.pop() + def process_received(self, income_message): message_type = income_message.jsonheader["message-type"] logging.debug("Received message! Header: {}, content: {}".format( income_message.jsonheader, income_message.content)) From e2f0e40c1b47429772f0fce2638aa3de5fdf6dd8 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Thu, 18 Apr 2019 21:04:15 +0300 Subject: [PATCH 15/47] Logging with ros fix --- Drone/client.py | 4 +++- Drone/ros_logging.py | 23 +++++++++++++++++++++++ 2 files changed, 26 insertions(+), 1 deletion(-) create mode 100644 Drone/ros_logging.py diff --git a/Drone/client.py b/Drone/client.py index 418ea07..273f8f8 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -16,13 +16,15 @@ parent_dir = os.path.dirname(current_dir) sys.path.insert(0, parent_dir) import messaging_lib as messaging +import ros_logging logging.basicConfig( # TODO all prints as logs level=logging.DEBUG, # INFO format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s", handlers=[ logging.FileHandler("client_logs.log"), - logging.StreamHandler() + logging.StreamHandler(), + ros_logging.RosHandler(), ]) ConfigOption = collections.namedtuple("ConfigOption", ["section", "option", "value"]) diff --git a/Drone/ros_logging.py b/Drone/ros_logging.py new file mode 100644 index 0000000..12cb634 --- /dev/null +++ b/Drone/ros_logging.py @@ -0,0 +1,23 @@ +import logging +import rospy + + +class RosHandler(logging.Handler): + + level_map = { + logging.DEBUG: rospy.logdebug, + logging.INFO: rospy.loginfo, + logging.WARNING: rospy.logwarn, + logging.ERROR: rospy.logerr, + logging.CRITICAL: rospy.logfatal + } + + def emit(self, record): + try: + self.level_map[record.levelno]("%s: %s" % (record.name, record.msg)) + except KeyError: + rospy.logerr("unknown log level %s LOG: %s: %s" % (record.levelno, record.name, record.msg)) + + +def route_logger_to_ros(logger_name): + logging.getLogger(logger_name).addHandler(RosHandler()) From d79bb7d0c6871a0ba6f84f06125aa31f8403f87b Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Thu, 18 Apr 2019 22:17:48 +0300 Subject: [PATCH 16/47] Ros logging fixes... --- Drone/client.py | 2 -- Drone/copter_client.py | 3 +++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index 273f8f8..8751006 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -16,7 +16,6 @@ parent_dir = os.path.dirname(current_dir) sys.path.insert(0, parent_dir) import messaging_lib as messaging -import ros_logging logging.basicConfig( # TODO all prints as logs level=logging.DEBUG, # INFO @@ -24,7 +23,6 @@ logging.basicConfig( # TODO all prints as logs handlers=[ logging.FileHandler("client_logs.log"), logging.StreamHandler(), - ros_logging.RosHandler(), ]) ConfigOption = collections.namedtuple("ConfigOption", ["section", "option", "value"]) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 8e975a2..77e8594 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -8,6 +8,8 @@ from FlightLib import LedLib import client import messaging_lib as messaging import play_animation +import ros_logging + class CopterClient(client.Client): @@ -73,5 +75,6 @@ def _command_takeoff(*args, **kwargs): if __name__ == "__main__": rospy.init_node('Swarm_client', anonymous=True) + ros_logging.route_logger_to_ros("client") copter_client = CopterClient() copter_client.start() From bea343adf3645ba0dca75ec35e1a274c847a1b64 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Thu, 18 Apr 2019 22:25:51 +0300 Subject: [PATCH 17/47] Another ros fix attempt --- Drone/copter_client.py | 2 +- Drone/ros_logging.py | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 77e8594..dba2b15 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -75,6 +75,6 @@ def _command_takeoff(*args, **kwargs): if __name__ == "__main__": rospy.init_node('Swarm_client', anonymous=True) - ros_logging.route_logger_to_ros("client") + ros_logging.route_logger_to_ros() copter_client = CopterClient() copter_client.start() diff --git a/Drone/ros_logging.py b/Drone/ros_logging.py index 12cb634..8fdaf54 100644 --- a/Drone/ros_logging.py +++ b/Drone/ros_logging.py @@ -19,5 +19,8 @@ class RosHandler(logging.Handler): rospy.logerr("unknown log level %s LOG: %s: %s" % (record.levelno, record.name, record.msg)) -def route_logger_to_ros(logger_name): - logging.getLogger(logger_name).addHandler(RosHandler()) +def route_logger_to_ros(logger_name=None): + if logger_name is not None: + logging.getLogger(logger_name).addHandler(RosHandler()) + else: + logging.getLogger().addHandler(RosHandler()) From 34d01d68d3588a846cac14b55b5d7dde1c012961 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Fri, 19 Apr 2019 09:20:01 +0300 Subject: [PATCH 18/47] ros logging... --- Drone/ros_logging.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Drone/ros_logging.py b/Drone/ros_logging.py index 8fdaf54..58a3716 100644 --- a/Drone/ros_logging.py +++ b/Drone/ros_logging.py @@ -13,10 +13,11 @@ class RosHandler(logging.Handler): } def emit(self, record): - try: - self.level_map[record.levelno]("%s: %s" % (record.name, record.msg)) - except KeyError: - rospy.logerr("unknown log level %s LOG: %s: %s" % (record.levelno, record.name, record.msg)) + if "rosout" not in record.msg: + try: + self.level_map[record.levelno]("%s: %s" % (record.name, record.msg)) + except KeyError: + rospy.logerr("unknown log level %s LOG: %s: %s" % (record.levelno, record.name, record.msg)) def route_logger_to_ros(logger_name=None): From 838337b2b10918679ff1c14c876de6b08cd9c18e Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Fri, 19 Apr 2019 09:29:13 +0300 Subject: [PATCH 19/47] Still ros logging --- Drone/client.py | 2 +- Drone/ros_logging.py | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index 8751006..8450eb3 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -22,7 +22,7 @@ logging.basicConfig( # TODO all prints as logs format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s", handlers=[ logging.FileHandler("client_logs.log"), - logging.StreamHandler(), + #logging.StreamHandler(), ]) ConfigOption = collections.namedtuple("ConfigOption", ["section", "option", "value"]) diff --git a/Drone/ros_logging.py b/Drone/ros_logging.py index 58a3716..ed02263 100644 --- a/Drone/ros_logging.py +++ b/Drone/ros_logging.py @@ -13,11 +13,11 @@ class RosHandler(logging.Handler): } def emit(self, record): - if "rosout" not in record.msg: - try: - self.level_map[record.levelno]("%s: %s" % (record.name, record.msg)) - except KeyError: - rospy.logerr("unknown log level %s LOG: %s: %s" % (record.levelno, record.name, record.msg)) + #if "rosout" not in record.msg: + try: + self.level_map[record.levelno]("%s: %s" % (record.name, record.msg)) + except KeyError: + rospy.logerr("unknown log level %s LOG: %s: %s" % (record.levelno, record.name, record.msg)) def route_logger_to_ros(logger_name=None): From f35d25a7b4449825ba61e7db0b59b734b6a58874 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Fri, 19 Apr 2019 09:33:29 +0300 Subject: [PATCH 20/47] RL --- Drone/client.py | 3 ++- Drone/ros_logging.py | 10 +++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index 8450eb3..b23dc8e 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -21,7 +21,8 @@ logging.basicConfig( # TODO all prints as logs level=logging.DEBUG, # INFO format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s", handlers=[ - logging.FileHandler("client_logs.log"), + logging.NullHandler() + #logging.FileHandler("client_logs.log"), #logging.StreamHandler(), ]) diff --git a/Drone/ros_logging.py b/Drone/ros_logging.py index ed02263..58a3716 100644 --- a/Drone/ros_logging.py +++ b/Drone/ros_logging.py @@ -13,11 +13,11 @@ class RosHandler(logging.Handler): } def emit(self, record): - #if "rosout" not in record.msg: - try: - self.level_map[record.levelno]("%s: %s" % (record.name, record.msg)) - except KeyError: - rospy.logerr("unknown log level %s LOG: %s: %s" % (record.levelno, record.name, record.msg)) + if "rosout" not in record.msg: + try: + self.level_map[record.levelno]("%s: %s" % (record.name, record.msg)) + except KeyError: + rospy.logerr("unknown log level %s LOG: %s: %s" % (record.levelno, record.name, record.msg)) def route_logger_to_ros(logger_name=None): From 09649c96db3d65e45bf503e8aaa1cd0583f9ef57 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Fri, 19 Apr 2019 11:26:35 +0300 Subject: [PATCH 21/47] Another logging tweaks --- Drone/client.py | 31 ++++++++++++++++--------------- Drone/copter_client.py | 3 +-- messaging_lib.py | 41 +++++++++++++++++++++-------------------- 3 files changed, 38 insertions(+), 37 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index b23dc8e..0c952c1 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -21,11 +21,12 @@ logging.basicConfig( # TODO all prints as logs level=logging.DEBUG, # INFO format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s", handlers=[ - logging.NullHandler() - #logging.FileHandler("client_logs.log"), - #logging.StreamHandler(), + logging.FileHandler("client_logs.log"), + logging.StreamHandler(), ]) +logger = logging.getLogger(__name__) + ConfigOption = collections.namedtuple("ConfigOption", ["section", "option", "value"]) active_client = None # maybe needs to be refactored @@ -110,33 +111,33 @@ class Client(object): self._process_connections() except (KeyboardInterrupt, InterruptedError): - logging.critical("Caught interrupt, exiting!") + logger.critical("Caught interrupt, exiting!") self.selector.close() def _reconnect(self, timeout=2, attempt_limit=5): - logging.info("Trying to connect to {}:{} ...".format(self.server_host, self.server_port)) + logger.info("Trying to connect to {}:{} ...".format(self.server_host, self.server_port)) attempt_count = 0 while not self.connected: - logging.info("Waiting for connection, attempt {}".format(attempt_count)) + logger.info("Waiting for connection, attempt {}".format(attempt_count)) try: self.client_socket = socket.socket() self.client_socket.settimeout(timeout) self.client_socket.connect((self.server_host, self.server_port)) except socket.error as error: if error.errno != errno.EINTR: - logging.warning("Can not connect due error: {}".format(error)) + logger.warning("Can not connect due error: {}".format(error)) attempt_count += 1 time.sleep(timeout) else: - logging.critical("Shutting down on keyboard interrupt") + logger.critical("Shutting down on keyboard interrupt") raise KeyboardInterrupt else: - logging.info("Connection to server successful!") + logger.info("Connection to server successful!") self._connect() break if attempt_count >= attempt_limit: - logging.info("Too many attempts. Trying to get new server IP") + logger.info("Too many attempts. Trying to get new server IP") self.broadcast_bind() attempt_count = 0 @@ -161,7 +162,7 @@ class Client(object): message.income_raw = data message.process_message() if message.content: - logging.info("Received broadcast message {} from {}".format(message.content, addr)) + logger.info("Received broadcast message {} from {}".format(message.content, addr)) if message.content["command"] == "server_ip": args = message.content["args"] self.server_port = int(args["port"]) @@ -169,7 +170,7 @@ class Client(object): self.write_config(False, ConfigOption("SERVER", "port", self.server_port), ConfigOption("SERVER", "host", self.server_host)) - logging.info("Binding to new IP: {}:{}".format(self.server_host, self.server_port)) + logger.info("Binding to new IP: {}:{}".format(self.server_host, self.server_port)) break finally: broadcast_client.close() @@ -186,7 +187,7 @@ class Client(object): try: connection.process_events(mask) except Exception as error: - logging.error( + logger.error( "Exception {} occurred for {}! Resetting connection!".format(error, connection.addr) ) self.server_connection.close() @@ -194,7 +195,7 @@ class Client(object): break if not self.selector.get_map(): - logging.warning("No active connections left!") + logger.warning("No active connections left!") return @@ -209,7 +210,7 @@ def _response_time(): @messaging.message_callback("config_write") def _command_config_write(*args, **kwargs): options = [ConfigOption(**raw_option) for raw_option in kwargs["options"]] - logging.info("Writing config options: {}".format(options)) + logger.info("Writing config options: {}".format(options)) active_client.write_config(kwargs["reload"], *options) if __name__ == "__main__": diff --git a/Drone/copter_client.py b/Drone/copter_client.py index dba2b15..7297df3 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -11,7 +11,6 @@ import play_animation import ros_logging - class CopterClient(client.Client): def load_config(self): super(CopterClient, self).load_config() @@ -74,7 +73,7 @@ def _command_takeoff(*args, **kwargs): if __name__ == "__main__": + ros_logging.route_logger_to_ros(messaging.__name__) rospy.init_node('Swarm_client', anonymous=True) - ros_logging.route_logger_to_ros() copter_client = CopterClient() copter_client.start() diff --git a/messaging_lib.py b/messaging_lib.py index 79d36f9..a387d85 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -15,6 +15,7 @@ except ImportError: PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on", "callback", "callback_args", "callback_kwargs", ]) +logger = logging.getLogger(__name__) class MessageManager: @@ -133,7 +134,7 @@ class MessageManager: def message_callback(string_command): def inner(f): ConnectionManager.messages_callbacks[string_command] = f - logging.debug("Registered message function {} for {}".format(f, string_command)) + logger.debug("Registered message function {} for {}".format(f, string_command)) def wrapper(*args, **kwargs): return f(*args, **kwargs) @@ -146,7 +147,7 @@ def message_callback(string_command): def request_callback(string_command): def inner(f): ConnectionManager.requests_callbacks[string_command] = f - logging.debug("Registered callback function {} for {}".format(f, string_command)) + logger.debug("Registered callback function {} for {}".format(f, string_command)) def wrapper(*args, **kwargs): return f(*args, **kwargs) @@ -202,13 +203,13 @@ class ConnectionManager(object): self._set_selector_events_mask('rw') def close(self): - logging.info("Closing connection to {}".format(self.addr)) + logger.info("Closing connection to {}".format(self.addr)) try: self.selector.unregister(self.socket) except AttributeError: pass except Exception as error: - logging.error("{}: Error during selector unregistering: {}".format(self.addr, error)) + logger.error("{}: Error during selector unregistering: {}".format(self.addr, error)) finally: self.selector = None @@ -217,7 +218,7 @@ class ConnectionManager(object): except AttributeError: pass except OSError as error: - logging.error("{}: Error during socket closing: {}".format(self.addr, error)) + logger.error("{}: Error during socket closing: {}".format(self.addr, error)) finally: self.socket = None @@ -255,9 +256,9 @@ class ConnectionManager(object): else: if data: self._recv_buffer += data - logging.debug("Received {} from {}".format(data, self.addr)) + logger.debug("Received {} from {}".format(data, self.addr)) else: - logging.warning("Connection to {} lost!".format(self.addr)) + logger.warning("Connection to {} lost!".format(self.addr)) if not self.resume_queue: self._recv_buffer = b'' @@ -265,7 +266,7 @@ class ConnectionManager(object): def process_received(self, income_message): message_type = income_message.jsonheader["message-type"] - logging.debug("Received message! Header: {}, content: {}".format( + logger.debug("Received message! Header: {}, content: {}".format( income_message.jsonheader, income_message.content)) if message_type == "message": @@ -283,9 +284,9 @@ class ConnectionManager(object): try: self.messages_callbacks[command](**args) except KeyError: - logging.warning("Command {} does not exist!".format(command)) + logger.warning("Command {} does not exist!".format(command)) except Exception as error: - logging.error("Error during command {} execution: {}".format(command, error)) + logger.error("Error during command {} execution: {}".format(command, error)) def _process_request(self, message): command = message.content["requested_value"] @@ -294,9 +295,9 @@ class ConnectionManager(object): try: value = self.requests_callbacks[command](**args) except KeyError: - logging.warning("Request {} does not exist!".format(command)) + logger.warning("Request {} does not exist!".format(command)) except Exception as error: # TODO send response error\cancel - logging.error("Error during command {} execution: {}".format(command, error)) + logger.error("Error during command {} execution: {}".format(command, error)) else: self._send_response(command, request_id, value) @@ -310,7 +311,7 @@ class ConnectionManager(object): print(request) value = message.content["value"] print(45) - logging.debug( + logger.debug( "Request successfully closed with value {}".format(message.content["value"]) ) @@ -319,7 +320,7 @@ class ConnectionManager(object): break else: - logging.warning("Unexpected response!") + logger.warning("Unexpected response!") def _process_filetransfer(self, message): # TODO path? if message.jsonheader["content-type"] == "binary": @@ -328,9 +329,9 @@ class ConnectionManager(object): with open(filepath, 'wb') as f: f.write(message.content) except OSError as error: - logging.error("File {} can not be written due error: {}".format(filepath, error)) + logger.error("File {} can not be written due error: {}".format(filepath, error)) else: - logging.info("File {} successfully received ".format(filepath)) + logger.info("File {} successfully received ".format(filepath)) def write(self): with self._send_lock: @@ -347,7 +348,7 @@ class ConnectionManager(object): # Resource temporarily unavailable (errno EWOULDBLOCK) pass except Exception as error: - logging.warning("Attempt to send message {} to {} failed due error: {}".format( + logger.warning("Attempt to send message {} to {} failed due error: {}".format( self._send_buffer, self.addr, error)) if not self.resume_queue: @@ -355,7 +356,7 @@ class ConnectionManager(object): raise error else: - logging.debug("Sent {} to {}".format(self._send_buffer[:sent], self.addr)) + logger.debug("Sent {} to {}".format(self._send_buffer[:sent], self.addr)) self._send_buffer = self._send_buffer[sent:] def _send(self, data): @@ -392,9 +393,9 @@ class ConnectionManager(object): with open(filepath, 'rb') as f: data = f.read() except OSError as error: - logging.warning("File can not be opened due error: ".format(error)) + logger.warning("File can not be opened due error: ".format(error)) else: - logging.info("Sending file {} to {} (as: {})".format(filepath, self.addr, dest_filepath)) + logger.info("Sending file {} to {} (as: {})".format(filepath, self.addr, dest_filepath)) self._send(MessageManager.create_message( data, "binary", "filetransfer", "binary", {"filepath": dest_filepath} )) From 1bc17e3d67571012d14b597482d714db0680998d Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Fri, 19 Apr 2019 11:34:15 +0300 Subject: [PATCH 22/47] rl --- Drone/copter_client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 7297df3..c34e1dc 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -73,7 +73,7 @@ def _command_takeoff(*args, **kwargs): if __name__ == "__main__": - ros_logging.route_logger_to_ros(messaging.__name__) + ros_logging.route_logger_to_ros() rospy.init_node('Swarm_client', anonymous=True) copter_client = CopterClient() copter_client.start() From 67a31d70ae5d998232ae63623c8e2d9468c450f4 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Sun, 5 May 2019 22:24:39 +0300 Subject: [PATCH 23/47] Initial commit of new animation_lib and async tasking_lib with suppurt of managing tasks on-the-go and interrupt them immediately. Improvements in FlightLib (including better selfcheck code) and client. Massive testing required (will be done soon) --- Drone/FlightLib/FlightLib.py | 212 +++++++++++++++++------------------ Drone/animation_lib.py | 90 +++++++++++++++ Drone/client.py | 13 ++- Drone/copter_client.py | 120 ++++++++++++++++++-- Drone/play_animation.py | 4 +- Drone/tasking_lib.py | 193 +++++++++++++++++++++++++++++++ Server/server_qt.py | 3 + messaging_lib.py | 9 +- 8 files changed, 511 insertions(+), 133 deletions(-) create mode 100644 Drone/animation_lib.py create mode 100644 Drone/tasking_lib.py diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index e2960b5..b5fdb7c 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -11,7 +11,7 @@ from mavros_msgs.srv import SetMode from mavros_msgs.srv import CommandBool from std_srvs.srv import Trigger -module_logger = logging.getLogger("FlightLib") +logger = logging.getLogger(__name__) # create proxy service navigate = rospy.ServiceProxy('navigate', srv.Navigate) @@ -22,24 +22,26 @@ get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) landing = rospy.ServiceProxy('/land', Trigger) -module_logger.info("Proxy services inited") +logger.info("Proxy services inited") # globals -FREQUENCY = 1000/25 # HZ +FREQUENCY = 1000 / 25 # HZ TOLERANCE = 0.2 interrupt_event = threading.Event() +checklist = [] + def interrupt(): - module_logger.info("Performing function interrupt") + logger.info("Performing function interrupt") interrupt_event.set() def init(node_name="CleverSwarmFlight", anon=True, no_signals=True): - module_logger.info("Initing ROS node") + logger.info("Initing ROS node") rospy.init_node(node_name, anonymous=anon, disable_signals=no_signals) - module_logger.info("Ros node inited") + logger.info("Ros node inited") def get_distance3d(x1, y1, z1, x2, y2, z2): @@ -55,89 +57,78 @@ def check(check_name): for failure in failures: msg = "[{}]: Failure: {}".format(check_name, failure) msgs.append(msg) - module_logger.warning(msg) + logger.warning(msg) return msgs else: - module_logger.info("[{}]: OK".format(check_name)) + logger.info("[{}]: OK".format(check_name)) return None + + checklist.append(wrapper) return wrapper + return inner @check("Linear velocity estimation") -def check_linear_speeds(): - failures = [] +def check_linear_speeds(speed_limit=0.1): telemetry = get_telemetry(frame_id='body') - speed_limit = 0.1 + if telemetry.vx >= speed_limit: - failures.append("X velocity estimation: {:.3f} m/s".format(telemetry.vx)) + yield ("X velocity estimation: {:.3f} m/s".format(telemetry.vx)) if telemetry.vy >= speed_limit: - failures.append("Y velocity estimation: {:.3f} m/s".format(telemetry.vy)) + yield ("Y velocity estimation: {:.3f} m/s".format(telemetry.vy)) if telemetry.vz >= speed_limit: - failures.append("Z velocity estimation: {:.3f} m/s".format(telemetry.vz)) - return failures + yield ("Z velocity estimation: {:.3f} m/s".format(telemetry.vz)) @check("Angular velocity estimation") -def check_angular_speeds(): - failures = [] +def check_angular_speeds(rate_limit=0.05): telemetry = get_telemetry(frame_id='body') - rate_limit = 0.05 + if telemetry.pitch_rate >= rate_limit: - failures.append("Pitch rate estimation: {:.3f} rad/s".format(telemetry.pitch_rate)) + yield ("Pitch rate estimation: {:.3f} rad/s".format(telemetry.pitch_rate)) if telemetry.roll_rate >= rate_limit: - failures.append("Roll rate estimation: {:.3f} rad/s".format(telemetry.roll_rate)) + yield ("Roll rate estimation: {:.3f} rad/s".format(telemetry.roll_rate)) if telemetry.yaw_rate >= rate_limit: - failures.append("Yaw rate estimation: {:.3f} rad/s".format(telemetry.yaw_rate)) - return failures + yield ("Yaw rate estimation: {:.3f} rad/s".format(telemetry.yaw_rate)) @check("Angles estimation") -def check_angles(): - failures = [] +def check_angles(angle_limit=math.radians(5)): telemetry = get_telemetry(frame_id='body') - angle_limit = math.radians(5) if abs(telemetry.pitch) >= angle_limit: - failures.append("Pitch estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.pitch, - math.degrees(telemetry.pitch))) + yield ("Pitch estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.pitch, + math.degrees(telemetry.pitch))) if abs(telemetry.roll) >= angle_limit: - failures.append("Roll estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.roll, - math.degrees(telemetry.roll))) + yield ("Roll estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.roll, + math.degrees(telemetry.roll))) if abs(telemetry.yaw) >= angle_limit: - failures.append("Yaw estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.yaw, - math.degrees(telemetry.yaw))) - return failures + yield ("Yaw estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.yaw, + math.degrees(telemetry.yaw))) def selfcheck(): - msgs = [] - linear_speeds = check_linear_speeds() - angular_speeds = check_angular_speeds() - angles = check_angles() - if linear_speeds: - msgs.extend(linear_speeds) - if angular_speeds: - msgs.extend(angular_speeds) - if angles: - msgs.extend(angles) + checks = [] + for check_f in checklist: + msg = check_f() + checks += msg if msg else [] - return msgs + return checks -def navto(x, y, z, yaw=float('nan'), frame_id='aruco_map'): +def navto(x, y, z, yaw=float('nan'), frame_id='aruco_map', **kwargs): set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw) telemetry = get_telemetry(frame_id=frame_id) - module_logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) - module_logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z)) + logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) + logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z)) return True def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLERANCE, frame_id='aruco_map', - freq=FREQUENCY, timeout=5000, wait=False): - - module_logger.info('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) + freq=FREQUENCY, timeout=5000, wait=False, interrupter=interrupt_event): + logger.info('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed) # waiting for completion @@ -146,31 +137,32 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLE time_start = rospy.get_rostime() while (get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z) > tolerance) or wait: - if interrupt_event.is_set(): - module_logger.warning("Flight function interrupted!") - interrupt_event.clear() - break + if interrupter.is_set(): + logger.warning("Flight function interrupted!") + interrupter.clear() + return telemetry = get_telemetry(frame_id=frame_id) - module_logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( + logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) + logger.info('Current delta: | {:.3f}'.format( + get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z))) time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 if timeout is not None: if time_passed >= timeout: - module_logger.warning('Reaching point timed out! | time: {:3f} seconds'.format(time_passed / 1000)) + logger.warning('Reaching point timed out! | time: {:3f} seconds'.format(time_passed / 1000)) return wait rate.sleep() else: - module_logger.info("Point reached!") + logger.info("Point reached!") return True def reach_attitude(z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLERANCE, frame_id='aruco_map', - freq=FREQUENCY, timeout=5000, wait=False): - - module_logger.info('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw)) + freq=FREQUENCY, timeout=5000, wait=False, interrupter=interrupt_event): + logger.info('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw)) current_telem = get_telemetry(frame_id=frame_id) navigate(frame_id=frame_id, x=current_telem.x, y=current_telem.y, z=z, yaw=yaw, speed=speed) @@ -180,31 +172,36 @@ def reach_attitude(z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLERANCE, fram time_start = rospy.get_rostime() while (abs(z - telemetry.z) > tolerance) or wait: - if interrupt_event.is_set(): - module_logger.warning("Flight function interrupted!") - interrupt_event.clear() - break + if interrupter.is_set(): + logger.warning("Flight function interrupted!") + interrupter.clear() + return telemetry = get_telemetry(frame_id=frame_id) - module_logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( + logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 if timeout is not None: if time_passed >= timeout: - module_logger.warning('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed / 1000)) + logger.warning('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed / 1000)) return wait rate.sleep() else: - module_logger.info("Attitude reached!") + logger.info("Attitude reached!") return True +def stop(frame_id='body', hold_speed=1.0): + navigate(frame_id=frame_id, yaw=float('nan'), speed=hold_speed) + + def land(descend=True, z=1.0, frame_id_descend="aruco_map", frame_id_land="aruco_map", - timeout_descend=5000, timeout_land=7500, freq=FREQUENCY): + timeout_descend=5000, timeout_land=7500, freq=FREQUENCY, interrupter=interrupt_event): if descend: - module_logger.info("Descending to: | z: {:.3f}".format(z)) - reach_attitude(z=z, frame_id=frame_id_descend, timeout=timeout_descend, freq=freq, yaw=1.57) #TODO yaw + logger.info("Descending to: | z: {:.3f}".format(z)) + reach_attitude(z=z, frame_id=frame_id_descend, timeout=timeout_descend, freq=freq, yaw=1.57, # TODO yaw + interrupter=interrupter) landing() telemetry = get_telemetry(frame_id='aruco_map') @@ -212,32 +209,32 @@ def land(descend=True, z=1.0, frame_id_descend="aruco_map", frame_id_land="aruco time_start = rospy.get_rostime() while telemetry.armed: - if interrupt_event.is_set(): - module_logger.warning("Flight function interrupted!") - interrupt_event.clear() - break + if interrupter.is_set(): + logger.warning("Flight function interrupted!") + interrupter.clear() + return telemetry = get_telemetry(frame_id=frame_id_land) - module_logger.info("Landing...") + logger.info("Landing... | z: {}".format(telemetry.z)) time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 if timeout_land is not None: if time_passed >= timeout_land: - module_logger.warning('Landing timed out! | time: {:3f} seconds'.format(time_passed / 1000)) - module_logger.warning("Disarming!") + logger.warning('Landing timed out! | time: {:3f} seconds'.format(time_passed / 1000)) + logger.warning("Disarming!") arming(False) return False rate.sleep() else: - module_logger.info("Landing succeeded!") + logger.info("Landing succeeded!") return True -def takeoff(z=1.0, speed=0.8, frame_id='body', freq=FREQUENCY, - timeout_arm=2000, timeout_takeoff=5000, wait=False, tolerance=TOLERANCE, emergency_land=False): - module_logger.info("Starting takeoff!") - print("Starting takeoff!") - module_logger.info("Arming, going to OFFBOARD mode") +def takeoff(z=1.0, speed=0.8, frame_id='body', freq=FREQUENCY, + timeout_arm=2000, timeout_takeoff=5000, wait=False, tolerance=TOLERANCE, emergency_land=False, + interrupter=interrupt_event): + logger.info("Starting takeoff!") + logger.info("Arming, going to OFFBOARD mode") # Arming check set_rates(thrust=0.1, auto_arm=True) @@ -246,54 +243,53 @@ def takeoff(z=1.0, speed=0.8, frame_id='body', freq=FREQUENCY, time_start = rospy.get_rostime() while (not telemetry.armed) or wait: - if interrupt_event.is_set(): - module_logger.warning("Flight function interrupted!") - interrupt_event.clear() - return None + if interrupter.is_set(): + logger.warning("Flight function interrupted!") + interrupter.clear() + return telemetry = get_telemetry(frame_id=frame_id) - module_logger.info("Arming...") + logger.info("Arming...") time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 if timeout_arm is not None: if time_passed >= timeout_arm: if not telemetry.armed: - module_logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed / 1000)) + logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed / 1000)) return False else: break rate.sleep() - module_logger.info("Armed!") - print("Armed!") - - # Reach height - telemetry = get_telemetry(frame_id=frame_id) - z0 = get_telemetry().z - navigate(z=z, speed=speed, frame_id=frame_id, auto_arm=True) - current_height = abs(get_telemetry().z - z0 - z) - while current_height > tolerance or wait: - if interrupt_event.is_set(): - module_logger.warning("Flight function interrupted!") - interrupt_event.clear() - return None + logger.info("Armed!") - current_height = abs(get_telemetry().z - z0 - z) - module_logger.info("Takeoff...") + # Reach height + z0 = get_telemetry().z + z_dest = z + z0 + navigate(z=z_dest, speed=speed, frame_id=frame_id, auto_arm=True) + current_height = abs(get_telemetry().z - z_dest) + while current_height > tolerance or wait: + if interrupter.is_set(): + logger.warning("Flight function interrupted!") + interrupter.clear() + return + + current_height = abs(get_telemetry().z - z_dest) + logger.info("Takeoff...") time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 if timeout_takeoff is not None: if time_passed >= timeout_takeoff: if not wait: - module_logger.warning('Takeoff timed out! | time: {:3f} seconds'.format(time_passed / 1000)) + logger.warning('Takeoff timed out! | time: {:3f} seconds'.format(time_passed / 1000)) if emergency_land: - module_logger.info("Preforming emergency land") - land(descend=False) + logger.info("Preforming emergency land") + land(descend=False, interrupter=interrupter) return False else: break rate.sleep() - module_logger.info("Takeoff succeeded!") - print("Takeoff succeeded!") + logger.info("Takeoff succeeded!") + # print("Takeoff succeeded!") return True diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py new file mode 100644 index 0000000..f60de92 --- /dev/null +++ b/Drone/animation_lib.py @@ -0,0 +1,90 @@ +import time +import csv +import rospy +import logging +import threading + +from FlightLib import FlightLib +from FlightLib import LedLib + +import tasking_lib as tasking + +logger = logging.getLogger(__name__) + +interrupt_event = threading.Event() + + +def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0): + imported_frames = [] + try: + animation_file = open(filepath) + except IOError: + logging.error("File {} can't be opened".format(filepath)) + else: + with animation_file: + csv_reader = csv.reader( + animation_file, delimiter=',', quotechar='|' + ) + for row in csv_reader: + frame_number, x, y, z, yaw, red, green, blue = row + imported_frames.append({ + 'number': int(frame_number), + 'x': float(x) + x0, + 'y': float(y) + y0, + 'z': float(z) + z0, + 'yaw': float(yaw), + 'red': int(red), + 'green': int(green), + 'blue': int(blue), + }) + return imported_frames + + +def convert_frame(frame): + return (frame['x'], frame['y'], frame['z']), (frame["red"], frame["green"], frame["blue"]), frame["yaw"] + + +def execute_frame(point=(), color=(), yaw=float('Nan'), frame_id='aruco_map', use_leds=True, + flight_func=FlightLib.navto, flight_kwargs=None, interrupter=interrupt_event): + + if flight_kwargs is None: + flight_kwargs = {} + + flight_func(*point, yaw=yaw, frame_id=frame_id, interrupter=interrupt_event, **flight_kwargs) + if use_leds: + if color: + LedLib.fill(*color) + + +def execute_animation(frames, frame_delay, frame_id='aruco_map', use_leds=True, flight_func=FlightLib.navto, + interrupter=interrupt_event): + next_frame_time = 0 + for frame in frames: + if interrupter.is_set(): + logger.warning("Animation playing function interrupted!") + interrupter.clear() + return + execute_frame(*convert_frame(frame), frame_id=frame_id, use_leds=use_leds, flight_func=flight_func, + interrupter=interrupter) + + next_frame_time += frame_delay + tasking.wait(next_frame_time, interrupter) + + +def takeoff(z=1.5, safe_takeoff=True, timeout=5000, frame_id='aruco_map', use_leds=True, + interrupter=interrupt_event): + if use_leds: + LedLib.wipe_to(255, 0, 0) + FlightLib.takeoff(z=z, wait=False, timeout_takeoff=timeout, frame_id=frame_id, emergency_land=safe_takeoff, + interrupter=interrupter) + if use_leds: + LedLib.blink(0, 255, 0, wait=50) + + +def land(z=1.5, descend=False, timeout=5000, frame_id='aruco_map', use_leds=True, + interrupter=interrupt_event): + if use_leds: + LedLib.blink(255, 0, 0) + FlightLib.land(z=z, descend=descend, timeout_land=timeout, frame_id_land=frame_id, interrupter=interrupter) + if use_leds: + LedLib.off() diff --git a/Drone/client.py b/Drone/client.py index 0c952c1..c8a3d0d 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -110,7 +110,7 @@ class Client(object): self._reconnect() self._process_connections() - except (KeyboardInterrupt, InterruptedError): + except (KeyboardInterrupt, ): logger.critical("Caught interrupt, exiting!") self.selector.close() @@ -187,11 +187,12 @@ class Client(object): try: connection.process_events(mask) except Exception as error: - logger.error( - "Exception {} occurred for {}! Resetting connection!".format(error, connection.addr) - ) - self.server_connection.close() - self.connected = False + if error.errno != errno.EINTR: + logger.error( + "Exception {} occurred for {}! Resetting connection!".format(error, connection.addr) + ) + self.server_connection.close() + self.connected = False break if not self.selector.get_map(): diff --git a/Drone/copter_client.py b/Drone/copter_client.py index c34e1dc..1e1a15f 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -1,41 +1,46 @@ import os import time import rospy +import logging from FlightLib import FlightLib from FlightLib import LedLib import client + import messaging_lib as messaging -import play_animation +import tasking_lib as tasking +import animation_lib as animation + import ros_logging +logger = logging.getLogger(__name__) + class CopterClient(client.Client): def load_config(self): super(CopterClient, self).load_config() self.FRAME_ID = self.config.get('COPTERS', 'frame_id') - play_animation.FRAME_ID = self.FRAME_ID 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.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common') self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common') self.X0 = self.config.getfloat('PRIVATE', 'x0') self.Y0 = self.config.getfloat('PRIVATE', 'y0') self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds') - play_animation.USE_LEDS = self.USE_LEDS def start(self): super(CopterClient, self).start() + logger.info("Init ROS node") rospy.init_node('Swarm_client', anonymous=True) if self.USE_LEDS: LedLib.init_led() + @messaging.request_callback("selfcheck") def _response_selfcheck(): check = FlightLib.selfcheck() @@ -54,11 +59,11 @@ def _response_cell(): @messaging.message_callback("service_restart") def _command_service_restart(*args, **kwargs): - os.system("systemctl restart"+kwargs["name"]) + os.system("systemctl restart" + kwargs["name"]) @messaging.message_callback("led_test") -def _command_config_write(*args, **kwargs): +def _command_led_test(*args, **kwargs): LedLib.chase(255, 255, 255) time.sleep(2) LedLib.off() @@ -66,14 +71,107 @@ def _command_config_write(*args, **kwargs): @messaging.message_callback("takeoff") def _command_takeoff(*args, **kwargs): - play_animation.takeoff(z=client.active_client.TAKEOFF_HEIGHT, - timeout=client.active_client.TAKEOFF_TIME, - safe_takeoff=client.active_client.SAFE_TAKEOFF, - ) + task_manager.add_task(0, 0, animation.takeoff, + task_kwargs={ + "z": client.active_client.TAKEOFF_HEIGHT, + "timeout": client.active_client.TAKEOFF_TIME, + "safe_takeoff": client.active_client.SAFE_TAKEOFF, + "use_leds": client.active_client.USE_LEDS, + } + ) + + +@messaging.message_callback("land") +def _command_land(*args, **kwargs): + task_manager.reset() + task_manager.add_task(-5, 0, animation.land, + task_kwargs={ + "z": client.active_client.TAKEOFF_HEIGHT, + "timeout": client.active_client.TAKEOFF_TIME, + "frame_id": client.active_client.FRAME_ID, + "use_leds": client.active_client.USE_LEDS, + } + ) + + +@messaging.message_callback("disarm") +def _command_disarm(*args, **kwargs): + task_manager.reset() + task_manager.add_task(-10, 0, FlightLib.arming(False)) + + +@messaging.message_callback("stop") +def _command_stop(*args, **kwargs): + task_manager.stop() + + +@messaging.message_callback("pause") +def _command_stop(*args, **kwargs): + task_manager.pause() + + +@messaging.message_callback("resume") +def _command_stop(*args, **kwargs): + task_manager.resume() + + +@messaging.message_callback("start_animation") +def _play_animation(*args, **kwargs): + gap = 0.25 + start_time = kwargs["start_time"] # TODO + frames = animation.load_animation(os.path.abspath("animation.csv"), + x0=client.active_client.X0 + client.active_client.X0_COMMON, + y0=client.active_client.Y0 + client.active_client.Y0_COMMON, + ) + + task_manager.add_task(start_time, -1, animation.takeoff, + task_kwargs={ + "z": client.active_client.TAKEOFF_HEIGHT, + "timeout": client.active_client.TAKEOFF_TIME, + "safe_takeoff": client.active_client.SAFE_TAKEOFF, + "frame_id": client.active_client.FRAME_ID, + "use_leds": client.active_client.USE_LEDS, + } + ) + + rfp_time = start_time + client.active_client.TAKEOFF_TIME + gap + task_manager.add_task(rfp_time, -1, animation.execute_frame, + task_kwargs={ + "point": animation.convert_frame(frames[0]), + "timeout": client.active_client.RFP_TIME, + "frame_id": client.active_client.FRAME_ID, + "use_leds": client.active_client.USE_LEDS, + "flight_func": FlightLib.reach_point, + } + ) + + animation_time = rfp_time + client.active_client.RFP_TIME + gap + frame_delay = 0.125 # TODO from animation file + task_manager.add_task(animation_time, -1, animation.execute_animation, + task_kwargs={ + "frames": frames, + "frame_delay": frame_delay, + "frame_id": client.active_client.FRAME_ID, + "use_leds": client.active_client.USE_LEDS, + } + ) + + land_time = animation_time + len(frames)*frame_delay + gap + task_manager.add_task(land_time, -1, animation.land, + task_kwargs={ + "z": client.active_client.TAKEOFF_HEIGHT, + "timeout": client.active_client.TAKEOFF_TIME, + "frame_id": client.active_client.FRAME_ID, + "use_leds": client.active_client.USE_LEDS, + }, + ) if __name__ == "__main__": + # rospy.init_node('Swarm_client', anonymous=True) ros_logging.route_logger_to_ros() - rospy.init_node('Swarm_client', anonymous=True) copter_client = CopterClient() + task_manager = tasking.TaskManager() + + task_manager.start() copter_client.start() diff --git a/Drone/play_animation.py b/Drone/play_animation.py index 99eee63..cf0cf0b 100644 --- a/Drone/play_animation.py +++ b/Drone/play_animation.py @@ -5,7 +5,7 @@ import logging from FlightLib import FlightLib from FlightLib import LedLib -module_logger = logging.getLogger("Animation player") +logger = logging.getLogger(__name__) animation_file_path = 'animation.csv' @@ -17,7 +17,7 @@ def takeoff(z=1.5, safe_takeoff=True, timeout=5000): if USE_LEDS: LedLib.wipe_to(255, 0, 0) FlightLib.takeoff(z=z, wait=True, timeout_takeoff=timeout, emergency_land=safe_takeoff,) - LedLib.blink(0, 255, 0) + LedLib.blink(0, 255, 0, wait=50) def land(descend=False): diff --git a/Drone/tasking_lib.py b/Drone/tasking_lib.py new file mode 100644 index 0000000..365061e --- /dev/null +++ b/Drone/tasking_lib.py @@ -0,0 +1,193 @@ +import heapq +import time +import logging +import threading +import collections + +logger = logging.getLogger(__name__) +Task = collections.namedtuple("Task", ["func", "args", "kwargs", "delayable", ]) + + +def wait(end, interrupter=None, maxsleep=1): # Added features to interrupter sleep and set max sleeping interval + def interrupted(): + if interrupter is None: + return False + else: + return interrupter.is_set() + + while not interrupted(): # Basic implementation of pause module until() + now = time.time() + diff = min(end - now, maxsleep) + if diff <= 0: + break + else: + time.sleep(diff / 2) + + +class TaskManager(object): + def __init__(self): + self.task_queue = [] + self._active_task = None + + self._processor_thread = threading.Thread(target=self._task_processor, name="Task processing thread") + self._processor_thread.daemon = True + + self._timeout_thread = threading.Thread(target=self._task_time_interrupter, name="Task timeouts thread") + self._processor_thread.daemon = True + + self._task_lock = threading.RLock() + self._queue_lock = threading.RLock() + + self._running_event = threading.Event() + self._interrupt_event = threading.Event() + self.shutdown_event = threading.Event() + + def add_task(self, timestamp, priority, task_function, + task_args=(), task_kwargs=None, task_delayable=False): + if task_kwargs is None: + task_kwargs = {} + + heapq.heappush(self.task_queue, (timestamp, priority, + Task(task_function, task_args, task_kwargs, task_delayable) + )) + logger.debug(self.task_queue) + if self._processor_thread.is_alive(): + self._update_queue() + + def _remove_task(self, task): + with self._queue_lock: + self.task_queue.remove(task) + heapq.heapify(self.task_queue) + + def start(self, timeouts=False): + logger.info("Starting") + self._processor_thread.start() + if timeouts: + self._timeout_thread.start() + self.resume() + + def stop(self): + self.pause(interrupt=True) + with self._queue_lock: + self.task_queue = [] + + def shutdown(self): + self.stop() + self.shutdown_event.set() + self._running_event.clear() + self._processor_thread.join(timeout=5) + + def pause(self, interrupt=False): + if interrupt: + self.interrupt() + self._running_event.clear() + logger.info("Task queue paused") + + def resume(self): + self._running_event.set() + logger.info("Task queue resumed") + + def reset(self): + self.stop() + self.resume() + + def _update_queue(self): + logger.info("Queue updated") + with self._queue_lock, self._task_lock: + if self.task_queue: + if self._active_task is None: + self._active_task = self.task_queue[0] + elif self.task_queue[0] is not self._active_task: + if self.task_queue[0] < self._active_task: + self.change_active_task(self.task_queue[0]) + + def interrupt(self): + self._interrupt_event.set() + while self._interrupt_event.is_set(): + pass + logger.info("Task execution successfully interrupted") + + def change_active_task(self, task): + self.pause(interrupt=True) + + with self._task_lock: + if not self._active_task[2].delayable: + self._remove_task(self._active_task) + self._active_task = task + + self.resume() + + def execute_task(self): + with self._task_lock: + task = self._active_task[2] + start_time = self._active_task[0] + + logger.info("Waiting util task execution time:{}".format(start_time)) + wait(start_time, self._interrupt_event, 1) + + if not self._interrupt_event.is_set(): + logger.info("Executing task {}".format(task)) + try: + task.func(*task.args, interrupter=self._interrupt_event, **task.kwargs) + except Exception as e: + logger.error("Error '{}' occurred in task {}".format(e, task)) + else: + logger.warning("Task interrupted before execution") + + self._interrupt_event.clear() + + try: + logger.debug("Removing task") + self._remove_task(self._active_task) + except ValueError: + logger.warning("Task already removed, probably task changed") + else: + with self._task_lock: + self._active_task = None + + self._update_queue() + logger.info("Execution done") + + def _task_processor(self): + logger.info("Tasking thread started") + self._update_queue() # Initial tick if tasks added before start + while not self.shutdown_event.is_set(): + self._running_event.wait() + if self._active_task is not None: + self.execute_task() + + def _task_time_interrupter(self): # TODO revork; temporary disabled + raise NotImplementedError + logger.info("Timeouts thread started") + while not self.shutdown_event.is_set(): + self._running_event.wait() + try: + if self.task_queue[1] is not self._active_task: # If pending task is more important than current + if self.task_queue[1] < self._active_task: # TODO look at timeout time + logger.warning("Changing low-priority task due timeout") + self.change_active_task(self.task_queue[1]) + except IndexError: + time.sleep(0.01) + + +if __name__ == "__main__": + logger.addHandler(logging.StreamHandler()) + logger.setLevel(logging.DEBUG) + + def printer(stri, interrupter, *args, **kwargs): + logger.info("String: {}, timenow: {}".format(stri, time.time())) + wait(time.time()+30, interrupter) + + tasker = TaskManager() # Lower priority first! + tasker.start() + + tasker.add_task(0, 10, printer, ("Task1 ", )) + time.sleep(1) + tasker.add_task(time.time(), 10, printer, ("Lol ", )) + tasker.add_task(time.time()+10, 5, printer, ("Kek ", )) + tasker.add_task(time.time()+7, 1, printer, ("Dededededee", )) + time.sleep(3) + tasker.add_task(time.time()+7, 0, printer, ("Iiiiiii", )) + + while True: + pass diff --git a/Server/server_qt.py b/Server/server_qt.py index 6096b26..20bd373 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -52,6 +52,7 @@ class MainWindow(QtWidgets.QMainWindow): copter.get_response("batt_voltage", self._set_copter_data, callback_args=(row_num, 2)) copter.get_response("cell_voltage", self._set_copter_data, callback_args=(row_num, 3)) copter.get_response("selfcheck", self._set_copter_data, callback_args=(row_num, 4)) + copter.get_response("time", self._set_copter_data, callback_args=(row_num, 5)) self.ui.start_button.setEnabled(True) self.ui.takeoff_button.setEnabled(True) @@ -67,6 +68,8 @@ class MainWindow(QtWidgets.QMainWindow): model.setData(model.index(row, col), str(value)) # TODO different handling else: model.setData(model.index(row, col), str(value)) + elif col == 5: + model.setData(model.index(row, col), time.ctime(int(value))) @pyqtSlot() def send_starttime(self): diff --git a/messaging_lib.py b/messaging_lib.py index a387d85..b7e2ba3 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -297,22 +297,19 @@ class ConnectionManager(object): except KeyError: logger.warning("Request {} does not exist!".format(command)) except Exception as error: # TODO send response error\cancel - logger.error("Error during command {} execution: {}".format(command, error)) + logger.error("Error during request {} processing: {}".format(command, error)) else: self._send_response(command, request_id, value) def _process_response(self, message): request_id, requested_value = message.content["request_id"], message.content["requested_value"] with self._request_lock: - for key, value in self._request_queue.items(): + for key, value in self._request_queue.items(): # TODO as try [] if (key == request_id) and (value.requested_value == requested_value): - print(54) request = self._request_queue.pop(key) - print(request) value = message.content["value"] - print(45) logger.debug( - "Request successfully closed with value {}".format(message.content["value"]) + "Request {} successfully closed with value {}".format(request, message.content["value"]) ) f = request.callback From 176b21e329d05e20e5bef4b8182903482e1577a8 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Mon, 6 May 2019 22:41:00 +0300 Subject: [PATCH 24/47] Ros init fix with threading --- Drone/copter_client.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 1e1a15f..f10a704 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -173,5 +173,6 @@ if __name__ == "__main__": copter_client = CopterClient() task_manager = tasking.TaskManager() - task_manager.start() copter_client.start() + task_manager.start() + From bf452a3b0b2da3cb14ccf4f7fa1812350a796238 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Thu, 9 May 2019 12:36:39 +0300 Subject: [PATCH 25/47] Start fix --- Drone/copter_client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index f10a704..c71690e 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -34,11 +34,11 @@ class CopterClient(client.Client): self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds') def start(self): - super(CopterClient, self).start() logger.info("Init ROS node") rospy.init_node('Swarm_client', anonymous=True) if self.USE_LEDS: LedLib.init_led() + super(CopterClient, self).start() @messaging.request_callback("selfcheck") From ca6a1be2547bb8a7b3d69956613654bea7bd24e7 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Thu, 9 May 2019 12:45:23 +0300 Subject: [PATCH 26/47] Temporary fix --- Drone/client.py | 1 + Drone/copter_client.py | 1 + Drone/ros_logging.py | 3 ++- 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/Drone/client.py b/Drone/client.py index c8a3d0d..308ef04 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -105,6 +105,7 @@ class Client(object): return timenow def start(self): + logger.info("Starting client") try: while True: self._reconnect() diff --git a/Drone/copter_client.py b/Drone/copter_client.py index c71690e..ef080cb 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -38,6 +38,7 @@ class CopterClient(client.Client): rospy.init_node('Swarm_client', anonymous=True) if self.USE_LEDS: LedLib.init_led() + super(CopterClient, self).start() diff --git a/Drone/ros_logging.py b/Drone/ros_logging.py index 58a3716..845a9de 100644 --- a/Drone/ros_logging.py +++ b/Drone/ros_logging.py @@ -15,7 +15,8 @@ class RosHandler(logging.Handler): def emit(self, record): if "rosout" not in record.msg: try: - self.level_map[record.levelno]("%s: %s" % (record.name, record.msg)) + #self.level_map[record.levelno]("%s: %s" % (record.name, record.msg)) + print(record.levelno, record.name, record.msg) except KeyError: rospy.logerr("unknown log level %s LOG: %s: %s" % (record.levelno, record.name, record.msg)) From f2cd5c0d6ae9004f201bd4b5ae821116a845ffb1 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Thu, 9 May 2019 12:48:28 +0300 Subject: [PATCH 27/47] Ros logging... --- Drone/copter_client.py | 3 ++- Drone/ros_logging.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index ef080cb..09f56e8 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -170,10 +170,11 @@ def _play_animation(*args, **kwargs): if __name__ == "__main__": # rospy.init_node('Swarm_client', anonymous=True) - ros_logging.route_logger_to_ros() copter_client = CopterClient() task_manager = tasking.TaskManager() copter_client.start() + ros_logging.route_logger_to_ros() + ros_logging.route_logger_to_ros(logger) task_manager.start() diff --git a/Drone/ros_logging.py b/Drone/ros_logging.py index 845a9de..db1bdee 100644 --- a/Drone/ros_logging.py +++ b/Drone/ros_logging.py @@ -13,10 +13,10 @@ class RosHandler(logging.Handler): } def emit(self, record): + print(record.levelno, record.name, record.msg) if "rosout" not in record.msg: try: #self.level_map[record.levelno]("%s: %s" % (record.name, record.msg)) - print(record.levelno, record.name, record.msg) except KeyError: rospy.logerr("unknown log level %s LOG: %s: %s" % (record.levelno, record.name, record.msg)) From 48d1fee24dbfff7f9d0e1a98ef556c1f5896aa81 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Thu, 9 May 2019 12:51:19 +0300 Subject: [PATCH 28/47] f --- Drone/ros_logging.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Drone/ros_logging.py b/Drone/ros_logging.py index db1bdee..c395f3c 100644 --- a/Drone/ros_logging.py +++ b/Drone/ros_logging.py @@ -16,6 +16,7 @@ class RosHandler(logging.Handler): print(record.levelno, record.name, record.msg) if "rosout" not in record.msg: try: + pass #self.level_map[record.levelno]("%s: %s" % (record.name, record.msg)) except KeyError: rospy.logerr("unknown log level %s LOG: %s: %s" % (record.levelno, record.name, record.msg)) From dc60616035aacc590a1d4b28b5b1637cf6914985 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Sun, 12 May 2019 13:55:08 +0300 Subject: [PATCH 29/47] rl --- Drone/copter_client.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 09f56e8..1a43657 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -175,6 +175,11 @@ if __name__ == "__main__": copter_client.start() ros_logging.route_logger_to_ros() - ros_logging.route_logger_to_ros(logger) + ros_logging.route_logger_to_ros("__main__") + ros_logging.route_logger_to_ros("client") + ros_logging.route_logger_to_ros("messaging") + ros_logging.route_logger_to_ros("messaging") + + task_manager.start() From b6cc84e474d9ae5b3bfbeeab7f16704dc958e220 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Sun, 12 May 2019 14:25:55 +0300 Subject: [PATCH 30/47] Another logging approach... --- Drone/copter_client.py | 2 +- logging_lib.py | 44 ++++++++++++++++++++++++++++++++++++++++++ messaging_lib.py | 5 ++++- 3 files changed, 49 insertions(+), 2 deletions(-) create mode 100644 logging_lib.py diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 1a43657..bca5747 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -35,7 +35,7 @@ class CopterClient(client.Client): def start(self): logger.info("Init ROS node") - rospy.init_node('Swarm_client', anonymous=True) + rospy.init_node('Swarm_client', anonymous=True, log_level=rospy.DEBUG) if self.USE_LEDS: LedLib.init_led() diff --git a/logging_lib.py b/logging_lib.py new file mode 100644 index 0000000..d53100a --- /dev/null +++ b/logging_lib.py @@ -0,0 +1,44 @@ +import logging + +try: + import rospy +except ImportError: + ros = False +else: + ros = True + + +class Logger: + def __init__(self, logger=logging.getLogger(), use_ros=False): + self.ros = True if use_ros and ros else False + self.logger = logger + + def info(self, msg): + self.logger.info(msg) + + if self.ros: + rospy.loginfo(msg) + + def debug(self, msg): + self.logger.debug(msg) + + if self.ros: + rospy.logdebug(msg) + + def warning(self, msg): + self.logger.warning(msg) + + if self.ros: + rospy.logwarn(msg) + + def error(self, msg): + self.logger.error(msg) + + if self.ros: + rospy.logerr(msg) + + def critical(self, msg): + self.logger.critical(msg) + + if self.ros: + rospy.logfatal(msg) diff --git a/messaging_lib.py b/messaging_lib.py index b7e2ba3..e23f422 100644 --- a/messaging_lib.py +++ b/messaging_lib.py @@ -12,10 +12,13 @@ try: except ImportError: import selectors2 as selectors +import logging_lib + PendingRequest = collections.namedtuple("PendingRequest", ["value", "requested_value", # "expires_on", "callback", "callback_args", "callback_kwargs", ]) -logger = logging.getLogger(__name__) +_logger = logging.getLogger(__name__) +logger = logging_lib.Logger(_logger, True) class MessageManager: From 5693599c2d1cd323f9fd7d10300bac1129fb1cd3 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Sun, 12 May 2019 14:37:00 +0300 Subject: [PATCH 31/47] Selfcheck debug --- Drone/FlightLib/FlightLib.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index b5fdb7c..ce4e78c 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -52,6 +52,7 @@ def check(check_name): def inner(f): def wrapper(*args, **kwargs): failures = f(*args, **kwargs) + print(failures) if failures: msgs = [] for failure in failures: From d9944ad3e5a7e580d40cfc0392d4ba0ee434a67d Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Sun, 12 May 2019 14:42:59 +0300 Subject: [PATCH 32/47] Selfcheck --- Drone/FlightLib/FlightLib.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index ce4e78c..9d4dbc0 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -53,12 +53,14 @@ def check(check_name): def wrapper(*args, **kwargs): failures = f(*args, **kwargs) print(failures) - if failures: - msgs = [] - for failure in failures: - msg = "[{}]: Failure: {}".format(check_name, failure) - msgs.append(msg) - logger.warning(msg) + msgs = [] + for failure in failures: + print(failure) + msg = "[{}]: Failure: {}".format(check_name, failure) + msgs.append(msg) + logger.warning(msg) + + if msgs: return msgs else: logger.info("[{}]: OK".format(check_name)) From 5317902a4cf305b65a1c11e63b6114b3a0eca7d3 Mon Sep 17 00:00:00 2001 From: Arthur Date: Tue, 21 May 2019 20:03:09 +0300 Subject: [PATCH 33/47] Server: add lib import from parent directory --- Server/server.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Server/server.py b/Server/server.py index 9b04863..654a809 100644 --- a/Server/server.py +++ b/Server/server.py @@ -8,6 +8,10 @@ import selectors import collections import configparser +import os,sys,inspect # Add parent dir to PATH to import messaging_lib +current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parent_dir = os.path.dirname(current_dir) +sys.path.insert(0, parent_dir) import messaging_lib as messaging # All imports sorted in pyramid just because From 49d05962a8d868bd2ddbd280e2c587c8ce73dcc5 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Tue, 21 May 2019 21:45:42 +0300 Subject: [PATCH 34/47] Error handling fixes (errno attribute error) --- Drone/client.py | 33 +++++++++++++++++++-------------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/Drone/client.py b/Drone/client.py index 308ef04..3b8e9ea 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -125,13 +125,15 @@ class Client(object): self.client_socket.settimeout(timeout) self.client_socket.connect((self.server_host, self.server_port)) except socket.error as error: - if error.errno != errno.EINTR: - logger.warning("Can not connect due error: {}".format(error)) - attempt_count += 1 - time.sleep(timeout) - else: - logger.critical("Shutting down on keyboard interrupt") - raise KeyboardInterrupt + if isinstance(error, OSError): + if error.errno == errno.EINTR: + logger.critical("Shutting down on keyboard interrupt") + raise KeyboardInterrupt + + logger.warning("Can not connect due error: {}".format(error)) + attempt_count += 1 + time.sleep(timeout) + else: logger.info("Connection to server successful!") self._connect() @@ -187,14 +189,17 @@ class Client(object): connection = key.data try: connection.process_events(mask) + except Exception as error: - if error.errno != errno.EINTR: - logger.error( - "Exception {} occurred for {}! Resetting connection!".format(error, connection.addr) - ) - self.server_connection.close() - self.connected = False - break + logger.error( + "Exception {} occurred for {}! Resetting connection!".format(error, connection.addr) + ) + self.server_connection.close() + self.connected = False + + if isinstance(error, OSError): + if error.errno == errno.EINTR: + raise KeyboardInterrupt if not self.selector.get_map(): logger.warning("No active connections left!") From eeaab1af6fd01f0250cb51349a7c5049c58c8c05 Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Tue, 21 May 2019 21:46:20 +0300 Subject: [PATCH 35/47] Remover *args in commands --- Drone/copter_client.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index bca5747..95b29e3 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -59,7 +59,7 @@ def _response_cell(): @messaging.message_callback("service_restart") -def _command_service_restart(*args, **kwargs): +def _command_service_restart(**kwargs): os.system("systemctl restart" + kwargs["name"]) @@ -71,7 +71,7 @@ def _command_led_test(*args, **kwargs): @messaging.message_callback("takeoff") -def _command_takeoff(*args, **kwargs): +def _command_takeoff(**kwargs): task_manager.add_task(0, 0, animation.takeoff, task_kwargs={ "z": client.active_client.TAKEOFF_HEIGHT, @@ -83,7 +83,7 @@ def _command_takeoff(*args, **kwargs): @messaging.message_callback("land") -def _command_land(*args, **kwargs): +def _command_land(**kwargs): task_manager.reset() task_manager.add_task(-5, 0, animation.land, task_kwargs={ @@ -96,28 +96,28 @@ def _command_land(*args, **kwargs): @messaging.message_callback("disarm") -def _command_disarm(*args, **kwargs): +def _command_disarm(**kwargs): task_manager.reset() task_manager.add_task(-10, 0, FlightLib.arming(False)) @messaging.message_callback("stop") -def _command_stop(*args, **kwargs): +def _command_stop(**kwargs): task_manager.stop() @messaging.message_callback("pause") -def _command_stop(*args, **kwargs): +def _command_stop(**kwargs): task_manager.pause() @messaging.message_callback("resume") -def _command_stop(*args, **kwargs): +def _command_stop(**kwargs): task_manager.resume() @messaging.message_callback("start_animation") -def _play_animation(*args, **kwargs): +def _play_animation(**kwargs): gap = 0.25 start_time = kwargs["start_time"] # TODO frames = animation.load_animation(os.path.abspath("animation.csv"), From 94bc736b40bf5afe7ff0a4ae99f0c003df9c17df Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Tue, 21 May 2019 21:47:09 +0300 Subject: [PATCH 36/47] Added connection check and NaN check for all present checks --- Drone/FlightLib/FlightLib.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index 9d4dbc0..4df24da 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -72,10 +72,24 @@ def check(check_name): return inner +def _check_nans(*values): + return any(math.isnan(x) for x in values) + + +@check("FCU connection") +def check_connection(): + telemetry = get_telemetry() + if not telemetry.connected: + yield ("Flight controller is not connected!") + + @check("Linear velocity estimation") def check_linear_speeds(speed_limit=0.1): telemetry = get_telemetry(frame_id='body') + if _check_nans(telemetry.vx, telemetry.vy, telemetry.vz): + yield ("Velocity estimation is not available") + if telemetry.vx >= speed_limit: yield ("X velocity estimation: {:.3f} m/s".format(telemetry.vx)) if telemetry.vy >= speed_limit: @@ -88,6 +102,9 @@ def check_linear_speeds(speed_limit=0.1): def check_angular_speeds(rate_limit=0.05): telemetry = get_telemetry(frame_id='body') + if _check_nans(telemetry.pitch_rate, telemetry.roll_rate, telemetry.yaw_rate): + yield ("Angular velocities estimation is not available") + if telemetry.pitch_rate >= rate_limit: yield ("Pitch rate estimation: {:.3f} rad/s".format(telemetry.pitch_rate)) if telemetry.roll_rate >= rate_limit: @@ -99,6 +116,10 @@ def check_angular_speeds(rate_limit=0.05): @check("Angles estimation") def check_angles(angle_limit=math.radians(5)): telemetry = get_telemetry(frame_id='body') + + if _check_nans(telemetry.pitch, telemetry.roll, telemetry.yaw): + yield ("Angular velocities estimation is not available") + if abs(telemetry.pitch) >= angle_limit: yield ("Pitch estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.pitch, math.degrees(telemetry.pitch))) From cfffaa8c5536ea1320ecf3fad370ec6958ce79ae Mon Sep 17 00:00:00 2001 From: Artem30801 Date: Tue, 21 May 2019 21:58:46 +0300 Subject: [PATCH 37/47] Command "starttime" -> "start_animation" --- Server/server.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Server/server.py b/Server/server.py index 9b04863..40ab607 100644 --- a/Server/server.py +++ b/Server/server.py @@ -203,8 +203,7 @@ class Server: def send_starttime(self, copter, dt=0): timenow = self.time_now() print('Now:', time.ctime(timenow), "+ dt =", dt) - copter.send_message("starttime", {"time": str(timenow + dt)}) # TODO change start_on - # TODO all commands as tasks with timestamp and priority + copter.send_message("start_animation", {"time": str(timenow + dt)}) def requires_connect(f): From 132040dcb4aa851385dcb2b40eaeb9c33fb82fe1 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 30 May 2019 10:15:32 +0000 Subject: [PATCH 38/47] Drone: Simplify tasking_lib, correct some bugs --- Drone/tasking_lib.py | 143 ++++++++++++++++--------------------------- 1 file changed, 53 insertions(+), 90 deletions(-) diff --git a/Drone/tasking_lib.py b/Drone/tasking_lib.py index 365061e..3aa70ae 100644 --- a/Drone/tasking_lib.py +++ b/Drone/tasking_lib.py @@ -3,77 +3,81 @@ import time import logging import threading import collections +import itertools logger = logging.getLogger(__name__) Task = collections.namedtuple("Task", ["func", "args", "kwargs", "delayable", ]) +def wait(end, interrupter=None, maxsleep=0.1): # Added features to interrupter sleep and set max sleeping interval -def wait(end, interrupter=None, maxsleep=1): # Added features to interrupter sleep and set max sleeping interval - def interrupted(): - if interrupter is None: - return False - else: - return interrupter.is_set() + interrupted = False - while not interrupted(): # Basic implementation of pause module until() + while not interrupted: # Basic implementation of pause module until() now = time.time() diff = min(end - now, maxsleep) + if interrupter is None: + interrupted = False + else: + interrupted = interrupter.is_set() if diff <= 0: break else: time.sleep(diff / 2) + + if interrupted: + logger.warning("Waiting was interrupted!") class TaskManager(object): def __init__(self): self.task_queue = [] - self._active_task = None + self._counter = itertools.count() # unique sequence count self._processor_thread = threading.Thread(target=self._task_processor, name="Task processing thread") self._processor_thread.daemon = True - - self._timeout_thread = threading.Thread(target=self._task_time_interrupter, name="Task timeouts thread") - self._processor_thread.daemon = True - - self._task_lock = threading.RLock() - self._queue_lock = threading.RLock() + self._task_queue_lock = threading.RLock() self._running_event = threading.Event() self._interrupt_event = threading.Event() - self.shutdown_event = threading.Event() + self._shutdown_event = threading.Event() def add_task(self, timestamp, priority, task_function, task_args=(), task_kwargs=None, task_delayable=False): + + self._interrupt_event.set() + if task_kwargs is None: task_kwargs = {} - heapq.heappush(self.task_queue, (timestamp, priority, - Task(task_function, task_args, task_kwargs, task_delayable) - )) - logger.debug(self.task_queue) - if self._processor_thread.is_alive(): - self._update_queue() + task = Task(task_function, task_args, task_kwargs, task_delayable) - def _remove_task(self, task): - with self._queue_lock: - self.task_queue.remove(task) - heapq.heapify(self.task_queue) + count = next(self._counter) + entry = (timestamp, priority, count, task) + + with self._task_queue_lock: + heapq.heappush(self.task_queue, entry) + + print(self.task_queue) + + def pop_task(self): + with self._task_queue_lock: + if self.task_queue: + return heapq.heappop(self.task_queue) + raise KeyError('Pop from an empty priority queue') def start(self, timeouts=False): - logger.info("Starting") + logger.info("Task manager is started") self._processor_thread.start() - if timeouts: - self._timeout_thread.start() self.resume() def stop(self): self.pause(interrupt=True) - with self._queue_lock: - self.task_queue = [] + with self._task_queue_lock: + del self.task_queue[:] def shutdown(self): self.stop() - self.shutdown_event.set() + self._shutdown_event.set() self._running_event.clear() self._processor_thread.join(timeout=5) @@ -91,36 +95,18 @@ class TaskManager(object): self.stop() self.resume() - def _update_queue(self): - logger.info("Queue updated") - with self._queue_lock, self._task_lock: - if self.task_queue: - if self._active_task is None: - self._active_task = self.task_queue[0] - elif self.task_queue[0] is not self._active_task: - if self.task_queue[0] < self._active_task: - self.change_active_task(self.task_queue[0]) - def interrupt(self): self._interrupt_event.set() while self._interrupt_event.is_set(): pass logger.info("Task execution successfully interrupted") - def change_active_task(self, task): - self.pause(interrupt=True) - - with self._task_lock: - if not self._active_task[2].delayable: - self._remove_task(self._active_task) - self._active_task = task - - self.resume() - def execute_task(self): - with self._task_lock: - task = self._active_task[2] - start_time = self._active_task[0] + with self._task_queue_lock: + if self.task_queue: + start_time, priority, count, task = self.task_queue[0] + else: + return logger.info("Waiting util task execution time:{}".format(start_time)) wait(start_time, self._interrupt_event, 1) @@ -133,42 +119,20 @@ class TaskManager(object): logger.error("Error '{}' occurred in task {}".format(e, task)) else: logger.warning("Task interrupted before execution") + self._interrupt_event.clear() + return - self._interrupt_event.clear() + if time.time() > start_time: + self.pop_task() - try: - logger.debug("Removing task") - self._remove_task(self._active_task) - except ValueError: - logger.warning("Task already removed, probably task changed") - else: - with self._task_lock: - self._active_task = None - - self._update_queue() - logger.info("Execution done") + logger.info("Execution done") def _task_processor(self): logger.info("Tasking thread started") - self._update_queue() # Initial tick if tasks added before start - while not self.shutdown_event.is_set(): + # self._update_queue() # Initial tick if tasks added before start + while not self._shutdown_event.is_set(): self._running_event.wait() - if self._active_task is not None: - self.execute_task() - - def _task_time_interrupter(self): # TODO revork; temporary disabled - raise NotImplementedError - logger.info("Timeouts thread started") - while not self.shutdown_event.is_set(): - self._running_event.wait() - try: - if self.task_queue[1] is not self._active_task: # If pending task is more important than current - if self.task_queue[1] < self._active_task: # TODO look at timeout time - logger.warning("Changing low-priority task due timeout") - self.change_active_task(self.task_queue[1]) - except IndexError: - time.sleep(0.01) - + self.execute_task() if __name__ == "__main__": logger.addHandler(logging.StreamHandler()) @@ -179,15 +143,14 @@ if __name__ == "__main__": wait(time.time()+30, interrupter) tasker = TaskManager() # Lower priority first! - tasker.start() - tasker.add_task(0, 10, printer, ("Task1 ", )) + tasker.start() + tasker.add_task(time.time(), 10, printer, ("Task1 ", )) + tasker.add_task(time.time()+10, 5, printer, ("Task2 ", )) time.sleep(1) - tasker.add_task(time.time(), 10, printer, ("Lol ", )) - tasker.add_task(time.time()+10, 5, printer, ("Kek ", )) - tasker.add_task(time.time()+7, 1, printer, ("Dededededee", )) + tasker.add_task(time.time()+7, 1, printer, ("Task3", )) time.sleep(3) - tasker.add_task(time.time()+7, 0, printer, ("Iiiiiii", )) + tasker.add_task(time.time()+7, 0, printer, ("Task4", )) while True: pass From b9f2a043fecabd62fc65f688a8c3bb3e895fe71d Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 30 May 2019 10:18:37 +0000 Subject: [PATCH 39/47] Drone: modify task_manager start --- Drone/copter_client.py | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 95b29e3..c4277cd 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -14,9 +14,6 @@ import animation_lib as animation import ros_logging -logger = logging.getLogger(__name__) - - class CopterClient(client.Client): def load_config(self): super(CopterClient, self).load_config() @@ -33,11 +30,13 @@ class CopterClient(client.Client): self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds') - def start(self): - logger.info("Init ROS node") + def start(self, task_manager_instance): + client.logger.info("Init ROS node") rospy.init_node('Swarm_client', anonymous=True, log_level=rospy.DEBUG) if self.USE_LEDS: LedLib.init_led() + + task_manager_instance.start() super(CopterClient, self).start() @@ -85,7 +84,7 @@ def _command_takeoff(**kwargs): @messaging.message_callback("land") def _command_land(**kwargs): task_manager.reset() - task_manager.add_task(-5, 0, animation.land, + task_manager.add_task(0, 0, animation.land, task_kwargs={ "z": client.active_client.TAKEOFF_HEIGHT, "timeout": client.active_client.TAKEOFF_TIME, @@ -172,14 +171,17 @@ if __name__ == "__main__": # rospy.init_node('Swarm_client', anonymous=True) copter_client = CopterClient() task_manager = tasking.TaskManager() - - copter_client.start() + + #print logging.root.manager.loggerDict + #task_manager.start() + copter_client.start(task_manager) + loggers = [logging.getLogger(name) for name in logging.root.manager.loggerDict] + if loggers is None: + print("No loggers!") + else: + print("Loggers",loggers) ros_logging.route_logger_to_ros() ros_logging.route_logger_to_ros("__main__") ros_logging.route_logger_to_ros("client") ros_logging.route_logger_to_ros("messaging") - ros_logging.route_logger_to_ros("messaging") - - - task_manager.start() From 79b120aa14c44ac90dcc0e67df7a22cf194ea557 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Thu, 30 May 2019 17:05:12 +0000 Subject: [PATCH 40/47] Drone: remove interrupt function with infinite loop from tasking_lib --- Drone/tasking_lib.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/Drone/tasking_lib.py b/Drone/tasking_lib.py index 3aa70ae..47f7a57 100644 --- a/Drone/tasking_lib.py +++ b/Drone/tasking_lib.py @@ -66,12 +66,13 @@ class TaskManager(object): raise KeyError('Pop from an empty priority queue') def start(self, timeouts=False): + print("Task manager is started") logger.info("Task manager is started") self._processor_thread.start() self.resume() def stop(self): - self.pause(interrupt=True) + self.pause() with self._task_queue_lock: del self.task_queue[:] @@ -82,8 +83,8 @@ class TaskManager(object): self._processor_thread.join(timeout=5) def pause(self, interrupt=False): - if interrupt: - self.interrupt() +# if interrupt: +# self.interrupt() self._running_event.clear() logger.info("Task queue paused") @@ -95,11 +96,11 @@ class TaskManager(object): self.stop() self.resume() - def interrupt(self): - self._interrupt_event.set() - while self._interrupt_event.is_set(): - pass - logger.info("Task execution successfully interrupted") +# def interrupt(self): +# self._interrupt_event.set() +# while self._interrupt_event.is_set(): +# pass +# logger.info("Task execution successfully interrupted") def execute_task(self): with self._task_queue_lock: From c9960fdbdcf9ba98432a5eb0e35c5aa49b563088 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 31 May 2019 13:37:14 +0000 Subject: [PATCH 41/47] animation_lib: delete unnecessary condition --- Drone/animation_lib.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index f60de92..d98ca03 100644 --- a/Drone/animation_lib.py +++ b/Drone/animation_lib.py @@ -45,11 +45,7 @@ def convert_frame(frame): def execute_frame(point=(), color=(), yaw=float('Nan'), frame_id='aruco_map', use_leds=True, - flight_func=FlightLib.navto, flight_kwargs=None, interrupter=interrupt_event): - - if flight_kwargs is None: - flight_kwargs = {} - + flight_func=FlightLib.navto, flight_kwargs={}, interrupter=interrupt_event): flight_func(*point, yaw=yaw, frame_id=frame_id, interrupter=interrupt_event, **flight_kwargs) if use_leds: if color: From d09b3d4e548a910321e5ee01e786470c2cd81ac8 Mon Sep 17 00:00:00 2001 From: Arthur Date: Tue, 4 Jun 2019 17:55:35 +0300 Subject: [PATCH 42/47] LedLib: make led functions interruptable --- Drone/FlightLib/LedLib.py | 80 ++++++++++++++++++++++++--------------- 1 file changed, 50 insertions(+), 30 deletions(-) diff --git a/Drone/FlightLib/LedLib.py b/Drone/FlightLib/LedLib.py index 4215bef..ef3b7a9 100644 --- a/Drone/FlightLib/LedLib.py +++ b/Drone/FlightLib/LedLib.py @@ -1,8 +1,8 @@ from __future__ import print_function -from threading import Thread +import threading import time from rpi_ws281x import * - +from tasking_lib import wait as wait_until # LED strip configuration: LED_COUNT = 29 # Number of LED pixels. LED_PIN = 21 # GPIO pin connected to the pixels (18 uses PWM!) (10 uses SPI /dev/spidev0.0). @@ -29,6 +29,15 @@ direct = False l = 0 wait_ms = 10 +INTERRUPTER = threading.Event() +INTERRUPTER_UNSET = threading.Event() + +def delay(delay_time, interrupter=INTERRUPTER, maxsleep=0.01): + global mode + wait_until(time.time()+delay_time, interrupter, maxsleep) + if interrupter.is_set(): + mode = "off" + # functions def math_wheel(pos): @@ -43,52 +52,57 @@ def math_wheel(pos): return Color(0, pos * 3, 255 - pos * 3) -def rainbow(wait=10, direction=False): - global wait_ms, direct, mode +def rainbow(wait=10, direction=False, interrupter=INTERRUPTER): + global wait_ms, direct, mode, INTERRUPTER wait_ms = wait direct = direction mode = "rainbow" + INTERRUPTER=interrupter -def fill(red, green, blue): - global r, g, b, mode +def fill(red, green, blue, interrupter=INTERRUPTER): + global r, g, b, mode, INTERRUPTER r = red g = green b = blue mode = "fill" + INTERRUPTER=interrupter -def blink(red, green, blue, wait=250): - global r, g, b, wait_ms, mode +def blink(red, green, blue, wait=250, interrupter=INTERRUPTER): + global r, g, b, wait_ms, mode, INTERRUPTER r = red g = green b = blue wait_ms = wait mode = "blink" + INTERRUPTER=interrupter -def chase(red, green, blue, wait=50, direction=False): - global r, g, b, wait_ms, direct, mode +def chase(red, green, blue, wait=50, direction=False, interrupter=INTERRUPTER): + global r, g, b, wait_ms, direct, mode, INTERRUPTER r = red g = green b = blue wait_ms = wait direct = direction mode = "chase" + INTERRUPTER=interrupter -def wipe_to(red, green, blue, wait=50, direction=False): - global r, g, b, wait_ms, direct, mode +def wipe_to(red, green, blue, wait=50, direction=False, interrupter=INTERRUPTER): + global r, g, b, wait_ms, direct, mode, INTERRUPTER r = red g = green b = blue wait_ms = wait direct = direction mode = "wipe_to" + INTERRUPTER=interrupter -def fade_to(red, green, blue, wait=20): # do not working with rainbow (solid colors only) - global r, g, b, r_prev, g_prev, b_prev, wait_ms, mode +def fade_to(red, green, blue, wait=20, interrupter=INTERRUPTER): # do not working with rainbow (solid colors only) + global r, g, b, r_prev, g_prev, b_prev, wait_ms, mode, INTERRUPTER r_prev = r g_prev = g b_prev = b @@ -97,10 +111,11 @@ def fade_to(red, green, blue, wait=20): # do not working with rainbow (solid co b = blue wait_ms = wait mode = "fade_to" + INTERRUPTER=interrupter -def run(red, green, blue, length=strip.numPixels(), direction=False, wait=25): - global r, g, b, l, wait_ms, direct, mode +def run(red, green, blue, length=strip.numPixels(), direction=False, wait=25, interrupter=INTERRUPTER): + global r, g, b, l, wait_ms, direct, mode, INTERRUPTER r = red g = green b = blue @@ -108,6 +123,7 @@ def run(red, green, blue, length=strip.numPixels(), direction=False, wait=25): direct = direction wait_ms = wait mode = "run" + INTERRUPTER=interrupter def off(): @@ -128,27 +144,29 @@ def strip_rainbow_frame(iteration, direction): strip.show() -def strip_chase_step(color, direction): +def strip_chase_step(color, direction, interrupter=INTERRUPTER): for q in range(3): for i in range(0, strip.numPixels(), 3): n = ((strip.numPixels() - 1) * direction) - (i + q) strip.setPixelColor(abs(n), color) strip.show() - time.sleep(wait_ms / 1000.0) + delay(wait_ms / 1000.0, interrupter) for i in range(0, strip.numPixels(), 3): n = ((strip.numPixels() - 1) * direction) - (i + q) strip.setPixelColor(abs(n), 0) -def strip_wipe(color, direction): +def strip_wipe(color, direction, interrupter=INTERRUPTER): for i in range(strip.numPixels()): n = ((strip.numPixels() - 1) * direction) - i strip.setPixelColor(abs(n), color) - time.sleep(wait_ms / 1000.0) + delay(wait_ms / 1000.0, interrupter) + if interrupter.is_set(): + return strip.show() -def strip_fade(r1, g1, b1, r2, g2, b2, frames=50): +def strip_fade(r1, g1, b1, r2, g2, b2, frames=50, interrupter=INTERRUPTER): r_delta = (r2-r1)//frames g_delta = (g2-g1)//frames b_delta = (b2-b1)//frames @@ -157,7 +175,9 @@ def strip_fade(r1, g1, b1, r2, g2, b2, frames=50): green = g1 + (g_delta * i) blue = b1 + (b_delta * i) strip_set(Color(red, green, blue)) - time.sleep(wait_ms / 1000.0) + delay(wait_ms / 1000.0, interrupter) + if interrupter.is_set(): + return strip_set(Color(r2, g2, b2)) @@ -190,39 +210,39 @@ def led_thread(): if iteration >= 256: iteration = 0 strip_rainbow_frame(iteration, direct) - time.sleep(wait_ms / 1000.0) + delay(wait_ms / 1000.0, INTERRUPTER) iteration += 1 elif mode == "fill": strip_set(Color(r, g, b)) mode = "" elif mode == "blink": strip_set(Color(r, g, b)) - time.sleep(wait_ms / 1000.0) + delay(wait_ms / 1000.0, INTERRUPTER) strip_set(Color(0, 0, 0)) - time.sleep(wait_ms / 1000.0) + delay(wait_ms / 1000.0, INTERRUPTER) elif mode == "chase": strip_chase_step(Color(r, g, b), direct) elif mode == "wipe_to": - strip_wipe(Color(r, g, b,), direct) + strip_wipe(Color(r, g, b,), direct, INTERRUPTER) mode = "fill" elif mode == "fade_to": - strip_fade(r_prev, g_prev, b_prev, r, g, b) + strip_fade(r_prev, g_prev, b_prev, r, g, b, interrupter=INTERRUPTER) mode = "" elif mode == "run": strip_run_step(r, g, b, l, direct, iteration) - time.sleep(wait_ms / 1000.0) + delay(wait_ms / 1000.0, INTERRUPTER) iteration += 1 elif mode == "off": strip_off() mode = "" else: - time.sleep(1 / 1000) + delay(1 / 1000.0, interrupter=INTERRUPTER_UNSET) # init def init_led(): strip.begin() - t_l = Thread(target=led_thread) + t_l = threading.Thread(target=led_thread) t_l.daemon = True t_l.start() From 01ac1743978d1585221c2face66dbdc96a91c5e7 Mon Sep 17 00:00:00 2001 From: Arthur Date: Tue, 4 Jun 2019 17:59:45 +0300 Subject: [PATCH 43/47] Add more debug information and take out global variables as default values --- Drone/FlightLib/FlightLib.py | 144 ++++++++++++++++++++++------------- 1 file changed, 92 insertions(+), 52 deletions(-) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index 4df24da..196bd28 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -25,17 +25,27 @@ landing = rospy.ServiceProxy('/land', Trigger) logger.info("Proxy services inited") # globals -FREQUENCY = 1000 / 25 # HZ +FREQUENCY = 40 # HZ TOLERANCE = 0.2 - -interrupt_event = threading.Event() +SPEED = 1.0 +SPEED_TAKEOFF = 0.8 +TIMEOUT = 5.0 +TIMEOUT_ARMED = 2.0 +TIMEOUT_DESCEND = TIMEOUT +TIMEOUT_LAND = 8.0 +Z_DESCEND = 0.5 +Z_TAKEOFF = 1.0 +FRAME_ID = 'map' +INTERRUPTER = threading.Event() checklist = [] +def arming_wrapper(state=False, interrupter=INTERRUPTER): + arming(state) def interrupt(): logger.info("Performing function interrupt") - interrupt_event.set() + INTERRUPTER.set() def init(node_name="CleverSwarmFlight", anon=True, no_signals=True): @@ -140,174 +150,204 @@ def selfcheck(): return checks -def navto(x, y, z, yaw=float('nan'), frame_id='aruco_map', **kwargs): +def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, **kwargs): set_position(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw) telemetry = get_telemetry(frame_id=frame_id) logger.info('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) + print('Going to: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) logger.info('Telemetry now: | z: {:.3f}'.format(telemetry.z)) + print('Telemetry now: | z: {:.3f}'.format(telemetry.z)) return True -def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLERANCE, frame_id='aruco_map', - freq=FREQUENCY, timeout=5000, wait=False, interrupter=interrupt_event): +def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, + freq=FREQUENCY, timeout=TIMEOUT, interrupter=INTERRUPTER, wait=False): logger.info('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) + print('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw)) navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed) # waiting for completion telemetry = get_telemetry(frame_id=frame_id) rate = rospy.Rate(freq) - time_start = rospy.get_rostime() + time_start = time.time() while (get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z) > tolerance) or wait: if interrupter.is_set(): - logger.warning("Flight function interrupted!") + logger.warning("Reach point function interrupted!") + print("Reach point function interrupted!") interrupter.clear() - return + return False telemetry = get_telemetry(frame_id=frame_id) logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) + print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( + telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) logger.info('Current delta: | {:.3f}'.format( get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z))) + print('Current delta: | {:.3f}'.format( + get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z))) - time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 + time_passed = time.time() - time_start if timeout is not None: if time_passed >= timeout: - logger.warning('Reaching point timed out! | time: {:3f} seconds'.format(time_passed / 1000)) + logger.warning('Reaching point timed out! | time: {:3f} seconds'.format(time_passed)) + print('Reaching point timed out! | time: {:3f} seconds'.format(time_passed)) return wait rate.sleep() - else: - logger.info("Point reached!") - return True + + logger.info("Point reached!") + print("Point reached!") + return True -def reach_attitude(z=0.0, yaw=float('nan'), speed=1.0, tolerance=TOLERANCE, frame_id='aruco_map', - freq=FREQUENCY, timeout=5000, wait=False, interrupter=interrupt_event): +def reach_altitude(z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, + freq=FREQUENCY, timeout=TIMEOUT, interrupter=INTERRUPTER, wait=False): logger.info('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw)) + print('Reaching attitude: | z: {:.3f} yaw: {:.3f}'.format(z, yaw)) current_telem = get_telemetry(frame_id=frame_id) navigate(frame_id=frame_id, x=current_telem.x, y=current_telem.y, z=z, yaw=yaw, speed=speed) - # waiting for completion telemetry = get_telemetry(frame_id=frame_id) rate = rospy.Rate(freq) - time_start = rospy.get_rostime() + time_start = time.time() while (abs(z - telemetry.z) > tolerance) or wait: if interrupter.is_set(): - logger.warning("Flight function interrupted!") + logger.warning("Reach altitude function interrupted!") + print("Reach altitude function interrupted!") interrupter.clear() return telemetry = get_telemetry(frame_id=frame_id) logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) + print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format( + telemetry.x, telemetry.y, telemetry.z, telemetry.yaw)) - time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 + time_passed = time.time() - time_start if timeout is not None: if time_passed >= timeout: - logger.warning('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed / 1000)) + logger.warning('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed)) + print('Reaching attitude timed out! | time: {:3f} seconds'.format(time_passed)) return wait rate.sleep() - else: - logger.info("Attitude reached!") - return True + + logger.info("Altitude reached!") + print("Altitude reached!") + return True -def stop(frame_id='body', hold_speed=1.0): +def stop(frame_id='body', hold_speed=SPEED): navigate(frame_id=frame_id, yaw=float('nan'), speed=hold_speed) -def land(descend=True, z=1.0, frame_id_descend="aruco_map", frame_id_land="aruco_map", - timeout_descend=5000, timeout_land=7500, freq=FREQUENCY, interrupter=interrupt_event): +def land(descend=True, z=Z_DESCEND, frame_id_descend=FRAME_ID, frame_id_land=FRAME_ID, + timeout_descend=TIMEOUT_DESCEND, timeout_land=TIMEOUT_LAND, freq=FREQUENCY, interrupter=INTERRUPTER): if descend: logger.info("Descending to: | z: {:.3f}".format(z)) - reach_attitude(z=z, frame_id=frame_id_descend, timeout=timeout_descend, freq=freq, yaw=1.57, # TODO yaw + print("Descending to: | z: {:.3f}".format(z)) + reach_altitude(z=z, frame_id=frame_id_descend, timeout=timeout_descend, freq=freq, yaw=float('nan'), # TODO yaw interrupter=interrupter) landing() + print("Land is started") - telemetry = get_telemetry(frame_id='aruco_map') + telemetry = get_telemetry(frame_id=frame_id_land) rate = rospy.Rate(freq) - time_start = rospy.get_rostime() + time_start = time.time() while telemetry.armed: if interrupter.is_set(): - logger.warning("Flight function interrupted!") + logger.warning("Land function interrupted!") + print("Land function interrupted!") interrupter.clear() - return + return False telemetry = get_telemetry(frame_id=frame_id_land) logger.info("Landing... | z: {}".format(telemetry.z)) - time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 + print("Landing... | z: {}".format(telemetry.z)) + time_passed = time.time() - time_start if timeout_land is not None: if time_passed >= timeout_land: - logger.warning('Landing timed out! | time: {:3f} seconds'.format(time_passed / 1000)) + logger.warning('Landing timed out! | time: {:3f} seconds'.format(time_passed)) logger.warning("Disarming!") + print("Landing timed out, disarming!!!") arming(False) return False rate.sleep() - else: - logger.info("Landing succeeded!") - return True + + logger.info("Landing succeeded!") + print("Landing succeeded!") + return True -def takeoff(z=1.0, speed=0.8, frame_id='body', freq=FREQUENCY, - timeout_arm=2000, timeout_takeoff=5000, wait=False, tolerance=TOLERANCE, emergency_land=False, - interrupter=interrupt_event): +def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY, + timeout_arm=TIMEOUT_ARMED, timeout_takeoff=TIMEOUT, wait=False, tolerance=TOLERANCE, emergency_land=False, + interrupter=INTERRUPTER): logger.info("Starting takeoff!") + print("Starting takeoff!") logger.info("Arming, going to OFFBOARD mode") # Arming check set_rates(thrust=0.1, auto_arm=True) telemetry = get_telemetry(frame_id=frame_id) rate = rospy.Rate(freq) - time_start = rospy.get_rostime() + time_start = time.time() while (not telemetry.armed) or wait: if interrupter.is_set(): - logger.warning("Flight function interrupted!") + logger.warning("Takeoff function interrupted!") + print("Takeoff function interrupted!") interrupter.clear() return telemetry = get_telemetry(frame_id=frame_id) logger.info("Arming...") - time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 + print("Arming...") + time_passed = time.time() - time_start if timeout_arm is not None: if time_passed >= timeout_arm: if not telemetry.armed: - logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed / 1000)) + logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed)) + print('Arming timed out! | time: {:3f} seconds'.format(time_passed)) return False else: break rate.sleep() logger.info("Armed!") + print("Armed") # Reach height z0 = get_telemetry().z z_dest = z + z0 - navigate(z=z_dest, speed=speed, frame_id=frame_id, auto_arm=True) - current_height = abs(get_telemetry().z - z_dest) - while current_height > tolerance or wait: + navigate(z=z, speed=speed, frame_id=frame_id, auto_arm=True) + current_diff = abs(get_telemetry().z - z_dest) + while (current_diff > tolerance) or wait: if interrupter.is_set(): logger.warning("Flight function interrupted!") + print("Flight function interrupted!") interrupter.clear() return - current_height = abs(get_telemetry().z - z_dest) + current_diff = abs(get_telemetry().z - z_dest) logger.info("Takeoff...") - time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 + print("Takeoff...") + time_passed = time.time() - time_start if timeout_takeoff is not None: if time_passed >= timeout_takeoff: if not wait: - logger.warning('Takeoff timed out! | time: {:3f} seconds'.format(time_passed / 1000)) + logger.warning('Takeoff timed out! | time: {:3f} seconds'.format(time_passed)) + print('Takeoff timed out! | time: {:3f} seconds'.format(time_passed)) if emergency_land: logger.info("Preforming emergency land") + print("Preforming emergency land") land(descend=False, interrupter=interrupter) return False else: @@ -315,5 +355,5 @@ def takeoff(z=1.0, speed=0.8, frame_id='body', freq=FREQUENCY, rate.sleep() logger.info("Takeoff succeeded!") - # print("Takeoff succeeded!") + print("Takeoff succeeded!") return True From 5f5ce277d053caf43f64ba31416ce27d2d89f5d5 Mon Sep 17 00:00:00 2001 From: Arthur Date: Tue, 4 Jun 2019 18:01:56 +0300 Subject: [PATCH 44/47] Make takeoff function interruptable --- Drone/animation_lib.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index d98ca03..9a9a2af 100644 --- a/Drone/animation_lib.py +++ b/Drone/animation_lib.py @@ -69,18 +69,23 @@ def execute_animation(frames, frame_delay, frame_id='aruco_map', use_leds=True, def takeoff(z=1.5, safe_takeoff=True, timeout=5000, frame_id='aruco_map', use_leds=True, interrupter=interrupt_event): + print(interrupter.is_set()) if use_leds: - LedLib.wipe_to(255, 0, 0) + LedLib.wipe_to(255, 0, 0, interrupter=interrupter) + if interrupter.is_set(): + return FlightLib.takeoff(z=z, wait=False, timeout_takeoff=timeout, frame_id=frame_id, emergency_land=safe_takeoff, interrupter=interrupter) + if interrupter.is_set(): + return if use_leds: - LedLib.blink(0, 255, 0, wait=50) + LedLib.blink(0, 255, 0, wait=50, interrupter=interrupter) def land(z=1.5, descend=False, timeout=5000, frame_id='aruco_map', use_leds=True, interrupter=interrupt_event): if use_leds: - LedLib.blink(255, 0, 0) + LedLib.blink(255, 0, 0, interrupter=interrupter) FlightLib.land(z=z, descend=descend, timeout_land=timeout, frame_id_land=frame_id, interrupter=interrupter) if use_leds: LedLib.off() From f45a0ba1b15b5e7e81e742aaa4244bb3a6de0117 Mon Sep 17 00:00:00 2001 From: Arthur Date: Tue, 4 Jun 2019 18:05:07 +0300 Subject: [PATCH 45/47] copter_client: little corrections --- Drone/copter_client.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/Drone/copter_client.py b/Drone/copter_client.py index c4277cd..ae46194 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -71,7 +71,7 @@ def _command_led_test(*args, **kwargs): @messaging.message_callback("takeoff") def _command_takeoff(**kwargs): - task_manager.add_task(0, 0, animation.takeoff, + task_manager.add_task(time.time(), 0, animation.takeoff, task_kwargs={ "z": client.active_client.TAKEOFF_HEIGHT, "timeout": client.active_client.TAKEOFF_TIME, @@ -97,7 +97,11 @@ def _command_land(**kwargs): @messaging.message_callback("disarm") def _command_disarm(**kwargs): task_manager.reset() - task_manager.add_task(-10, 0, FlightLib.arming(False)) + task_manager.add_task(-5, 0, FlightLib.arming_wrapper, + task_kwargs={ + "state": False + } + ) @messaging.message_callback("stop") @@ -115,10 +119,11 @@ def _command_stop(**kwargs): task_manager.resume() -@messaging.message_callback("start_animation") +@messaging.message_callback("start") def _play_animation(**kwargs): - gap = 0.25 - start_time = kwargs["start_time"] # TODO + gap = 0.25 + start_time = kwargs["time"] # TODO +""" print('start time = {}'.format(start_time)) frames = animation.load_animation(os.path.abspath("animation.csv"), x0=client.active_client.X0 + client.active_client.X0_COMMON, y0=client.active_client.Y0 + client.active_client.Y0_COMMON, @@ -164,7 +169,7 @@ def _play_animation(**kwargs): "frame_id": client.active_client.FRAME_ID, "use_leds": client.active_client.USE_LEDS, }, - ) + ) """ if __name__ == "__main__": From 3c1785477ce319cfb8895ad351168952f5c566f8 Mon Sep 17 00:00:00 2001 From: Arthur Date: Tue, 4 Jun 2019 18:08:50 +0300 Subject: [PATCH 46/47] tasking_lib: improve interruption, now it looks like working --- Drone/tasking_lib.py | 88 +++++++++++++++++++++++++++++++------------- 1 file changed, 62 insertions(+), 26 deletions(-) diff --git a/Drone/tasking_lib.py b/Drone/tasking_lib.py index 47f7a57..42bfbf4 100644 --- a/Drone/tasking_lib.py +++ b/Drone/tasking_lib.py @@ -8,24 +8,20 @@ import itertools logger = logging.getLogger(__name__) Task = collections.namedtuple("Task", ["func", "args", "kwargs", "delayable", ]) -def wait(end, interrupter=None, maxsleep=0.1): # Added features to interrupter sleep and set max sleeping interval +INTERRUPTER = threading.Event() - interrupted = False +def wait(end, interrupter=INTERRUPTER, maxsleep=0.1): # Added features to interrupter sleep and set max sleeping interval - while not interrupted: # Basic implementation of pause module until() + while not interrupter.is_set(): # Basic implementation of pause module until() now = time.time() diff = min(end - now, maxsleep) - if interrupter is None: - interrupted = False - else: - interrupted = interrupter.is_set() if diff <= 0: break else: time.sleep(diff / 2) - - if interrupted: + else: logger.warning("Waiting was interrupted!") + print("Waiting was interrupted!") class TaskManager(object): @@ -38,24 +34,41 @@ class TaskManager(object): self._task_queue_lock = threading.RLock() self._running_event = threading.Event() - self._interrupt_event = threading.Event() + self._reset_event = threading.Event() + self._wait_interrupt_event = threading.Event() + self._task_interrupt_event = threading.Event() self._shutdown_event = threading.Event() def add_task(self, timestamp, priority, task_function, - task_args=(), task_kwargs=None, task_delayable=False): + task_args=(), task_kwargs={}, task_delayable=False): - self._interrupt_event.set() + self._wait_interrupt_event.set() + self._running_event.clear() - if task_kwargs is None: - task_kwargs = {} - - task = Task(task_function, task_args, task_kwargs, task_delayable) + task = Task(task_function, task_args, task_kwargs, task_delayable) count = next(self._counter) entry = (timestamp, priority, count, task) - + with self._task_queue_lock: + if self.task_queue: + entry_old = self.task_queue[0] + else: + entry_old = entry + heapq.heappush(self.task_queue, entry) + + if self.task_queue[0] != entry_old: + self._task_interrupt_event.set() + print("Task queue updated with more priority task") + + if self._reset_event.is_set(): + self._task_interrupt_event.set() + self._reset_event.clear() + print("Task queue updated after reset") + + self._wait_interrupt_event.clear() + self._running_event.set() print(self.task_queue) @@ -72,29 +85,38 @@ class TaskManager(object): self.resume() def stop(self): - self.pause() + self.pause(interrupt=True) with self._task_queue_lock: del self.task_queue[:] + print self.task_queue def shutdown(self): self.stop() self._shutdown_event.set() + self._wait_interrupt_event.set() + self._task_interrupt_event.set() self._running_event.clear() self._processor_thread.join(timeout=5) - def pause(self, interrupt=False): -# if interrupt: -# self.interrupt() + def pause(self, interrupt=True): + if interrupt: + self._wait_interrupt_event.set() + self._task_interrupt_event.set() self._running_event.clear() logger.info("Task queue paused") + print("Task queue paused") def resume(self): self._running_event.set() + self._wait_interrupt_event.clear() + self._task_interrupt_event.clear() logger.info("Task queue resumed") + print("Task queue resumed") def reset(self): self.stop() self.resume() + self._reset_event.set() # def interrupt(self): # self._interrupt_event.set() @@ -110,23 +132,37 @@ class TaskManager(object): return logger.info("Waiting util task execution time:{}".format(start_time)) - wait(start_time, self._interrupt_event, 1) + print("Waiting util task execution time:{}".format(start_time)) + wait(start_time, self._wait_interrupt_event) - if not self._interrupt_event.is_set(): + if not self._wait_interrupt_event.is_set(): logger.info("Executing task {}".format(task)) + print("Executing task {}".format(task)) try: - task.func(*task.args, interrupter=self._interrupt_event, **task.kwargs) + task.func(*task.args, interrupter=self._task_interrupt_event, **task.kwargs) except Exception as e: logger.error("Error '{}' occurred in task {}".format(e, task)) + print("Error '{}' occurred in task {}".format(e, task)) else: logger.warning("Task interrupted before execution") - self._interrupt_event.clear() + print("Task interrupted before execution") + self._wait_interrupt_event.clear() return if time.time() > start_time: - self.pop_task() + start_time_n, priority_n, count_n, task_n = self.task_queue[0] + if (task_n == task) and (start_time_n == start_time): + self.pop_task() + try: + print("Pop {} function!".format(task.func.__name__)) + except Exception as e: + print("Pop something!") + + if self._task_interrupt_event.is_set(): + self._task_interrupt_event.clear() logger.info("Execution done") + print("Execution done") def _task_processor(self): logger.info("Tasking thread started") From b80a19cb9677c76204c7bc64dedba39f7d284cef Mon Sep 17 00:00:00 2001 From: Arthur Date: Tue, 4 Jun 2019 18:15:00 +0300 Subject: [PATCH 47/47] Server: change start_animation command name to start --- Server/server.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Server/server.py b/Server/server.py index d909837..b1ee126 100644 --- a/Server/server.py +++ b/Server/server.py @@ -207,7 +207,7 @@ class Server: def send_starttime(self, copter, dt=0): timenow = self.time_now() print('Now:', time.ctime(timenow), "+ dt =", dt) - copter.send_message("start_animation", {"time": str(timenow + dt)}) + copter.send_message("start", {"time": str(timenow + dt)}) def requires_connect(f):