mirror of
https://github.com/CopterExpress/clever-show.git
synced 2026-05-30 16:59:32 +00:00
86 lines
2.6 KiB
Python
86 lines
2.6 KiB
Python
import time
|
|
import csv
|
|
import rospy
|
|
import logging
|
|
from FlightLib import FlightLib
|
|
from FlightLib import LedLib
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
|
|
animation_file_path = 'animation.csv'
|
|
USE_LEDS = True
|
|
FRAME_ID = 'aruco_map'
|
|
|
|
|
|
def takeoff(z=1.5, safe_takeoff=True, timeout=5000):
|
|
if USE_LEDS:
|
|
LedLib.wipe_to(255, 0, 0)
|
|
FlightLib.takeoff(z=z, wait=True, timeout_takeoff=timeout, emergency_land=safe_takeoff,)
|
|
LedLib.blink(0, 255, 0, wait=50)
|
|
|
|
|
|
def land(descend=False):
|
|
if USE_LEDS:
|
|
LedLib.blink(255, 0, 0)
|
|
FlightLib.land(descend=descend)
|
|
if USE_LEDS:
|
|
LedLib.off()
|
|
|
|
|
|
def animate_frame(current_frame, x0=0.0, y0=0.0, copter_frame_id=FRAME_ID):
|
|
FlightLib.navto(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'],
|
|
yaw=1.57, frame_id=copter_frame_id) # TODO yaw
|
|
if USE_LEDS:
|
|
LedLib.fill(current_frame['red'], current_frame['green'], current_frame['blue'])
|
|
|
|
|
|
def reach_frame(current_frame, x0=0.0, y0=0.0, timeout=5000, copter_frame_id=FRAME_ID):
|
|
FlightLib.reach_point(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'],
|
|
yaw=1.57, timeout=timeout, frame_id=copter_frame_id) # TODO yaw
|
|
if USE_LEDS:
|
|
LedLib.fill(current_frame['red'], current_frame['green'], current_frame['blue'])
|
|
|
|
|
|
def read_animation_file(filepath=animation_file_path):
|
|
imported_frames = []
|
|
try:
|
|
animation_file = open(filepath)
|
|
except IOError:
|
|
logging.error("File {} can't be opened".format(filepath))
|
|
else:
|
|
with animation_file:
|
|
csv_reader = csv.reader(
|
|
animation_file, delimiter=',', quotechar='|'
|
|
)
|
|
for row in csv_reader:
|
|
frame_number, x, y, z, yaw, red, green, blue = row
|
|
imported_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),
|
|
})
|
|
return imported_frames
|
|
|
|
|
|
if __name__ == '__main__':
|
|
rospy.init_node('Animation_player', anonymous=True)
|
|
if USE_LEDS:
|
|
LedLib.init_led()
|
|
X0 = 0.5
|
|
Y0 = 1.0
|
|
frames = read_animation_file()
|
|
rate = rospy.Rate(8)
|
|
takeoff()
|
|
FlightLib.reach_point(x=frames[0]['x']+X0, y=frames[0]['y']+Y0, z=frames[0]['z'], yaw=11.57)
|
|
for frame in frames:
|
|
animate_frame(frame, x0=X0, y0=Y0)
|
|
rate.sleep()
|
|
land()
|
|
time.sleep(1)
|