diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index 0bc5265..7712276 100644 --- a/Drone/animation_lib.py +++ b/Drone/animation_lib.py @@ -13,16 +13,20 @@ logger = logging.getLogger(__name__) interrupt_event = threading.Event() -id = "Empty id" +anim_id = "Empty id" + +# TODO refactor as class +# TODO separate code for frames transformations (e.g. for gps) + def get_id(filepath="animation.csv"): - global id + global anim_id try: animation_file = open(filepath) except IOError: logging.error("File {} can't be opened".format(filepath)) - id = "No animation" - return id + anim_id = "No animation" + return anim_id else: with animation_file: csv_reader = csv.reader( @@ -30,20 +34,21 @@ def get_id(filepath="animation.csv"): ) row_0 = csv_reader.next() if len(row_0) == 1: - id = row_0[0] - print("Got animation_id: {}".format(id)) + anim_id = row_0[0] + print("Got animation_id: {}".format(anim_id)) else: print("No animation id in file") - return id + return anim_id + def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0): imported_frames = [] - global id + global anim_id try: animation_file = open(filepath) except IOError: logging.error("File {} can't be opened".format(filepath)) - id = "No animation" + anim_id = "No animation" else: with animation_file: csv_reader = csv.reader( @@ -51,8 +56,8 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0): ) row_0 = csv_reader.next() if len(row_0) == 1: - id = row_0[0] - print("Got animation_id: {}".format(id)) + anim_id = row_0[0] + print("Got animation_id: {}".format(anim_id)) else: print("No animation id in file") frame_number, x, y, z, yaw, red, green, blue = row_0 @@ -87,7 +92,6 @@ 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 = {} @@ -112,7 +116,7 @@ def execute_animation(frames, frame_delay, frame_id='aruco_map', use_leds=True, tasking.wait(next_frame_time, interrupter) -def takeoff(z=1.5, safe_takeoff=True, frame_id = 'map', timeout=5.0, use_leds=True, +def takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, use_leds=True, interrupter=interrupt_event): print(interrupter.is_set()) if use_leds: @@ -120,9 +124,9 @@ def takeoff(z=1.5, safe_takeoff=True, frame_id = 'map', timeout=5.0, use_leds=Tr if interrupter.is_set(): return result = FlightLib.takeoff(z=z, wait=False, timeout_takeoff=timeout, frame_id=frame_id, emergency_land=safe_takeoff, - interrupter=interrupter) + interrupter=interrupter) if result == 'not armed': - raise Exception('STOP') # Raise exception to clear task_manager if copter can't arm + raise Exception('STOP') # Raise exception to clear task_manager if copter can't arm if interrupter.is_set(): return if use_leds: diff --git a/Drone/copter_client.py b/Drone/copter_client.py index facecb4..ee67779 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -12,7 +12,7 @@ import messaging_lib as messaging import tasking_lib as tasking import animation_lib as animation -#logging.basicConfig( # TODO all prints as logs +# 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=[ @@ -21,7 +21,8 @@ import animation_lib as animation logger = logging.getLogger(__name__) -#import ros_logging + +# import ros_logging class CopterClient(client.Client): def load_config(self): @@ -45,7 +46,7 @@ class CopterClient(client.Client): rospy.init_node('Swarm_client', anonymous=True) if self.USE_LEDS: LedLib.init_led(self.LED_PIN) - + task_manager_instance.start() super(CopterClient, self).start() @@ -56,10 +57,12 @@ def _response_selfcheck(): check = FlightLib.selfcheck() return check if check else "OK" + @messaging.request_callback("anim_id") def _response_animation_id(): return animation.get_id() + @messaging.request_callback("batt_voltage") def _response_batt(): return FlightLib.get_telemetry('body').voltage @@ -69,47 +72,50 @@ def _response_batt(): def _response_cell(): return FlightLib.get_telemetry('body').cell_voltage + @messaging.message_callback("test") def _command_test(**kwargs): print("test") + @messaging.message_callback("service_restart") def _command_service_restart(**kwargs): os.system("systemctl restart" + kwargs["name"]) @messaging.message_callback("led_test") -def _command_led_test(*args, **kwargs): +def _command_led_test(**kwargs): LedLib.chase(255, 255, 255) time.sleep(2) LedLib.off() + @messaging.message_callback("led_fill") -def _command_emergency_led_fill(**kwargs): +def _command_led_fill(**kwargs): r = g = b = 0 - + try: r = kwargs["red"] except KeyError: pass - + try: g = kwargs["green"] except KeyError: pass - try: + try: b = kwargs["blue"] - except KeyError: + except KeyError: pass - - LedLib.fill(r, g, b) + LedLib.fill(r, g, b) @messaging.message_callback("flip") def _copter_flip(): FlightLib.flip() + @messaging.message_callback("takeoff") def _command_takeoff(**kwargs): task_manager.add_task(time.time(), 0, animation.takeoff, @@ -139,10 +145,10 @@ def _command_land(**kwargs): def _command_disarm(**kwargs): task_manager.reset() task_manager.add_task(-5, 0, FlightLib.arming_wrapper, - task_kwargs={ - "state": False - } - ) + task_kwargs={ + "state": False + } + ) @messaging.message_callback("stop") @@ -157,18 +163,18 @@ def _command_stop(**kwargs): @messaging.message_callback("resume") def _command_stop(**kwargs): - task_manager.resume() + task_manager.resume(time_to_start_next_task=kwargs.get("time", 0)) @messaging.message_callback("start") def _play_animation(**kwargs): start_time = float(kwargs["time"]) # TODO - if (animation.get_id() == 'No animation'): + if animation.get_id() == 'No animation': print("Can't start animation without animation file!") return - print("Start time = {}, wait for {} seconds".format(start_time, time.time()-start_time)) + print("Start time = {}, wait for {} seconds".format(start_time, time.time() - start_time)) frames = animation.load_animation(os.path.abspath("animation.csv"), x0=client.active_client.X0 + client.active_client.X0_COMMON, @@ -176,59 +182,58 @@ def _play_animation(**kwargs): ) task_manager.add_task(start_time, 0, 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, - } - ) + 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 task_manager.add_task(rfp_time, 0, animation.execute_frame, - task_kwargs={ - "point": animation.convert_frame(frames[0])[0], - "color": animation.convert_frame(frames[0])[1], - "frame_id": client.active_client.FRAME_ID, - "use_leds": client.active_client.USE_LEDS, - "flight_func": FlightLib.reach_point, - } - ) + task_kwargs={ + "point": animation.convert_frame(frames[0])[0], + "color": animation.convert_frame(frames[0])[1], + "frame_id": client.active_client.FRAME_ID, + "use_leds": client.active_client.USE_LEDS, + "flight_func": FlightLib.reach_point, + } + ) frame_time = rfp_time + client.active_client.RFP_TIME frame_delay = 0.125 # TODO from animation file for frame in frames: + point, color = animation.convert_frame(frame) # TODO add param to calculate delta task_manager.add_task(frame_time, 0, animation.execute_frame, - task_kwargs={ - "point": animation.convert_frame(frame)[0], - "color": animation.convert_frame(frame)[1], - "frame_id": client.active_client.FRAME_ID, - "use_leds": client.active_client.USE_LEDS, - "flight_func": FlightLib.navto, - } - ) + task_kwargs={ + "point": point, + "color": color, + "frame_id": client.active_client.FRAME_ID, + "use_leds": client.active_client.USE_LEDS, + "flight_func": FlightLib.navto, + } + ) frame_time += frame_delay land_time = frame_time + client.active_client.LAND_TIME task_manager.add_task(land_time, 0, animation.land, - task_kwargs={ - "timeout": client.active_client.TAKEOFF_TIME, - "frame_id": client.active_client.FRAME_ID, - "use_leds": client.active_client.USE_LEDS, - }, - ) + task_kwargs={ + "timeout": client.active_client.TAKEOFF_TIME, + "frame_id": client.active_client.FRAME_ID, + "use_leds": client.active_client.USE_LEDS, + }, + ) if __name__ == "__main__": - copter_client = CopterClient() task_manager = tasking.TaskManager() - + copter_client.start(task_manager) - #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() + # ros_logging.route_logger_to_ros("__main__") + # ros_logging.route_logger_to_ros("client") + # ros_logging.route_logger_to_ros("messaging") diff --git a/Drone/tasking_lib.py b/Drone/tasking_lib.py index 2e8b4c3..bf1f855 100644 --- a/Drone/tasking_lib.py +++ b/Drone/tasking_lib.py @@ -10,7 +10,9 @@ Task = collections.namedtuple("Task", ["func", "args", "kwargs", "delayable", ]) INTERRUPTER = threading.Event() -def wait(end, interrupter=INTERRUPTER, maxsleep=0.1): # Added features to interrupter sleep and set max sleeping interval + +def wait(end, interrupter=INTERRUPTER, maxsleep=0.1): + # Added features to interrupter sleep and set max sleeping interval while not interrupter.is_set(): # Basic implementation of pause module until() now = time.time() @@ -83,7 +85,7 @@ class TaskManager(object): return heapq.heappop(self.task_queue) raise KeyError('Pop from an empty priority queue') - def start(self, timeouts=False): + def start(self): #print("Task manager is started") #logger.info("Task manager is started") self._processor_thread.start() @@ -95,13 +97,13 @@ class TaskManager(object): with self._task_queue_lock: del self.task_queue[:] - def shutdown(self): + def shutdown(self, timeout=5.0): 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) + # self._wait_interrupt_event.set() + # self._task_interrupt_event.set() + # self._running_event.clear() #already exists in pause + self._processor_thread.join(timeout=timeout) def pause(self, interrupt=True): if interrupt: @@ -111,7 +113,7 @@ class TaskManager(object): #logger.info("Task queue paused") #print("Task queue paused") - def resume(self, time_to_start_next_task = 0): + def resume(self, time_to_start_next_task=0.0): if self.task_queue: next_task_time = self.task_queue[0][0] if time_to_start_next_task > next_task_time: