def run(self):
self.connect()
red = 0
green = 0
blue = 0
while True:
data = self.fetch_data()
if 'color' in data:
data = data.split()[1:]
color = map(int, data)
red, green, blue = color
elif data == 'rainbow':
LedLib.rainbow()
elif data == 'fill':
LedLib.fill(red, green, blue)
elif data == 'blink':
LedLib.blink(red, green, blue)
elif data == 'chase':
LedLib.chase(red, green, blue)
elif data == 'wipe_to':
LedLib.wipe_to(red, green, blue)
elif data == 'fade_to':
LedLib.fade_to(red, green, blue)
elif data == 'run':
LedLib.fade_to(red, green, blue)
elif data == 'close':
LedLib.off()
time.sleep(0.001)
def do_next_animation(current_frame):
FlightLib.navto(round(float(current_frame['x']), 4),
round(float(current_frame['y']), 4),
round(float(current_frame['z']), 4),
round(float(current_frame['yaw']), 4),
speed=2)
LedLib.fill(int(current_frame['green']), int(current_frame['red']),
int(current_frame['blue']))
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)
def _command_emergency_led_fill(**kwargs):
r = g = b = 0
try:
r = kwargs["red"]
except KeyError:
pass
try:
g = kwargs["green"]
except KeyError:
pass
try:
b = kwargs["blue"]
except KeyError:
pass
LedLib.fill(r, g, b)
def start_animation_1(self):
# xml parcer
time.sleep(4.5)
types = {
'x': float,
'y': float,
'z': float,
'yaw': float,
'yaw_rate': float,
'speed': float,
'tolerance': float,
'frame_id': str,
'mode': str,
'wait_ms': int,
'timeout': int,
'z_coefficient': float,
'timeout_arm': int,
'timeout_land': int,
'preland': bool,
'r': int,
'g': int,
'b': int,
}
def parse_xml(xml_file=None, xml_str=None):
if (xml_file is None
and xml_str is None) or (xml_file is not None
and xml_str is not None):
raise ValueError('You must use one parameter')
if xml_str is None:
with open(xml_file, 'r') as f:
xml = f.read().strip()
else:
xml = xml_str
xmldict = xmltodict.parse(xml)
xmldict = dict(xmldict['DroneSwarm'])['time']
ready = {}
if type(xmldict) != list:
xmldict = [xmldict]
for t in xmldict:
time = float(t['@t'])
ready[time] = {}
try:
if type(t['copter']) != list:
t['copter'] = [t['copter']]
for copter in t['copter']:
copternum = int(copter['@n'])
ready[time][copternum] = []
copter.pop('@n')
for action in copter:
actiondict = {}
try:
for prm in dict(copter[action]):
val = dict(copter[action])[prm]
prm = str(prm.replace('@', ''))
try:
actiondict[prm] = types[prm](val)
except KeyError:
print("Types hasn't got " + prm +
', use str.')
actiondict[prm] = str(val)
# print {action: actiondict}
ready[time][copternum].append(
{str(action): actiondict})
except ValueError:
raise ValueError(
'You can use only "n" parameter in "copter" tag'
)
except KeyError:
pass
try:
swarm = t['swarm']
copternum = 0
ready[time][copternum] = []
for action in swarm:
actiondict = {}
try:
if swarm[action] is not None:
for prm in dict(swarm[action]):
val = dict(swarm[action])[prm]
prm = str(prm.replace('@', ''))
try:
actiondict[prm] = types[prm](val)
except KeyError:
print("Types hasn't got " + prm +
', use str.')
actiondict[prm] = str(val)
ready[time][copternum].append(
{str(action): actiondict})
except TypeError:
raise ValueError(
'You can use only one "swarm" tag')
except KeyError:
pass
return ready
data = ''
xm = parse_xml(xml.xml)
print(xm)
n = 0
for i in xm:
for k in xm[i]:
# k = copter number
s = str(xm.keys())[str(xm.keys()).index('[') + 1:-2]
p = s.split(', ')
try:
timeout = str(((float(p[n + 1]) - float(p[n])) * 1000) -
2000)
except:
print('end')
for l in xm[i][k]:
o = str(l.keys())
f = o[o.index('[\'') + 2:-3] # f = function
# l[f] = parameters
if k == 0:
k = 'all'
if f == 'circle':
x = str(l[f]['x'])
y = str(l[f]['y'])
z = str(l[f]['z'])
r = str(l[f]['r'])
data = 'f.circle' + '(' + x + ',' + y + ',' + z + ',' + r + ',' + 'timeout =' + timeout + ')'
print(data, k)
print('_______________________')
self.sender(bytes(data, 'utf-8'), str(k))
if f == 'music':
file = str(l[f]['file'])
os.system(r"start" + file)
time.sleep(3)
elif f == 'led':
print(n)
r = float(l[f]['r'])
g = float(l[f]['g'])
b = float(l[f]['b'])
led.fill(r, g, b)
print(bytes(data, 'utf-8'), str(k))
print('_______________________')
elif f == 'reach':
x = float(l[f]['x'])
y = float(l[f]['y'])
z = float(l[f]['z'])
try:
speed = float(l[f]['speed'])
f.reach(x, y, z, speed=speed, timeout=timeout)
except:
speed = 0.6
f.reach(x, y, z, speed=speed, timeout=timeout)
print(bytes(data, 'utf-8'), str(k))
print('_______________________')
elif f == 'takeoff':
z = float(l[f]['z'])
try:
speed = float(l[f]['speed'])
f.takeoff(z,
timeout_arm=1000,
speed=speed,
timeout_fcu=(float(timeout) - 1000) // 2,
timeout=(float(timeout) - 1000) // 2)
print(bytes(data, 'utf-8'), str(k))
except:
f.takeoff(z,
timeout_arm=1000,
timeout_fcu=str(
(float(timeout) - 1000) // 2),
timeout=(float(timeout) - 1000) // 2)
print(bytes(data, 'utf-8'), str(k))
print('_______________________')
elif f == 'land':
f.land(timeout=float(timeout))
print(bytes(data, 'utf-8'), str(k))
print('_______________________')
elif f == 'attitude':
z = float(l[f]['z'])
f.attitude(z, timeout=timeout)
print(bytes(data, 'utf-8'), str(k))
print('_______________________')
elif f == 'stay':
x = str(l[f]['x'])
y = str(l[f]['y'])
print(bytes(x + ' ' + y, 'utf-8'), str(k))
print('_______________________')
s = str(xm.keys())[str(xm.keys()).index('[') + 1:-2]
p = s.split(', ')
print(p)
try:
time.sleep((float(timeout) + 1500) // 1000)
except:
print('end of list')
n += 1
rospy.Timer(rospy.Duration(dt), start_animation, oneshot=True)
elif command == 'takeoff':
play_animation.takeoff(safe_takeoff=SAFE_TAKEOFF)
elif command == 'pause':
pause_animation()
elif command == 'resume':
resume_animation()
elif command == 'stop':
stop_animation()
FlightLib.interrupt()
elif command == 'land':
play_animation.land()
elif command == 'disarm':
FlightLib.arming(False)
elif command == 'led_test':
LedLib.fill(255, 255, 255)
time.sleep(2)
LedLib.off()
elif command == 'request':
request_target = args['value']
print("Got request for:", request_target)
response = ""
if request_target == 'test':
response = "test_success"
elif request_target == 'id':
response = COPTER_ID
elif request_target == 'selfcheck':
check = FlightLib.selfcheck()
response = check if check else "OK"
elif request_target == 'batt_voltage':
def _command_led_fill(*args, **kwargs):
r = kwargs.get("red", 0)
g = kwargs.get("green", 0)
b = kwargs.get("blue", 0)
LedLib.fill(r, g, b)
import time
from FlightLib import FlightLib as f
f.init('CleverSwarmFlight')
from FlightLib import LedLib as led
led.fill(255, 255, 255)
f.safety_check()
f.takeoff(yaw=0)
led.rainbow()
f.reach(0.5, 0.5, 1)
led.run(255, 0, 0, 10)
f.spin(yaw_rate=0.5, yaw_final=0)
led.chase(0, 255, 0)
f.land()
led.off()
time.sleep(3)