diff --git a/Drone/animation_lib.py b/Drone/animation_lib.py index 1e51a2c..92bbcb2 100644 --- a/Drone/animation_lib.py +++ b/Drone/animation_lib.py @@ -9,13 +9,12 @@ from FlightLib import FlightLib from FlightLib import LedLib import tasking_lib as tasking +import transformation_lib as transform logger = logging.getLogger(__name__) interrupt_event = threading.Event() -# TODO separate code for frames transformations (e.g. for gps) - def requires_import(f): def wrapper(*args, **kwargs): @@ -27,7 +26,7 @@ def requires_import(f): return wrapper -class AnimationLoader: +class AnimationLoader(object): def __init__(self): self.anim_id = None self.fps = None @@ -40,7 +39,7 @@ class AnimationLoader: self.points = np.empty((0, 3)) # to transform points self.colors = np.empty((0, 3), int) # to manipulate brightness/etc self.yaws = np.empty((0, 0)) # to rotate - self.extras = [] + self.extras = [] # extra functions and params for each frame def load_csv(self, filepath="animation.csv"): try: @@ -49,30 +48,32 @@ class AnimationLoader: logging.error("File {} can't be opened".format(filepath)) return False else: - with animation_file: - csv_reader = csv.reader(animation_file, delimiter=',', quotechar='|') - lines = list(csv_reader) - print(lines) - - if len(lines[0]) == 1: - header = lines.pop(0)[0] - print("Got animation header: {}".format(header)) - jsonheader = json.loads(header) - self.anim_id = jsonheader.pop("file", None) - self.anim_headers = header - else: - print("No header in the file") - pass - - self.frames = len(lines) - print(lines) - for row in lines: - if row: - self._parse_row(row) - + self._read_csv(animation_file) self.imported = True return True + def _read_csv(self, animation_file): + with animation_file: + csv_reader = csv.reader(animation_file, delimiter=',', quotechar='|') + lines = list(csv_reader) + print(lines) + + if len(lines[0]) == 1: + header = lines.pop(0)[0] + print("Got animation header: {}".format(header)) + jsonheader = json.loads(header) + self.anim_id = jsonheader.pop("file", None) + self.anim_headers = header + else: + print("No header in the file") + pass + + self.frames = len(lines) + print(lines) + for row in lines: + if row: + self._parse_row(row) + def _parse_row(self, row): _frame_number, x, y, z, yaw, red, green, blue = row[:8] try: @@ -112,6 +113,15 @@ class AnimationLoader: next = __next__ # python 2 compatibility + def offset(self, offset): + self.points = transform.translate(self.points, np.array([offset])) + + def rotate_z(self, angle, origin=None, modify_yaw=False): + origin = np.array([origin]) if origin is not None else np.array([[0, 0, 0]]) + self.points = transform.rotate_z(self.points, angle, origin) + if modify_yaw: + self.yaws += angle + if __name__ == "__main__": # for testing animation loader anim = AnimationLoader() @@ -120,14 +130,10 @@ if __name__ == "__main__": # for testing animation loader print(fr) -def property_true(value): - return value == 1 - - -class AnimationPlayer: +class AnimationPlayer(object): def __init__(self, frames, frame_delay, interrupter=interrupt_event, common_kwargs=None): self.frames = list(frames) - self._frame_time = None + self.frame_time = None self.frame_delay = frame_delay self.interrupter = interrupter @@ -139,11 +145,13 @@ class AnimationPlayer: f(*args, **kwargs) def _after_frame(self): - self._frame_time += self.frame_delay - tasking.wait(self._frame_time, self.interrupter) + self.frame_time += self.frame_delay + tasking.wait(self.frame_time, self.interrupter) - def execute_animation(self, start_time=None): - self._frame_time = start_time if start_time is not None else time.time() + def execute_animation(self, start_time=None, **kwargs): + self.frame_time = start_time if start_time is not None else time.time() + + self._override = None # additional cleanup in case of restarting same animation for frame in self.frames: if self.interrupter.is_set(): @@ -159,7 +167,7 @@ class AnimationPlayer: point, color, yaw, extras = frame if self._override is not None: - if property_true(extras.get(self._override, False)): + if extras.get(self._override, False): return # don't execute anything if long-term override is active else: self._override = None # reset override when not active anymore @@ -170,11 +178,11 @@ class AnimationPlayer: except KeyError: pass else: - if property_true(val): + if val: if self._execute_extra(frame, key, extra): return # returns if override extra is preformed - point_flight(*frame, interrupter=self.interrupter, **self.common_kwargs) + self._execution_wrapper(point_flight, *frame, interrupter=self.interrupter, **self.common_kwargs) def _execute_extra(self, frame, key, extra): extra_f, params = extra @@ -189,6 +197,25 @@ class AnimationPlayer: return override +class TaskingAnimationPlayer(AnimationPlayer): + def __init__(self, frames, frame_delay, task_manager=None, common_kwargs=None): + self.task_manager = task_manager if task_manager is not None else tasking.TaskManager() + + super(TaskingAnimationPlayer, self).__init__(frames, frame_delay, + interrupter=task_manager.task_interrupt_event, + common_kwargs=common_kwargs) + + def _execution_wrapper(self, f, *args, **kwargs): + self.task_manager.add_task(self.frame_time, 0, f, + task_args=args, + task_kwargs=kwargs, + ) + + def _after_frame(self): + self.frame_time += self.frame_delay + # no delay in task creating + + extra_functions = {} @@ -212,13 +239,14 @@ def execute_flip(frame, frame_id='aruco_map', flight_kwargs=None, interrupter=in FlightLib.flip(frame_id=frame_id, **flight_kwargs) -def point_flight(frame, frame_id='aruco_map', use_leds=True, +def point_flight(frame, frame_id='aruco_map', use_leds=True, use_yaw=False, flight_func=FlightLib.navto, flight_kwargs=None, interrupter=interrupt_event, **kwargs): if flight_kwargs is None: flight_kwargs = {} point, color, yaw, extras = frame + yaw = yaw if use_yaw else float('nan') flight_func(*point, yaw=yaw, frame_id=frame_id, interrupter=interrupter, **flight_kwargs) if use_leds: diff --git a/Drone/copter_client.py b/Drone/copter_client.py index fcb3721..cc10b09 100644 --- a/Drone/copter_client.py +++ b/Drone/copter_client.py @@ -203,18 +203,32 @@ def _command_resume(**kwargs): @messaging.message_callback("start") def _play_animation(**kwargs): - start_time = float(kwargs["time"]) # TODO + start_time = float(kwargs["time"]) - if animation.get_id() == 'No animation': + anim = animation.AnimationLoader() + loaded = anim.load_csv(os.path.abspath("animation.csv")) # TODO config + if not loaded: 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, - ) + # todo rotate z and gps transformation + + x0 = client.active_client.X0 + client.active_client.X0_COMMON + y0 = client.active_client.Y0 + client.active_client.Y0_COMMON + z0 = 0 # TODO + anim.offset([x0, y0, z0]) + + fps = anim.fps if anim.fps is not None else 8 + frame_delay = 1/fps + task_kwargs = { + "frame_id": client.active_client.FRAME_ID, + "use_leds": client.active_client.USE_LEDS, + "flight_func": FlightLib.navto, + } + + player = animation.TaskingAnimationPlayer(anim, frame_delay, task_manager, task_kwargs) task_manager.add_task(start_time, 0, animation.takeoff, task_kwargs={ @@ -227,32 +241,21 @@ def _play_animation(**kwargs): ) rfp_time = start_time + client.active_client.TAKEOFF_TIME - task_manager.add_task(rfp_time, 0, animation.execute_frame, + task_manager.add_task(rfp_time, 0, animation.point_flight, task_kwargs={ - "point": animation.convert_frame(frames[0])[0], - "color": animation.convert_frame(frames[0])[1], + "frame": anim[0], "frame_id": client.active_client.FRAME_ID, "use_leds": client.active_client.USE_LEDS, + # todo use yaw "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, yaw = animation.convert_frame(frame) # TODO add param to calculate delta - task_manager.add_task(frame_time, 0, animation.execute_frame, - 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 + player.execute_animation(frame_time) # animation executor (adding frame tasks) + + land_time = player.frame_time + client.active_client.LAND_TIME task_manager.add_task(land_time, 0, animation.land, task_kwargs={ "timeout": client.active_client.TAKEOFF_TIME, diff --git a/Drone/tasking_lib.py b/Drone/tasking_lib.py index bf1f855..f507950 100644 --- a/Drone/tasking_lib.py +++ b/Drone/tasking_lib.py @@ -38,8 +38,8 @@ class TaskManager(object): self._running_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() + self.task_interrupt_event = threading.Event() self._timeshift = 0.0 @@ -66,18 +66,14 @@ class TaskManager(object): 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") - + self.task_interrupt_event.set() + if self._reset_event.is_set(): - self._task_interrupt_event.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) def pop_task(self): with self._task_queue_lock: @@ -86,7 +82,6 @@ class TaskManager(object): raise KeyError('Pop from an empty priority queue') def start(self): - #print("Task manager is started") #logger.info("Task manager is started") self._processor_thread.start() self.resume() @@ -100,18 +95,14 @@ class TaskManager(object): 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() #already exists in pause self._processor_thread.join(timeout=timeout) def pause(self, interrupt=True): if interrupt: self._wait_interrupt_event.set() - self._task_interrupt_event.set() + self.task_interrupt_event.set() self._running_event.clear() #logger.info("Task queue paused") - #print("Task queue paused") def resume(self, time_to_start_next_task=0.0): if self.task_queue: @@ -120,9 +111,8 @@ class TaskManager(object): self._timeshift = time_to_start_next_task - next_task_time self._running_event.set() self._wait_interrupt_event.clear() - self._task_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)) def reset(self): self.stop() @@ -139,23 +129,19 @@ class TaskManager(object): 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)) try: - task.func(*task.args, interrupter=self._task_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)) if str(e) == 'STOP': self.reset() return else: #logger.warning("Task interrupted before execution") - #print("Task interrupted before execution") self._wait_interrupt_event.clear() return @@ -163,20 +149,14 @@ 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!") - if self._task_interrupt_event.is_set(): - self._task_interrupt_event.clear() + 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") - # self._update_queue() # Initial tick if tasks added before start while not self._shutdown_event.is_set(): self._running_event.wait() self.execute_task() diff --git a/Drone/transformation_lib.py b/Drone/transformation_lib.py new file mode 100644 index 0000000..548afe3 --- /dev/null +++ b/Drone/transformation_lib.py @@ -0,0 +1,45 @@ +import numpy as np +import math + +# all operands should be numpy arrays + + +def translate(point, offset): + result = point + offset + return result + + +def rotate_z(point, angle, origin=None): + origin = origin if origin is not None else np.array([[0, 0, 0]]) + + c, s = np.cos(angle), np.sin(angle) + rot = np.array([(c, -s, 0), + (s, c, 0), + (0, 0, 1)]) # 3d rotation matrix for z rotation + result = point - origin + result = np.matmul(result, rot) + + result += origin + return result + + +if __name__ == "__main__": + import matplotlib.pyplot as plt + from mpl_toolkits.mplot3d import Axes3D + + points = np.array([[1, 1, 0], [2, 1, 1]]) + orig = np.array([[1, 1, 1]]) + + w = rotate_z(points, np.radians(90), orig) + v = points + + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + ax.plot(v[:, 0], v[:, 1], v[:, 2]) + + #fig2 = plt.figure() + #ax2 = fig2.add_subplot(111, projection='3d') + ax.plot(w[:, 0], w[:, 1], w[:, 2]) + + plt.show() +