From f9fd81da1f9af018ee53d7be5decda4812898fb3 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Tue, 11 Jun 2019 14:19:32 +0300 Subject: [PATCH] Repair play animation, pause and stop also work now --- Drone/FlightLib/FlightLib.py | 32 ++++++------- Drone/FlightLib/LedLib.py | 4 +- Drone/animation_lib.py | 11 +++-- Drone/client_config.ini | 3 +- Drone/copter_client.py | 88 ++++++++++++++++++++---------------- Drone/tasking_lib.py | 66 ++++++++++++++------------- Server/server_qt.py | 2 +- 7 files changed, 111 insertions(+), 95 deletions(-) diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py index 196bd28..723e4a6 100644 --- a/Drone/FlightLib/FlightLib.py +++ b/Drone/FlightLib/FlightLib.py @@ -14,8 +14,8 @@ from std_srvs.srv import Trigger logger = logging.getLogger(__name__) # create proxy service -navigate = rospy.ServiceProxy('navigate', srv.Navigate) -set_position = rospy.ServiceProxy('set_position', srv.SetPosition) +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) @@ -152,12 +152,12 @@ def selfcheck(): 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) + #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)) + #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 @@ -293,8 +293,8 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY, logger.info("Arming, going to OFFBOARD mode") # Arming check - set_rates(thrust=0.1, auto_arm=True) - telemetry = get_telemetry(frame_id=frame_id) + set_rates(thrust=0.05, auto_arm=True) + telemetry = get_telemetry() rate = rospy.Rate(freq) time_start = time.time() @@ -303,9 +303,9 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY, logger.warning("Takeoff function interrupted!") print("Takeoff function interrupted!") interrupter.clear() - return + return 'interrupted' - telemetry = get_telemetry(frame_id=frame_id) + telemetry = get_telemetry() logger.info("Arming...") print("Arming...") time_passed = time.time() - time_start @@ -315,7 +315,7 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY, if not telemetry.armed: logger.warning('Arming timed out! | time: {:3f} seconds'.format(time_passed)) print('Arming timed out! | time: {:3f} seconds'.format(time_passed)) - return False + return 'not armed' else: break rate.sleep() @@ -326,14 +326,14 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY, # Reach height z0 = get_telemetry().z z_dest = z + z0 - navigate(z=z, speed=speed, frame_id=frame_id, auto_arm=True) + navigate(z=z, speed=speed, yaw = float('nan'), 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 + return 'interrupted' current_diff = abs(get_telemetry().z - z_dest) logger.info("Takeoff...") @@ -349,11 +349,11 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY, logger.info("Preforming emergency land") print("Preforming emergency land") land(descend=False, interrupter=interrupter) - return False + return 'time out' else: break rate.sleep() logger.info("Takeoff succeeded!") print("Takeoff succeeded!") - return True + return 'success' diff --git a/Drone/FlightLib/LedLib.py b/Drone/FlightLib/LedLib.py index ef3b7a9..00234d6 100644 --- a/Drone/FlightLib/LedLib.py +++ b/Drone/FlightLib/LedLib.py @@ -4,7 +4,7 @@ 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_COUNT = 60 # Number of LED pixels. LED_PIN = 21 # GPIO pin connected to the pixels (18 uses PWM!) (10 uses SPI /dev/spidev0.0). LED_FREQ_HZ = 800000 # LED signal frequency in hertz (usually 800khz) LED_DMA = 10 # DMA channel to use for generating signal (try 10) @@ -27,7 +27,7 @@ b_prev = 0 direct = False l = 0 -wait_ms = 10 +wait_ms = 5.0 INTERRUPTER = threading.Event() INTERRUPTER_UNSET = threading.Event() diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index 0a83faa..0bc5265 100644 --- a/Drone/animation_lib.py +++ b/Drone/animation_lib.py @@ -22,6 +22,7 @@ def get_id(filepath="animation.csv"): except IOError: logging.error("File {} can't be opened".format(filepath)) id = "No animation" + return id else: with animation_file: csv_reader = csv.reader( @@ -81,7 +82,7 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0): def convert_frame(frame): - return (frame['x'], frame['y'], frame['z']), (frame["red"], frame["green"], frame["blue"]), frame["yaw"] + 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, @@ -111,22 +112,24 @@ 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, timeout=5000, frame_id='aruco_map', 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: 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, + result = FlightLib.takeoff(z=z, wait=False, timeout_takeoff=timeout, frame_id=frame_id, emergency_land=safe_takeoff, interrupter=interrupter) + if result == 'not armed': + raise Exception('STOP') # Raise exception to clear task_manager if copter can't arm if interrupter.is_set(): return if use_leds: 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, +def land(z=1.5, descend=False, timeout=5.0, frame_id='aruco_map', use_leds=True, interrupter=interrupt_event): if use_leds: LedLib.blink(255, 0, 0, interrupter=interrupter) diff --git a/Drone/client_config.ini b/Drone/client_config.ini index 2fe7540..ecd816a 100644 --- a/Drone/client_config.ini +++ b/Drone/client_config.ini @@ -1,7 +1,7 @@ [SERVER] port = 25000 broadcast_port = 8181 -host = 192.168.43.168 +host = 192.168.11.169 buffer_size = 1024 [FILETRANSFER] @@ -19,6 +19,7 @@ takeoff_height = 1.5 takeoff_time = 8.0 safe_takeoff = False reach_first_point_time = 8.0 +land_time = 3.0 x0_common = 0 y0_common = 0 diff --git a/Drone/copter_client.py b/Drone/copter_client.py index 31f81f2..a362057 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -22,6 +22,7 @@ class CopterClient(client.Client): 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.LAND_TIME = self.config.getfloat('COPTERS', 'land_time') self.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common') self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common') @@ -127,55 +128,62 @@ def _command_stop(**kwargs): @messaging.message_callback("start") def _play_animation(**kwargs): - gap = 0.25 - start_time = kwargs["time"] # TODO - print('start time = {}'.format(start_time)) + start_time = float(kwargs["time"]) # TODO + + 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)) + 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, - } - ) + 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, + } + ) - 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, - } - ) + 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, + } + ) - animation_time = rfp_time + client.active_client.RFP_TIME + gap + frame_time = rfp_time + client.active_client.RFP_TIME 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, - } - ) + for frame in frames: + 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, + } + ) + frame_time += frame_delay - 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, - }, - ) + 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, + }, + ) if __name__ == "__main__": diff --git a/Drone/tasking_lib.py b/Drone/tasking_lib.py index 9e73e77..58ad524 100644 --- a/Drone/tasking_lib.py +++ b/Drone/tasking_lib.py @@ -21,7 +21,7 @@ def wait(end, interrupter=INTERRUPTER, maxsleep=0.1): # Added features to inter time.sleep(diff / 2) else: logger.warning("Waiting was interrupted!") - print("Waiting was interrupted!") + #print("Waiting was interrupted!") class TaskManager(object): @@ -39,7 +39,7 @@ class TaskManager(object): self._task_interrupt_event = threading.Event() self._shutdown_event = threading.Event() - self._timeshift = 0 + self._timeshift = 0.0 def add_task(self, timestamp, priority, task_function, task_args=(), task_kwargs={}, task_delayable=False): @@ -62,17 +62,17 @@ class TaskManager(object): if self.task_queue[0] != entry_old: self._task_interrupt_event.set() - print("Task queue updated with more priority task") + #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") + #print("Task queue updated after reset") self._wait_interrupt_event.clear() self._running_event.set() - print(self.task_queue) + #print(self.task_queue) def pop_task(self): with self._task_queue_lock: @@ -81,13 +81,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") + #print("Task manager is started") + #logger.info("Task manager is started") self._processor_thread.start() self.resume() def stop(self): - self._timeshift = 0 + self._timeshift = 0.0 self.pause(interrupt=True) with self._task_queue_lock: del self.task_queue[:] @@ -105,8 +105,8 @@ class TaskManager(object): self._wait_interrupt_event.set() self._task_interrupt_event.set() self._running_event.clear() - logger.info("Task queue paused") - print("Task queue paused") + #logger.info("Task queue paused") + #print("Task queue paused") def resume(self, time_to_start_next_task = 0): if self.task_queue: @@ -116,8 +116,8 @@ class TaskManager(object): self._running_event.set() self._wait_interrupt_event.clear() self._task_interrupt_event.clear() - logger.info("Task queue resumed with timeshift {}".format(self._timeshift)) - print("Task queue resumed with timeshift {}".format(self._timeshift)) + #logger.info("Task queue resumed with timeshift {}".format(self._timeshift)) + #print("Task queue resumed with timeshift {}".format(self._timeshift)) def reset(self): self.stop() @@ -129,24 +129,28 @@ class TaskManager(object): if self.task_queue: start_time, priority, count, task = self.task_queue[0] else: - self._timeshift = 0 + self._timeshift = 0.0 return - logger.info("Waiting util task execution time:{}".format(start_time + self._timeshift)) - print("Waiting util task execution time:{}".format(start_time + self._timeshift)) - wait(start_time + self._timeshift, self._wait_interrupt_event) + task_start_time = start_time + self._timeshift + #logger.info("Waiting util task execution time:{}".format(task_start_time)) + #print("Waiting util task execution time:{}".format(task_start_time)) + wait(task_start_time, self._wait_interrupt_event) if not self._wait_interrupt_event.is_set(): - logger.info("Executing task {}".format(task)) - print("Executing task {}".format(task)) + #logger.info("Executing task {}".format(task)) + #print("Executing task {}".format(task)) try: 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)) + #print("Error '{}' occurred in task {}".format(e, task)) + #if str(e) == 'STOP': + # self.reset() + # return else: - logger.warning("Task interrupted before execution") - print("Task interrupted before execution") + #logger.warning("Task interrupted before execution") + #print("Task interrupted before execution") self._wait_interrupt_event.clear() return @@ -154,30 +158,30 @@ class TaskManager(object): 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!") + #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") + #logger.info("Execution done") + #print("Execution done") def _task_processor(self): - logger.info("Tasking thread started") + #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() self.execute_task() if __name__ == "__main__": - logger.addHandler(logging.StreamHandler()) - logger.setLevel(logging.DEBUG) + #logger.addHandler(logging.StreamHandler()) + #logger.setLevel(logging.DEBUG) def printer(stri, interrupter, *args, **kwargs): - logger.info("String: {}, timenow: {}".format(stri, time.time())) + #logger.info("String: {}, timenow: {}".format(stri, time.time())) wait(time.time()+30, interrupter) tasker = TaskManager() # Lower priority first! diff --git a/Server/server_qt.py b/Server/server_qt.py index ee8143f..b0c9888 100644 --- a/Server/server_qt.py +++ b/Server/server_qt.py @@ -106,7 +106,7 @@ class MainWindow(QtWidgets.QMainWindow): self.ui.pause_button.setText('Resume') else: Client.broadcast_message('resume') - self.ui.pause_button.setText('Pause') + self.ui.pause_button.setText('Pause') @pyqtSlot() def test_leds(self):