Skip to content

Commit

Permalink
Animation update delay (#56)
Browse files Browse the repository at this point in the history 
* Client: Make time_delay programmable from animation file

* Client: Add yaw parameter in client_config
  • Loading branch information
@goldarte
goldarte committed Nov 22, 2019
1 parent d4c002f commit 3cfb4a9
Show file tree
Hide file tree
Showing 3 changed files with 104 additions and 73 deletions.
167 changes: 96 additions & 71 deletions Drone/animation_lib.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,28 @@
import rospy
import logging
import threading
import ConfigParser

from FlightLib import FlightLib
from FlightLib import LedLib
try:
from FlightLib import FlightLib
except ImportError:
print("Can't import FlightLib")
try:
from FlightLib import LedLib
except ImportError:
print("Can't import LedLib")

import tasking_lib as tasking

logger = logging.getLogger(__name__)

interrupt_event = threading.Event()

config = ConfigParser.ConfigParser()
config.read("client_config.ini")

default_delay = config.getfloat('ANIMATION', 'frame_delay')

anim_id = "Empty id"

# TODO refactor as class
Expand Down Expand Up @@ -54,23 +66,20 @@ def get_start_xy(filepath="animation.csv", x_ratio=1, y_ratio=1):
animation_file, delimiter=',', quotechar='|'
)
try:
row_0 = csv_reader.next()
row_frame = csv_reader.next()
except:
return float('nan'), float('nan')
if len(row_frame) == 1:
anim_id = row_frame[0]
logger.debug("Got animation_id: {}".format(row_frame[0]))
row_frame = csv_reader.next()
if len(row_frame) == 2:
logger.debug("Got frame delay: {}".format(row_frame[1]))
row_frame - csv_reader.next()
try:
frame_number, x, y, z, yaw, red, green, blue = row_frame
except:
return float('nan'), float('nan')
if len(row_0) == 1:
anim_id = row_0[0]
logger.debug("Got animation_id: {}".format(anim_id))
try:
frame_number, x, y, z, yaw, red, green, blue = csv_reader.next()
except:
return float('nan'), float('nan')
else:
anim_id = "Empty id"
logger.debug("No animation id in file")
try:
frame_number, x, y, z, yaw, red, green, blue = row_0
except:
return float('nan'), float('nan')
return float(x)*x_ratio, float(y)*y_ratio


Expand All @@ -84,13 +93,17 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_rati
anim_id = "No animation"
else:
with animation_file:
current_frame_delay = default_delay
csv_reader = csv.reader(
animation_file, delimiter=',', quotechar='|'
)
row_0 = csv_reader.next()
if len(row_0) == 1:
anim_id = row_0[0]
logger.debug("Got animation_id: {}".format(anim_id))
elif len(row_0) == 2:
current_frame_delay = float(row_0[1])
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
Expand All @@ -103,25 +116,31 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0, x_ratio=1, y_rati
'red': int(red),
'green': int(green),
'blue': int(blue),
'delay': current_frame_delay
})
for row in csv_reader:
frame_number, x, y, z, yaw, red, green, blue = row
imported_frames.append({
'number': int(frame_number),
'x': x_ratio*float(x) + x0,
'y': y_ratio*float(y) + y0,
'z': z_ratio*float(z) + z0,
'yaw': float(yaw),
'red': int(red),
'green': int(green),
'blue': int(blue),
})
if len(row) == 2:
current_frame_delay = float(row[1])
else:
frame_number, x, y, z, yaw, red, green, blue = row
imported_frames.append({
'number': int(frame_number),
'x': x_ratio*float(x) + x0,
'y': y_ratio*float(y) + y0,
'z': z_ratio*float(z) + z0,
'yaw': float(yaw),
'red': int(red),
'green': int(green),
'blue': int(blue),
'delay': current_frame_delay
})
return imported_frames

def correct_animation(frames, frame_delay=0.1, min_takeoff_height=0.5, move_delta=0.01, check_takeoff=True, check_land=True):
corrected_frames = copy.deepcopy(frames)
start_action = 'takeoff'
frames_to_start = 0
time_to_start = 0
if len(corrected_frames) == 0:
raise Exception('Nothing to correct!')
# Check takeoff
Expand All @@ -133,9 +152,10 @@ def correct_animation(frames, frame_delay=0.1, min_takeoff_height=0.5, move_delt
for i in range(len(corrected_frames)-1):
if corrected_frames[i-frames_to_start+1]['z'] - corrected_frames[i-frames_to_start]['z'] > move_delta:
break
time_to_start += corrected_frames[i-frames_to_start]['delay']
del corrected_frames[i-frames_to_start]
frames_to_start += 1
start_delay = frames_to_start*frame_delay
start_delay = time_to_start
# Check Land
# If copter lands in animation, landing points can be deleted
if (corrected_frames[len(corrected_frames)-1]['z'] < min_takeoff_height) and check_land:
Expand All @@ -156,60 +176,65 @@ def save_corrected_animation(frames, filename="corrected_animation.csv"):
corrected_animation = open(filename, mode='w+')
csv_writer = csv.writer(corrected_animation, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
for frame in frames:
csv_writer.writerow([frame['number'],frame['x'], frame['y'], frame['z']])
csv_writer.writerow([frame['number'],frame['x'], frame['y'], frame['z'], frame['delay']])
# print frame
corrected_animation.close()

def convert_frame(frame):
return ((frame['x'], frame['y'], frame['z']), (frame['red'], frame['green'], frame['blue']), frame['yaw'])

try:
def execute_frame(point=(), color=(), yaw=float('Nan'), 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 = {}

def execute_frame(point=(), color=(), yaw=float('Nan'), 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 use_leds:
if color:
LedLib.fill(*color)

flight_func(*point, yaw=yaw, frame_id=frame_id, auto_arm=auto_arm, interrupter=interrupt_event, **flight_kwargs)
if use_leds:
if color:
LedLib.fill(*color)


def execute_animation(frames, frame_delay, frame_id='aruco_map', use_leds=True, flight_func=FlightLib.navto,
interrupter=interrupt_event):
next_frame_time = 0
for frame in frames:

def execute_animation(frames, frame_delay, frame_id='aruco_map', use_leds=True, flight_func=FlightLib.navto,
interrupter=interrupt_event):
next_frame_time = 0
for frame in frames:
if interrupter.is_set():
logger.warning("Animation playing function interrupted!")
interrupter.clear()
return
execute_frame(*convert_frame(frame), frame_id=frame_id, use_leds=use_leds, flight_func=flight_func,
interrupter=interrupter)

next_frame_time += frame_delay
tasking.wait(next_frame_time, interrupter)


def takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, use_leds=True,
interrupter=interrupt_event):
if use_leds:
LedLib.wipe_to(255, 0, 0, interrupter=interrupter)
if interrupter.is_set():
logger.warning("Animation playing function interrupted!")
interrupter.clear()
return
execute_frame(*convert_frame(frame), frame_id=frame_id, use_leds=use_leds, flight_func=flight_func,
interrupter=interrupter)

next_frame_time += frame_delay
tasking.wait(next_frame_time, interrupter)
result = FlightLib.takeoff(height=z, timeout_takeoff=timeout, frame_id=frame_id,
emergency_land=safe_takeoff, interrupter=interrupter)
if result == 'not armed' or result == 'timeout':
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 takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, 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.wipe_to(255, 0, 0, interrupter=interrupter)
if interrupter.is_set():
return
result = FlightLib.takeoff(height=z, timeout_takeoff=timeout, frame_id=frame_id,
emergency_land=safe_takeoff, interrupter=interrupter)
if result == 'not armed' or result == 'timeout':
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=5.0, frame_id='aruco_map', use_leds=True,
interrupter=interrupt_event):
if use_leds:
LedLib.blink(255, 0, 0, interrupter=interrupter)
FlightLib.land(z=z, descend=descend, timeout_land=timeout, frame_id_land=frame_id, interrupter=interrupter)
if use_leds:
LedLib.off()
if use_leds:
LedLib.blink(255, 0, 0, interrupter=interrupter)
FlightLib.land(z=z, descend=descend, timeout_land=timeout, frame_id_land=frame_id, interrupter=interrupter)
if use_leds:
LedLib.off()

except NameError:
print("Can't create flying functions")
1 change: 1 addition & 0 deletions Drone/client_config.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ land_time = 3.0
x0_common = 0
y0_common = 0
z0_common = 0
yaw = 180
land_timeout = 6.0

[FLOOR FRAME]
Expand Down
9 changes: 7 additions & 2 deletions Drone/copter_client.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,7 @@ def load_config(self):
self.X0_COMMON = self.config.getfloat('COPTERS', 'x0_common')
self.Y0_COMMON = self.config.getfloat('COPTERS', 'y0_common')
self.Z0_COMMON = self.config.getfloat('COPTERS', 'z0_common')
self.YAW = self.config.get('COPTERS', 'yaw')
self.TAKEOFF_CHECK = self.config.getboolean('ANIMATION', 'takeoff_animation_check')
self.LAND_CHECK = self.config.getboolean('ANIMATION', 'land_animation_check')
self.FRAME_DELAY = self.config.getfloat('ANIMATION', 'frame_delay')
Expand Down Expand Up @@ -581,7 +582,6 @@ def _play_animation(*args, **kwargs):
check_takeoff=client.active_client.TAKEOFF_CHECK,
check_land=client.active_client.LAND_CHECK,
)

# Choose start action
if start_action == 'takeoff':
# Takeoff first
Expand Down Expand Up @@ -635,16 +635,21 @@ def _play_animation(*args, **kwargs):
# Play animation file
for frame in corrected_frames:
point, color, yaw = animation.convert_frame(frame)
if client.active_client.YAW == "animation":
yaw = frame["yaw"]
else:
yaw = math.radians(float(client.active_client.YAW))
task_manager.add_task(frame_time, 0, animation.execute_frame,
task_kwargs={
"point": point,
"color": color,
"yaw": yaw,
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
"flight_func": FlightLib.navto,
}
)
frame_time += client.active_client.FRAME_DELAY
frame_time += frame["delay"]

# Calculate land_time
land_time = frame_time + client.active_client.LAND_TIME
Expand Down

0 comments on commit 3cfb4a9

Please sign in to comment.