diff --git a/Drone/FlightLib2/FlightLib.py b/Drone/FlightLib2/FlightLib.py new file mode 100644 index 0000000..a41cbd7 --- /dev/null +++ b/Drone/FlightLib2/FlightLib.py @@ -0,0 +1,270 @@ +#!/usr/bin/python +from __future__ import print_function +import sys +import math +import time +import logging +import threading +import rospy +from clever import srv +from mavros_msgs.srv import SetMode +from mavros_msgs.srv import CommandBool +from std_srvs.srv import Trigger + +module_logger = logging.getLogger("FlightLib.FlightLib") + +# create proxy service +navigate = rospy.ServiceProxy('navigate', srv.Navigate) +set_position = rospy.ServiceProxy('set_position', srv.SetPosition) +set_rates = rospy.ServiceProxy('/set_rates', srv.SetRates) +set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) +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") + +# globals +FREQUENCY = 1000/25 # HZ +TOLERANCE = 0.2 + +interrupt_event = threading.Event() + + +def interrupt(): + module_logger.info("Performing function interrupt") + interrupt_event.set() + + +def init(node_name="CleverSwarmFlight", anon=True, no_signals=True): + module_logger.info("Initing ROS node") + rospy.init_node(node_name, anonymous=anon, disable_signals=no_signals) + module_logger.info("Ros node inited") + + +def get_distance3d(x1, y1, z1, x2, y2, z2): + return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2) + + +def check(check_name): + def inner(f): + def wrapper(*args, **kwargs): + result, failures = f(*args, **kwargs) + if failures: + msgs = [] + for failure in failures: + msg = "[{}]: Failure: {}".format(check_name, failure) + msgs.append(msg) + module_logger.warning(msg) + return msgs + else: + module_logger.info("[{}]: OK".format(check_name)) + return None + return wrapper + return inner + + +@check("Linear velocity estimation") +def check_linear_speeds(): + failures = [] + 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)) + if telemetry.vy >= speed_limit: + failures.append("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 + + +@check("Angular velocity estimation") +def check_angular_speeds(): + failures = [] + 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)) + if telemetry.roll_rate >= rate_limit: + failures.append("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 + + +@check("Angles estimation") +def check_angles(): + failures = [] + telemetry = get_telemetry(frame_id='body') + angle_limit = math.radians(1) + if abs(telemetry.pitch) >= angle_limit: + failures.append("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))) + if abs(telemetry.yaw) >= angle_limit: + failures.append("Yaw estimation: {:.3f} rad;{:.3f} degrees".format(telemetry.yaw, + math.degrees(telemetry.yaw))) + return failures + + +def selfcheck(): + msgs = [] + msgs.extend(check_linear_speeds()) + msgs.extend(check_angular_speeds()) + msgs.extend(check_angles()) + + return msgs + + +def navto(x, y, z, yaw=float('nan'), frame_id='aruco_map'): + 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)) + + 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)) + 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() + + 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 + + telemetry = get_telemetry(frame_id=frame_id) + module_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 point timed out! | time: {:3f} seconds'.format(time_passed / 1000)) + return wait + rate.sleep() + else: + module_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)) + current_telem = get_telemetry(frame_id=frame_id) + navigate(frame_id=frame_id, x=current_telem.x, y=current_telem.xy, 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() + + while (abs(z - telemetry.z) > tolerance) or wait: + if interrupt_event.is_set(): + module_logger.warning("Flight function interrupted!") + interrupt_event.clear() + break + + telemetry = get_telemetry(frame_id=frame_id) + module_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)) + return wait + else: + return True + rate.sleep() + else: + module_logger.info("Attitude reached!") + return True + + +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): + 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) + landing() + + telemetry = get_telemetry(frame_id='aruco_map') + rate = rospy.Rate(1000 / wait_ms) + time_start = rospy.get_rostime() + + while telemetry.armed: + if interrupt_event.is_set(): + module_logger.warning("Flight function interrupted!") + interrupt_event.clear() + break + + telemetry = get_telemetry(frame_id='aruco_map') + module_logger.info("Landing...") + 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!") + arming(False) + return False + rate.sleep() + else: + module_logger.info("Landing succeeded!") + return True + + +def takeoff(z=1.0, speed=0.5, frame_id='body', freq=FREQUENCY, + timeout_arm=750, timeout_takeoff=5000, wait=False, emergency_land=True): + module_logger.info("Starting takeoff!") + module_logger.info("Arming, going to OFFBOARD mode") + navigate(frame_id=frame_id_takeoff, speed=speed_takeoff, auto_arm=True) + + telemetry = get_telemetry(frame_id=frame_id) + rate = rospy.Rate(freq) + 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 + + telemetry = get_telemetry(frame_id=frame_id_takeoff) + module_logger.info("Arming...") + time_passed = (rospy.get_rostime() - time_start).to_sec() * 1000 + + if timeout is not None: + if time_passed >= timeout_arm: + module_logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed / 1000)) + return False + rate.sleep() + + module_logger.info("Armed!") + + result = reach_point(z=z, speed=speed, frame_id=frame_id_takeoff, timeout=timeout_takeoff, freq=freq, wait=wait) + if result: + module_logger.info("Takeoff succeeded!") + else: + module_logger.warning("Takeoff navigation failed") + if emergency_land: + module_logger.info("Preforming emergency land") + land(descend=False) + + return result + diff --git a/Drone/FlightLib2/__init__.py b/Drone/FlightLib2/__init__.py new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/Drone/FlightLib2/__init__.py @@ -0,0 +1 @@ + diff --git a/Drone/client.py b/Drone/client.py index d2c6606..eef625e 100644 --- a/Drone/client.py +++ b/Drone/client.py @@ -5,19 +5,29 @@ import struct import random import time import errno +import logging import threading import ConfigParser from contextlib import closing import rospy -from FlightLib.FlightLib import FlightLib -from FlightLib.FlightLib import LedLib +#from FlightLib.FlightLib import FlightLib +from FlightLib2 import FlightLib +from FlightLib.FlightLib import LedLib # TODO new ledlib import play_animation random.seed() +logging.basicConfig( # TODO all prints as logs + level=logging.INFO, + format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s", + handlers=[ + logging.FileHandler("client_logs"), + logging.StreamHandler() + ]) + NTP_PACKET_FORMAT = "!12I" NTP_DELTA = 2208988800L # 1970-01-01 00:00:00 NTP_QUERY = '\x1b' + 47 * '\0' @@ -52,7 +62,7 @@ def reconnect(t=2): print("Too many attempts. Trying to get new server IP") broadcst_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) broadcst_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) - broadcst_client.bind(("", 8181)) + broadcst_client.bind(("", broadcast_port)) while True: data, addr = broadcst_client.recvfrom(1024) print("Recieved broadcast message %s from %s"%(data, addr)) @@ -64,7 +74,7 @@ def reconnect(t=2): def send_all(msg): - clientSocket.sendall(struct.pack('>I', len(msg)) + msg) + clientSocket.sendall(struct.pack('>I', len(msg)) + msg) def recive_all(n): @@ -87,7 +97,7 @@ def recive_message(): def form_command(command, args=()): - return " ".join([command, args]) + return " ".join([command, args]) def parse_command(command_input): @@ -121,7 +131,7 @@ def animation_player(running_event, stop_event): rate = rospy.Rate(1000 / 125) play_animation.takeoff() play_animation.animate_frame(frames[0]) #Reach first point at the same time with others - rospy.sleep(5) + rospy.sleep(5) # TODO change to flightlib reach_point for frame in frames: running_event.wait() if stop_event.is_set(): @@ -164,37 +174,44 @@ def stop_animation(): # animation_thread.join() +def selfcheck(): + telemetry = FlightLib.get_telemetry('body') + return FlightLib.selfcheck(), telemetry.voltage + CONFIG_PATH = "client_config.ini" config = ConfigParser.ConfigParser() config.read(CONFIG_PATH) - -port = int(config.get('SERVER', 'port')) +broadcast_port = config.getint('SERVER', 'broadcast_port') +port = config.getint('SERVER', 'port') host = config.get('SERVER', 'host') -BUFFER_SIZE = int(config.get('SERVER', 'buffer_size')) +BUFFER_SIZE = config.getint('SERVER', 'buffer_size') +USE_NTP = config.getboolean('NTP', 'use_ntp') NTP_HOST = config.get('NTP', 'host') -NTP_PORT = int(config.get('NTP', 'port')) +NTP_PORT = config.getint('NTP', 'port') files_directory = config.get('FILETRANSFER', 'files_directory') -animation_file = config.get('COPTER', 'animation_file') +animation_file = config.get('FILETRANSFER', 'animation_file') -COPTER_ID = config.get('COPTER', 'id') +TAKEOFF_HEIGHT = config.getfloat('COPTERS', 'takeoff_height') +TAKEOFF_TIME = config.getint('COPTERS', 'takeoff_time') + +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('COPTER', 'id', COPTER_ID) - -TAKEOFF_HEIGHT = float(config.get('COPTER', 'takeoff_height')) - -USE_LEDS = config.getboolean('COPTER', 'use_leds') -play_animation.USE_LEDS = USE_LEDS + write_to_config('PRIVATE', 'id', COPTER_ID) rospy.init_node('Swarm_client', anonymous=True) if USE_LEDS: LedLib.init_led() print("Client started on copter:", COPTER_ID) -print("NTP time:", time.ctime(get_ntp_time(NTP_HOST, NTP_PORT))) +if USE_NTP: + print("NTP time:", time.ctime(get_ntp_time(NTP_HOST, NTP_PORT))) print("System time", time.ctime(time.time())) reconnect() @@ -225,9 +242,9 @@ try: resume_animation() elif command == 'stop': stop_animation() - #FlightLib.reach(5, 5, 2) + FlightLib.interrupt() elif command == 'land': - FlightLib.land1() # TODO dont forget change back to land + play_animation.land() # TODO dont forget change back to land elif command == 'disarm': FlightLib.arming(False) @@ -239,6 +256,8 @@ try: response = "test_succsess" elif request_target == 'id': response = COPTER_ID + elif request_target == 'selfcheck': + response = selfcheck() send_all(bytes(form_command("response", response))) print("Request responded with:", response) except socket.error as e: diff --git a/Drone/client_config.ini b/Drone/client_config.ini index c95d762..4134ffe 100644 --- a/Drone/client_config.ini +++ b/Drone/client_config.ini @@ -1,18 +1,26 @@ [SERVER] port = 25000 +broadcast_port = 8181 host = 192.168.1.2 buffer_size = 1024 [FILETRANSFER] -files_directory = files +files_directory = animation +animation_file = animation.csv [NTP] +use_ntp = True host = ntp1.stratum2.ru port = 123 -[COPTER] -id = copter2 +[COPTERS] takeoff_height = 1 -takeoff_timeout = 7 +takeoff_time = 5000 +x0_common = 0 +y0_common = 0 + +[PRIVATE] +id = copter2 use_leds = True -animation_file = animation.csv +x0 = 0 +y0 = 0 \ No newline at end of file diff --git a/Drone/play_animation.py b/Drone/play_animation.py index 7fbabf7..6d9915a 100644 --- a/Drone/play_animation.py +++ b/Drone/play_animation.py @@ -1,7 +1,7 @@ import time import csv import rospy -from FlightLib.FlightLib import FlightLib +from FlightLib2 import FlightLib #FlightLib.init('SingleCleverFlight') from FlightLib.FlightLib import LedLib @@ -9,22 +9,22 @@ animation_file_path = 'animation.csv' USE_LEDS = True -def takeoff(): +def takeoff(z=1.0): if USE_LEDS: LedLib.wipe_to(255, 0, 0) - FlightLib.takeoff1() # TODO dont forget change back to takeoff + FlightLib.takeoff(z=z, wait=True) # TODO dont forget change back to takeoff def land(): if USE_LEDS: LedLib.blink(255, 0, 0) - FlightLib.land1() + FlightLib.land() if USE_LEDS: 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) + FlightLib.navto(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'], yaw=1.57) # TODO yaw if USE_LEDS: LedLib.fill(current_frame['red'], current_frame['green'], current_frame['blue']) @@ -59,7 +59,7 @@ if __name__ == '__main__': frames = read_animation_file() rate = rospy.Rate(8) takeoff() - FlightLib.reach(x=frames[0]['x']+X0, y=frames[0]['y']+Y0, z=frames[0]['z'], yaw=11.57) + FlightLib.reach_point(x=frames[0]['x']+X0, y=frames[0]['y']+Y0, z=frames[0]['z'], yaw=11.57) for frame in frames: animate_frame(frame, x0=X0, y0=Y0) rate.sleep() diff --git a/Server/server_config.ini b/Server/server_config.ini index f7fa366..ac84317 100644 --- a/Server/server_config.ini +++ b/Server/server_config.ini @@ -1,8 +1,9 @@ [SERVER] port = 25000 +broadcast_port = 8181 buffer_size = 1024 [NTP] +use_ntp = True host = ntp1.stratum2.ru -port = 123 -#pool.ntp.org +port = 123 \ No newline at end of file diff --git a/Server/server_qt.py b/Server/server_qt.py index 1c9786a..3e4ecea 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -52,7 +52,7 @@ def ip_broadcast(ip, port): broadcast_sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) while True: msg = bytes(Client.form_command("server_ip ", ip, ), "UTF-8") - broadcast_sock.sendto(msg, ('255.255.255.255', 8181)) #TODO to config + broadcast_sock.sendto(msg, ('255.255.255.255', broadcast_port)) print("Broadcast sent") time.sleep(5) @@ -109,7 +109,7 @@ class Client: if self.copter_id is None: self.copter_id = self.get_response("id") print("Got copter id:", self.copter_id) - model.appendRow((QStandardItem(self.copter_id), )) # TODO: get responses for another columns + model.appendRow((QStandardItem(self.copter_id), )) # TODO: get responses for another columns def _send_all(self, msg): self.socket.sendall(struct.pack('>I', len(msg)) + msg) @@ -230,7 +230,7 @@ class Client: file = open(filepath, 'rb') chunk = file.read(BUFFER_SIZE) while chunk: - self._send_queue.append(chunk) + self._send_queue.append(chunk) # TODO os.sendfile chunk = file.read(BUFFER_SIZE) file.close() self.send(Client.form_command("/endoffile")) @@ -270,7 +270,10 @@ class MainWindow(QtWidgets.QMainWindow): @pyqtSlot() def send_starttime(self): dt = self.ui.start_delay_spin.value() - timenow = time.time() # TODO add NTP + if USE_NTP: + timenow = get_ntp_time(NTP_HOST, NTP_PORT) + else: + timenow = time.time() print('Now:', time.ctime(timenow), "+ dt =", dt) Client.send_to_selected(Client.form_command("starttime", (str(timenow + dt),))) @@ -299,7 +302,6 @@ class MainWindow(QtWidgets.QMainWindow): def disarm_all(self): Client.broadcast("disarm") - @pyqtSlot() def send_animations(self): path = str(QFileDialog.getExistingDirectory(self, "Select Animation Directory")) @@ -318,6 +320,7 @@ class MainWindow(QtWidgets.QMainWindow): # ANS = dr.get_response("someshit") # print(ANS) + model = QStandardItemModel() model.setHorizontalHeaderLabels( ('copter ID', 'animation ID', 'battery V', 'battery %', 'selfcheck', 'time UTC') @@ -331,11 +334,12 @@ config = configparser.ConfigParser() config.read("server_config.ini") port = int(config['SERVER']['port']) +broadcast_port = int(config['SERVER']['broadcast_port']) BUFFER_SIZE = int(config['SERVER']['buffer_size']) +USE_NTP = bool(config['NTP']['use_ntp']) NTP_HOST = config['NTP']['host'] NTP_PORT = int(config['NTP']['port']) - ServerSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) ServerSocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) host = socket.gethostname() @@ -347,7 +351,13 @@ if __name__ == "__main__": window = MainWindow() print('Server started on', host, ip, ":", port) - # print('Now:', time.ctime(get_ntp_time(NTP_HOST, NTP_PORT))) + + if USE_NTP: + now = time.ctime(get_ntp_time(NTP_HOST, NTP_PORT)) + else: + now = time.time() + print('Now:', time.ctime(now)) + print('Waiting for clients...') ServerSocket.bind((ip, port))