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 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 do_next_animation(self):
current_frame = self.frames[self.current_frame_number]
FlightLib.reach(
current_frame['x'], current_frame['y'], current_frame['z'],
speed=current_frame['speed']
)
LedLib.fade_to(
current_frame['r'], current_frame['g'], current_frame['b']
)
def takeoff(z=1.5,
safe_takeoff=True,
frame_id='map',
timeout=5.0,
use_leds=True,
interrupter=interrupt_event):
print(interrupter.is_set())
if use_leds:
LedLib.wipe_to(255, 0, 0, interrupter=interrupter)
if interrupter.is_set():
return
result = FlightLib.takeoff(z=z,
wait=False,
timeout_takeoff=timeout,
frame_id=frame_id,
emergency_land=safe_takeoff,
interrupter=interrupter)
if result == 'not armed':
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 _response_selfcheck(*args, **kwargs):
if check_state_topic(wait_new_status=True):
check = FlightLib.selfcheck()
return check if check else "OK"
else:
stop_subscriber()
return "NOT_CONNECTED_TO_FCU"
def check_failsafe_and_interruption(self):
global emergency
# check current state
state = [self.mode, self.armed, task_manager.get_last_task_name()]
mode, armed, last_task = state
# check external interruption
external_interruption = (mode != "OFFBOARD" and armed == True
and last_task not in [None, 'land'])
log_msg = ''
if emergency:
log_msg += 'emergency and '
if external_interruption:
log_msg += 'external interruption and '
# count interruptions to avoid px4 mode glitches
if state == self._last_state:
self._interruption_counter += 1
else:
self._interruption_counter = 0
logger.info(
"Possible expernal interruption, state_counter = {}".format(
self._interruption_counter))
# delete last ' end ' from log message
if len(log_msg) > 5:
log_msg = log_msg[:-5]
# clear task manager if emergency or external interruption
if emergency or (external_interruption and self._interruption_counter
>= self._max_interruptions):
if not self._tasks_cleared:
logger.info("Clear task manager because of {}".format(log_msg))
logger.info("Mode: {} | armed: {} | last task: {} ".format(
mode, armed, last_task))
task_manager.reset()
FlightLib.reset_delta()
self._tasks_cleared = True
self._interruption_counter = 0
else:
self._tasks_cleared = False
self._last_state = state
def get_start_position(cls):
x_start, y_start = animation.get_start_xy(
os.path.abspath("animation.csv"),
x_ratio=client.active_client.X_RATIO,
y_ratio=client.active_client.Y_RATIO,
)
x_delta = client.active_client.X0 + client.active_client.X0_COMMON
y_delta = client.active_client.Y0 + client.active_client.Y0_COMMON
z_delta = client.active_client.Z0 + client.active_client.Z0_COMMON
x = x_start + x_delta
y = y_start + y_delta
if not FlightLib._check_nans(x, y, z_delta):
return x, y, z_delta
return 'NO_POS'
def _command_takeoff_z(*args, **kwargs):
z_str = kwargs.get("z", None)
if z_str is not None:
telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
logger.info("Takeoff to z = {} at {}".format(z_str,
datetime.datetime.now()))
task_manager.add_task(0,
0,
FlightLib.reach_point,
task_kwargs={
"x": telem.x,
"y": telem.y,
"z": float(z_str),
"frame_id": client.active_client.FRAME_ID,
"timeout": client.active_client.TAKEOFF_TIME,
"auto_arm": True,
})
def update_telemetry_fast(self):
self.start_position = self.get_start_position()
try:
self.ros_telemetry = FlightLib.get_telemetry_locked(
client.active_client.FRAME_ID)
if self.ros_telemetry.connected:
self.armed = self.ros_telemetry.armed
self.mode = self.ros_telemetry.mode
self.selfcheck = self.get_selfcheck()
self.current_position = self.get_position(self.ros_telemetry)
else:
self.reset_telemetry_values()
except rospy.ServiceException:
rospy.logdebug("Some service is unavailable")
self.selfcheck = ["WAIT_ROS"]
except AttributeError as e:
rospy.logdebug(e)
except rospy.TransportException as e:
rospy.logdebug(e)
self.time = time.time()
self.round_telemetry()
def _command_move_start_to_current_position(*args, **kwargs):
x_start, y_start = animation.get_start_xy(
os.path.abspath("animation.csv"),
x_ratio=client.active_client.X_RATIO,
y_ratio=client.active_client.Y_RATIO,
)
logger.debug("x_start = {}, y_start = {}".format(x_start, y_start))
if not math.isnan(x_start):
telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
logger.debug("x_telem = {}, y_telem = {}".format(telem.x, telem.y))
if not math.isnan(telem.x):
client.active_client.config.set('PRIVATE', 'x0', telem.x - x_start)
client.active_client.config.set('PRIVATE', 'y0', telem.y - y_start)
client.active_client.rewrite_config()
client.active_client.load_config()
logger.info("Set start delta: {:.2f} {:.2f}".format(
client.active_client.X0, client.active_client.Y0))
else:
logger.debug("Wrong telemetry")
else:
logger.debug("Wrong animation file")
def _response_selfcheck():
check = FlightLib.selfcheck()
return check if check else "OK"
def land():
LedLib.rainbow()
FlightLib.land()
LedLib.off()
def takeoff():
FlightLib.takeoff()
LedLib.wipe_to(0, 255, 0)
import time
import csv
from FlightLib import FlightLib
FlightLib.init('SingleCleverFlight')
from FlightLib import LedLib
animation_file_path = 'animation.csv'
frames = []
def takeoff():
FlightLib.takeoff()
LedLib.wipe_to(0, 255, 0)
def land():
LedLib.rainbow()
FlightLib.land()
LedLib.off()
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 _response_batt():
return FlightLib.get_telemetry('body').voltage
def land(self):
FlightLib.land()
LedLib.off()
self.state_machine.switch_state(
new_state=StateMachine.PAUSE_STATE
)
def takeoff(self):
FlightLib.takeoff()
LedLib.rainbow()
self.state_machine.switch_state(
new_state=StateMachine.PAUSE_STATE
)
def _response_cell(*args, **kwargs):
if check_state_topic(wait_new_status=True):
return FlightLib.get_telemetry_locked('body').cell_voltage
else:
stop_subscriber()
return float('nan')
def _copter_flip():
FlightLib.flip(frame_id=client.active_client.FRAME_ID)
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
from FlightLib import FlightLib as f
f.init('server')
# from FlightLib import TelemLib
from FlightLib import LedLib as led
from time import sleep
import socket
import sys
sq = []
while True:
try:
sock = socket.socket()
sock.connect(('192.168.1.6', 35001))
except socket.error:
print("No connection. Sleep 10 secs.")
sleep(10)
else:
break
def animation():
def start_animation_1(self):
# xml parcer
time.sleep(4.5)
types = {
'x': float,
'y': float,
'z': float,
def _copter_flip(*args, **kwargs):
FlightLib.flip(frame_id=client.active_client.FRAME_ID)
def _command_set_z(*args, **kwargs):
telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
client.active_client.config.set('PRIVATE', 'z0', telem.z)
client.active_client.rewrite_config()
client.active_client.load_config()
logger.info("Set z offset to {:.2f}".format(client.active_client.Z0))
def _response_position(*args, **kwargs):
telem = FlightLib.get_telemetry_locked(client.active_client.FRAME_ID)
return "{:.2f} {:.2f} {:.2f} {:.1f} {}".format(
telem.x, telem.y, telem.z, math.degrees(telem.yaw),
client.active_client.FRAME_ID)
def selfcheck():
return FlightLib.selfcheck()
def _response_cell():
return FlightLib.get_telemetry('body').cell_voltage
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 telemetry_loop():
global telemetry, emergency
last_state = []
equal_state_counter = 0
max_count = 2
tasks_cleared = False
rate = rospy.Rate(client.active_client.TELEM_FREQ)
while not rospy.is_shutdown():
telemetry = telemetry._replace(animation_id=animation.get_id())
telemetry = telemetry._replace(git_version=subprocess.check_output(
"git log --pretty=format:'%h' -n 1", shell=True))
x_start, y_start = animation.get_start_xy(
os.path.abspath("animation.csv"),
x_ratio=client.active_client.X_RATIO,
y_ratio=client.active_client.Y_RATIO,
)
x_delta = client.active_client.X0 + client.active_client.X0_COMMON
y_delta = client.active_client.Y0 + client.active_client.Y0_COMMON
z_delta = client.active_client.Z0 + client.active_client.Z0_COMMON
if not math.isnan(x_start):
telemetry = telemetry._replace(
start_position='{:.2f} {:.2f} {:.2f}'.format(
x_start + x_delta, y_start + y_delta, z_delta))
else:
telemetry = telemetry._replace(start_position='NO_POS')
services_unavailable = FlightLib.check_ros_services_unavailable()
if not services_unavailable:
try:
ros_telemetry = FlightLib.get_telemetry_locked(
client.active_client.FRAME_ID)
if ros_telemetry.connected:
telemetry = telemetry._replace(armed=ros_telemetry.armed)
telemetry = telemetry._replace(
battery_v='{:.2f}'.format(ros_telemetry.voltage))
batt_empty_param = get_param('BAT_V_EMPTY')
batt_charged_param = get_param('BAT_V_CHARGED')
batt_cells_param = get_param('BAT_N_CELLS')
if batt_empty_param.success and batt_charged_param.success and batt_cells_param.success:
batt_empty = batt_empty_param.value.real
batt_charged = batt_charged_param.value.real
batt_cells = batt_cells_param.value.integer
try:
telemetry = telemetry._replace(
battery_p='{}'.format(
int(
min((ros_telemetry.voltage /
batt_cells - batt_empty) /
(batt_charged - batt_empty) *
100., 100))))
except ValueError:
telemetry = telemetry._replace(battery_p='nan')
else:
telemetry = telemetry._replace(battery_p='nan')
telemetry = telemetry._replace(
calibration_status=get_calibration_status())
telemetry = telemetry._replace(
system_status=get_sys_status())
telemetry = telemetry._replace(mode=ros_telemetry.mode)
check = FlightLib.selfcheck()
if not check:
check = "OK"
telemetry = telemetry._replace(selfcheck=str(check))
if not math.isnan(ros_telemetry.x):
telemetry = telemetry._replace(
current_position='{:.2f} {:.2f} {:.2f} {:.1f} {}'.
format(ros_telemetry.x, ros_telemetry.y,
ros_telemetry.z,
math.degrees(ros_telemetry.yaw),
client.active_client.FRAME_ID))
else:
telemetry = telemetry._replace(
current_position='NO_POS in {}'.format(
client.active_client.FRAME_ID))
else:
telemetry = telemetry._replace(battery_v='nan')
telemetry = telemetry._replace(battery_p='nan')
telemetry = telemetry._replace(calibration_status='NO_FCU')
telemetry = telemetry._replace(system_status='NO_FCU')
telemetry = telemetry._replace(mode='NO_FCU')
telemetry = telemetry._replace(selfcheck='NO_FCU')
telemetry = telemetry._replace(current_position='NO_POS')
except rospy.ServiceException:
logger.debug("Some service is unavailable")
except AttributeError as e:
logger.debug(e)
except rospy.TransportException as e:
logger.debug(e)
else:
telemetry = telemetry._replace(selfcheck='WAIT_ROS')
if client.active_client.TELEM_TRANSMIT:
try:
client.active_client.server_connection.send_message(
'telem',
args={'message': create_telemetry_message(telemetry)})
except AttributeError as e:
logger.debug(e)
if client.active_client.CLEAR_TASKS_WHEN_EMERGENCY:
mode = telemetry.mode
armed = telemetry.armed
last_task = task_manager.get_last_task_name()
state = [mode, armed, last_task]
if state == last_state:
equal_state_counter += 1
else:
equal_state_counter = 0
external_interruption = (mode != "OFFBOARD" and armed == True
and last_task not in [None, 'land'])
log_msg = ''
if emergency and external_interruption:
log_msg = "emergency and external interruption"
elif emergency:
log_msg = "emergency"
elif external_interruption:
log_msg = "external interruption"
logger.info(
"Possible expernal interruption, state_counter = {}".
format(equal_state_counter))
if emergency or (external_interruption
and equal_state_counter >= max_count):
if not tasks_cleared:
logger.info(
"Clear task manager because of {}".format(log_msg))
logger.info("Mode: {} | armed: {} | last task: {} ".format(
mode, armed, last_task))
task_manager.reset()
FlightLib.reset_delta()
tasks_cleared = True
equal_state_counter = 0
else:
tasks_cleared = False
last_state = state
if client.active_client.LOG_CPU_AND_MEMORY:
cpu_usage = psutil.cpu_percent(interval=None, percpu=True)
mem_usage = psutil.virtual_memory().percent
cpu_temp_info = psutil.sensors_temperatures()['cpu-thermal'][0]
cpu_temp = cpu_temp_info.current
# https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md
throttled_hex = subprocess.check_output("vcgencmd get_throttled",
shell=True).split('=')[1]
under_voltage = bool(int(bin(int(throttled_hex, 16))[2:][-1]))
power_state = 'normal' if not under_voltage else 'under voltage!'
if cpu_temp_info.critical:
cpu_temp_state = 'critical'
elif cpu_temp_info.high:
cpu_temp_state = 'high'
else:
cpu_temp_state = 'normal'
logger.info(
"CPU usage: {} | Memory: {} % | T: {} ({}) | Power: {}".format(
cpu_usage, mem_usage, cpu_temp, cpu_temp_state,
power_state))
delta = FlightLib.get_delta()
logger.info("Delta: {}".format(delta))
if delta > client.active_client.LAND_POS_DELTA:
_command_land()
rate.sleep()
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)
starttime = float(args['time'])
print("Starting on:", time.ctime(starttime))
dt = starttime - time.time()
if USE_NTP:
dt = starttime - get_ntp_time(NTP_HOST, NTP_PORT)
print("Until start:", dt)
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"