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