def start(self, task_manager_instance):
client.logger.info("Init ROS node")
rospy.init_node('Swarm_client', anonymous=True)
if self.USE_LEDS:
LedLib.init_led(self.LED_PIN)
task_manager_instance.start()
super(CopterClient, self).start()
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)
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 use_leds:
LedLib.blink(0, 255, 0, wait=50, interrupter=interrupter)
def start(self, task_manager_instance):
rospy.loginfo("Init ROS node")
rospy.init_node('clever_show_client')
if self.USE_LEDS:
LedLib.init_led(self.LED_PIN)
task_manager_instance.start()
if self.FRAME_ID == "floor":
if self.FLOOR_FRAME_EXISTS:
self.start_floor_frame_broadcast()
else:
rospy.logerror("Can't make floor frame!")
start_subscriber()
telemetry_thread.start()
super(CopterClient, self).start()
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()
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 execute_frame(point=(),
color=(),
yaw=float('Nan'),
frame_id='aruco_map',
use_leds=True,
flight_func=FlightLib.navto,
flight_kwargs=None,
interrupter=interrupt_event):
if flight_kwargs is None:
flight_kwargs = {}
flight_func(*point,
yaw=yaw,
frame_id=frame_id,
interrupter=interrupt_event,
**flight_kwargs)
if use_leds:
if color:
LedLib.fill(*color)
def land():
LedLib.rainbow()
FlightLib.land()
LedLib.off()
def takeoff():
FlightLib.takeoff()
LedLib.wipe_to(0, 255, 0)
def land(self):
FlightLib.land()
LedLib.off()
self.state_machine.switch_state(
new_state=StateMachine.PAUSE_STATE
)
from FlightLib import LedLib as led
import time
time.sleep(0.1)
print("Starting test...")
#print("Fill")
#led.fill(255, 0, 0)
#time.sleep(10)
#print("Wipe")
#led.wipe_to(0, 255, 0)
#time.sleep(10)
#print("Fade")
#led.fade_to(0, 0, 255)
#time.sleep(10)
#print("Rainbow")
#led.rainbow()
#time.sleep(10)
#print("Chase")
#led.chase(255, 0, 255)
#time.sleep(10)
print("Run")
led.run(0, 255, 255, 10, wait=150)
time.sleep(10)
#print("Blink")
#led.blink(255, 255, 255)
#time.sleep(10)
led.off()
print("The end")
time.sleep(3)
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
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 _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)
def _command_led_test(*args, **kwargs):
LedLib.chase(255, 255, 255)
time.sleep(2)
LedLib.off()
USE_LEDS = config.getboolean('PRIVATE', 'use_leds')
play_animation.USE_LEDS = USE_LEDS
COPTER_ID = config.get('PRIVATE', 'id')
if COPTER_ID == 'default':
COPTER_ID = 'copter' + str(random.randrange(9999)).zfill(4)
write_to_config('PRIVATE', 'id', COPTER_ID)
elif COPTER_ID == '/hostname':
COPTER_ID = socket.gethostname()
load_config()
rospy.init_node('Swarm_client', anonymous=True)
if USE_LEDS:
LedLib.init_led()
print("Client started on copter:", COPTER_ID)
if USE_NTP:
print("NTP time:", time.ctime(get_ntp_time(NTP_HOST, NTP_PORT)))
print("System time", time.ctime(time.time()))
reconnect()
print("Connected to server")
try:
while True:
try:
message = recive_message()
if message:
data = str(sock.recv(1024))
if b'stop' in data:
xm.write(data[:data.index(b'stop')])
t_0 = Thread(target=animation)
t_0.daemon = True
t_0.start()
time.sleep(2)
break
else:
xm.write(data)
else:
try:
sq = data.split('$$')
for i in range(len(sq) - 1):
print(sq[i])
eval(sq[i])
except Exception as e:
print(e)
except:
print('er')
sock.close()
except KeyboardInterrupt:
print("Shutting down")
led.off()
sock.close()
def takeoff(self):
FlightLib.takeoff()
LedLib.rainbow()
self.state_machine.switch_state(
new_state=StateMachine.PAUSE_STATE
)
import time
from FlightLib import FlightLib as f, LedLib as led
f.init('SingleCleverFlight')
led.wipe_to(0, 255, 0)
f.takeoff(1.5)
led.rainbow()
#rectangle
f.reach(0.25, 0.25, 1.2)
led.fade_to(255, 0, 0)
f.reach(1, 0.25, 1.2)
led.fade_to(0, 255, 0)
f.reach(1, 2.2, 1.2)
led.fade_to(0, 0, 255)
f.reach(0.25, 2.2, 1.2)
led.fade_to(255, 255, 0)
f.reach(0.25, 0.25, 1.2)
led.fade_to(255, 0, 0)
#center_spin
f.reach(0.7, 1.1, 1.5)
led.run(255, 0, 255, length=15, direction=True)
f.spin(yaw_rate=0.6)
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)