mirror of
https://github.com/CopterExpress/clever-show.git
synced 2026-06-03 18:49:32 +00:00
Refactored animation playing
For usage with extra functions (as flips) and transformations
This commit is contained in:
Artem30801
@@ -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:
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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()
|
||||
|
||||
45
Drone/transformation_lib.py
Normal file
45
Drone/transformation_lib.py
Normal file
@@ -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()
|
||||
|
||||
Reference in New Issue
Block a user