CopterExpress/clever-show
1
0
Fork 0
mirror of https://github.com/CopterExpress/clever-show.git synced 2026-05-26 15:13:26 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update animation lib and client specification

Browse Source
This commit is contained in:
Arthur Golubtsov
2020-05-27 15:34:07 +03:00
parent fa897d6e52
commit 673cd20a43
2 changed files with 121 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

183
Drone/animation_lib.py
View File

@@ -6,7 +6,6 @@ import numpy
import rospy
import logging
import threading
import ConfigParser
try:
from FlightLib import FlightLib
@@ -21,13 +20,66 @@ logger = logging.getLogger(__name__)
interrupt_event = threading.Event()
def moving(f1, f2, delta, x = True, y = True, z = True):
return ((abs(f1['x'] - f2['x']) > delta) and x
or (abs(f1['y'] - f2['y']) > delta) and y
or (abs(f1['z'] - f2['z']) > delta) and z)
def moving(f1, f2, delta, x=True, y=True, z=True):
return ((abs(f1.x - f2.x) > delta) and x
or (abs(f1.y - f2.y) > delta) and y
or (abs(f1.z - f2.z) > delta) and z)
def get_numbers(frames):
numbers = []
if frames:
for frame in frames:
numbers.append(frame.number)
return numbers
class Frame(object):
params_dict = {
"number": None,
"x": None,
"y": None,
"z": None,
"yaw": None,
"red": None,
"green": None,
"blue": None,
"delay": None,
}
def __init__(self, csv_row=None, delay=None):
for key, value in self.params_dict.items():
setattr(self, key, value)
if csv_row:
self.load_csv_row(csv_row)
if delay:
self.delay = delay
def load_csv_row(self, csv_row):
number, x, y, z, yaw, red, green, blue = csv_row
self.number = int(number)
self.x = float(x)
self.y = float(y)
self.z = float(z)
self.yaw = float(yaw)
self.red = int(red)
self.green = int(green)
self.blue = int(blue)
def get_pos(self):
if None in [self.x, self.y, self.z]:
return []
else:
return [self.x, self.y, self.z]
def get_color(self):
if None in [self.red, self.green, self.blue]:
return []
else:
return [self.red, self.green, self.blue]
def pose_is_valid(self):
return self.get_pos() and (self.yaw is not None)
class Animation(object):
def __init__(self, config = None, filepath = "animation.csv"):
def __init__(self, config=None, filepath="animation.csv"):
self.id = None
self.static_begin_time = None
self.takeoff_time = None
@@ -43,7 +95,7 @@ class Animation(object):
if config is not None:
self.update_frames(config, filepath)
def load(self, delay=0.1, filepath="animation.csv"):
def load(self, filepath="animation.csv", delay=0.1):
self.original_frames = []
self.corrected_frames = []
self.filepath = filepath
@@ -67,42 +119,32 @@ class Animation(object):
logger.debug("Got new frame delay: {}".format(current_frame_delay))
else:
logger.debug("No animation id in file")
frame_number, x, y, z, yaw, red, green, blue = row_0
self.original_frames.append({
'number': int(frame_number),
'x': float(x),
'y': float(y),
'z': float(z),
'yaw': float(yaw),
'red': int(red),
'green': int(green),
'blue': int(blue),
'delay': current_frame_delay
})
try:
frame = Frame(row_0, current_frame_delay)
except ValueError as e:
logger.error("Can't parse row in csv file. {}".format(e))
return
else:
self.original_frames.append(frame)
for row in csv_reader:
if len(row) == 2:
current_frame_delay = float(row[1])
logger.debug("Got new frame delay: {}".format(current_frame_delay))
else:
frame_number, x, y, z, yaw, red, green, blue = row
self.original_frames.append({
'number': int(frame_number),
'x': float(x),
'y': float(y),
'z': float(z),
'yaw': float(yaw),
'red': int(red),
'green': int(green),
'blue': int(blue),
'delay': current_frame_delay
})
try:
frame = Frame(row, current_frame_delay)
except ValueError as e:
logger.error("Can't parse row in csv file. {}".format(e))
return
else:
self.original_frames.append(frame)
self.split_animation()
'''
Split animation into 5 parts: static_begin, takeoff, route, land, static_end
* static_begin and static_end are arrays of frames in the beginning and the end of animation,
* static_begin and static_end are chains of frames in the beginning and the end of animation,
where the drone doesn't move
* takeoff and land are arrays of frames after and before static frames of animation,
* takeoff and land are chains of frames after and before static frames of animation,
where the drone doesn't move in xy plane, and it's z coordinate only increases or decreases, respectively.
* route is the rest of the animation
Count static_begin_time and takeoff_time
@@ -123,36 +165,41 @@ class Animation(object):
while i < len(frames) - 1:
if moving(frames[i], frames[i+1], move_delta):
break
self.static_begin_time += frames[i]['delay']
self.static_begin_time += frames[i].delay
i += 1
self.static_begin_frames = frames[:i+1]
frames = frames[i+1:]
i = 0
if i > 0:
self.static_begin_frames = frames[:i+1]
frames = frames[i+1:]
i = 0
# Select takeoff frames
while i < len(frames) - 1:
if moving(frames[i], frames[i+1], move_delta, z = False) or frames[i]['z'] - frames[i+1]['z'] <= 0:
if moving(frames[i], frames[i+1], move_delta, z = False) or (frames[i+1].z - frames[i].z <= 0):
break
self.takeoff_time += frames[i]['delay']
self.takeoff_time += frames[i].delay
i += 1
self.takeoff_frames = frames[:i+1]
frames = frames[i+1:]
if i > 0:
self.takeoff_frames = frames[:i+1]
frames = frames[i+1:]
i = len(frames) - 1 # Moving index from the end
# Select static end frames
while i >= 0:
if moving(frames[i], frames[i-1], move_delta):
break
i -= 1
self.static_end_frames = frames[i:]
frames = frames[:i]
i -= len(frames) - 1
if i < len(frames) - 1:
self.static_end_frames = frames[i+1:]
frames = frames[:i+1]
i = len(frames) - 1
# Select land frames
while i >= 0:
if moving(frames[i], frames[i-1], move_delta, z = False) or frames[i-1]['z'] - frames[i]['z'] >= 0:
if moving(frames[i], frames[i-1], move_delta, z = False) or (frames[i-1].z - frames[i].z <= 0):
break
i -= 1
self.land_frames = frames[i:]
if i < len(frames) - 1:
self.land_frames = frames[i+1:]
frames = frames[:i+1]
# Get route frames
self.route_frames = frames[:i]
self.route_frames = frames
def make_output_frames(self, static_begin, takeoff, route, land, static_end):
self.output_frames = []
@@ -166,24 +213,25 @@ class Animation(object):
self.output_frames += self.land_frames
if static_end:
self.output_frames += self.static_end_frames
self.output_frames_min_z = min(self.output_frames, key = lambda p: p['z'])['z']
self.output_frames_min_z = min(self.output_frames, key = lambda p: p.z).z
def update_frames(self, config, filepath):
self.load(config.animation_frame_delay, filepath)
self.make_output_frames(config.animation_output_static_begin,
config.animation_output_takeoff,
config.animation_output_route,
config.animation_output_land,
config.animation_output_static_end)
self.load(filepath, config.animation_frame_delay)
if self.original_frames:
self.make_output_frames(config.animation_output_static_begin,
config.animation_output_takeoff,
config.animation_output_route,
config.animation_output_land,
config.animation_output_static_end)
def get_scaled_output(self, ratio = (1,1,1), offset = (0,0,0)):
x0, y0, z0 = offset
x_ratio, y_ratio, z_ratio = ratio
scaled_frames = copy.deepcopy(self.output_frames)
for frame in scaled_frames:
frame['x'] = x_ratio*frame['x'] + x0
frame['y'] = y_ratio*frame['y'] + y0
frame['z'] = z_ratio*frame['z'] + z0
frame.x = x_ratio*frame.x + x0
frame.y = y_ratio*frame.y + y0
frame.z = z_ratio*frame.z + z0
return scaled_frames
def get_scaled_output_min_z(self, ratio = (1,1,1), offset = (0,0,0)):
@@ -195,9 +243,9 @@ class Animation(object):
x0, y0, z0 = offset
x_ratio, y_ratio, z_ratio = ratio
first_frame = self.output_frames[0]
x = x_ratio*first_frame['x'] + x0
y = y_ratio*first_frame['y'] + y0
z = z_ratio*first_frame['z'] + z0
x = x_ratio*first_frame.x + x0
y = y_ratio*first_frame.y + y0
z = z_ratio*first_frame.z + z0
return x, y, z
def get_start_action(self, start_action, current_height, takeoff_level):
@@ -221,20 +269,19 @@ class Animation(object):
with open(filepath, mode='w+') as corrected_animation:
csv_writer = csv.writer(corrected_animation, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
for frame in self.corrected_frames:
csv_writer.writerow([frame['number'], frame['x'], frame['y'], frame['z'], frame['red'], frame['green'], frame['blue'], frame['delay']])
def convert_frame(frame):
return ((frame['x'], frame['y'], frame['z']), (frame['red'], frame['green'], frame['blue']), frame['yaw'])
csv_writer.writerow([frame.number, frame.x, frame.y, frame.z, frame.red, frame.green, frame.blue, frame.delay])
try:
def execute_frame(point=(), color=(), yaw=float('Nan'), frame_id='aruco_map', use_leds=True,
def execute_frame(frame, frame_id='aruco_map', use_leds=True,
flight_func=FlightLib.navto, auto_arm=False, flight_kwargs=None, interrupter=interrupt_event):
if flight_kwargs is None:
flight_kwargs = {}
flight_func(*point, yaw=yaw, frame_id=frame_id, auto_arm=auto_arm, interrupter=interrupt_event, **flight_kwargs)
if frame.pose_is_valid():
flight_func(x=frame.x, y=frame.y, z=frame.z, yaw=frame.yaw, frame_id=frame_id, auto_arm=auto_arm, interrupter=interrupt_event, **flight_kwargs)
else:
logger.debug("Frame pose is not valid for flying")
if use_leds:
if color:
if frame.get_color:
LedLib.fill(*color)
def takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, use_leds=True,

6
Drone/config/spec/configspec_client.ini
View File

@@ -71,6 +71,12 @@ frame_delay = float(default=0.1, min=0.01)
ratio = float_list(default=list(1.0, 1.0, 1.0), min=3, max=3)
# Available options: 'animation', 'nan' or a number in degrees
yaw = string(default=180.0)
[[OUTPUT]]
static_begin = boolean(default=False)
takeoff = boolean(default=True)
route = boolean(default=True)
land = boolean(default=False)
static_end = boolean(default=False)
[LED]
use = boolean(default=False)