def __init__(self, ac_id):
wx.Frame.__init__(self, id=-1, parent=None, \
name=u'GVF', size=wx.Size(WIDTH, HEIGHT), \
style=wx.DEFAULT_FRAME_STYLE, title=u'Guidance Vector Field')
# Vehicle variables
self.ac_id = ac_id
self.course = 0
self.yaw = 0
self.XY = np.array([0, 0])
# Desired trajectory
self.timer_traj = 0 # We do not update the traj every time we receive a msg
self.timer_traj_lim = 7 # (7+1) * 0.25secs
self.s = 0
self.ke = 0
self.map_gvf = map2d(np.array([0, 0]), 150000)
self.traj = None
# Frame
self.canvas = FigureCanvas(self, -1, self.map_gvf.fig)
self.Bind(wx.EVT_CLOSE, self.OnClose)
self.redraw_timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.OnRedrawTimer, self.redraw_timer)
self.redraw_timer.Start(100)
# Ivy
self.interface = IvyMessagesInterface("GVF")
self.interface.subscribe(self.message_recv)
settings = PaparazziACSettings(ac_id)
def guidance_setmode(ac_id, value):
retval = ''
try:
settings = PaparazziACSettings(ac_id)
except Exception as e:
print(e)
return
try:
index = settings.name_lookup['auto2'].index
except Exception as e:
print(e)
print("auto2 setting not found, mode change not possible.")
return
if index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = ac_id
msg['index'] = index
msg['value'] = value
if verbose:
print_ivy_trace(msg)
retval = 'Guidance mode: ac_id=%d, index=%d, value=%d\n' % (
ac_id, index, value)
ivy_interface.send(msg)
if curl: print_curl_format()
return retval
def __init__(self, config, freq=10., verbose=False):
self.config = config
self.step = 1. / freq
self.verbose = verbose
self.ids = self.config['ids']
self.B = np.array(self.config['topology'])
self.Zdesired = np.array(self.config['desired_intervehicle_angles'])
self.k = np.array(self.config['gain'])
self.radius = np.array(self.config['desired_stationary_radius'])
self.aircraft = [Aircraft(i) for i in self.ids]
self.sigmas = np.zeros(len(self.aircraft))
for ac in self.aircraft:
settings = PaparazziACSettings(ac.id)
list_of_indexes = ['ell_a', 'ell_b']
for setting_ in list_of_indexes:
try:
index = settings.name_lookup[setting_].index
if setting_ == 'ell_a':
ac.a_index = index
if setting_ == 'ell_b':
ac.b_index = index
except Exception as e:
print(e)
print(setting_ + " setting not found, \
have you forgotten to check gvf.xml for your settings?"
)
# Start IVY interface
self._interface = IvyMessagesInterface("Circular Formation")
# bind to NAVIGATION message
def nav_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "NAVIGATION":
ac = self.aircraft[self.ids.index(ac_id)]
ac.XY[0] = float(msg.get_field(2))
ac.XY[1] = float(msg.get_field(3))
ac.initialized_nav = True
self._interface.subscribe(nav_cb, PprzMessage("telemetry",
"NAVIGATION"))
def gvf_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "GVF":
if int(msg.get_field(1)) == 1:
ac = self.aircraft[self.ids.index(ac_id)]
param = msg.get_field(4)
ac.XYc[0] = float(param[0])
ac.XYc[1] = float(param[1])
ac.a = float(param[2])
ac.b = float(param[3])
ac.s = float(msg.get_field(2))
ac.initialized_gvf = True
self._interface.subscribe(gvf_cb, PprzMessage("telemetry", "GVF"))
def main():
if len(sys.argv) != 6:
print(
"Usage: gvfFormationApp topology.txt desired_sigma.txt ids.txt radius k"
)
return
B = np.loadtxt(sys.argv[1])
desired_sigmas = np.loadtxt(sys.argv[2]) * np.pi / 180.0
ids = np.loadtxt(sys.argv[3])
radius = float(sys.argv[4])
k = float(sys.argv[5])
global list_ids
list_ids = np.ndarray.tolist(ids)
map(int, list_ids)
if np.size(ids) != np.size(B, 0):
print(
"The ammount of aircrafts in the topology and ids does not match")
return
for i in range(0, len(ids)):
list_aircraft.append(aircraft(int(ids[i])))
# Ivy
interface.subscribe(message_recv)
for ac in list_aircraft:
settings = PaparazziACSettings(ac.id)
list_of_indexes = ['ell_a', 'ell_b']
for setting_ in list_of_indexes:
try:
index = settings.name_lookup[setting_].index
if setting_ == 'ell_a':
ac.a_index = index
if setting_ == 'ell_b':
ac.b_index = index
except Exception as e:
print(e)
print(setting_ + " setting not found, \
have you forgotten gvf.xml in your settings?")
try:
while True:
time.sleep(0.5)
formation(B, desired_sigmas, radius, k)
except KeyboardInterrupt:
return
def __init__(self, ac_id):
wx.Frame.__init__(self, id=-1, parent=None, \
name=u'GVF', size=wx.Size(WIDTH, HEIGHT), \
style=wx.DEFAULT_FRAME_STYLE, title=u'Guidance Vector Field')
# Vehicle variables
self.ac_id = ac_id
self.course = 0
self.yaw = 0
self.XY = np.array([0, 0])
# Desired trajectory
self.timer_traj = 0 # We do not update the traj every time we receive a msg
self.timer_traj_lim = 7 # (7+1) * 0.25secs
self.s = 0
self.kn = 0
self.ke = 0
self.map_gvf = map2d(np.array([0, 0]), 150000)
self.traj = None
# Frame
self.canvas = FigureCanvas(self, -1, self.map_gvf.fig)
self.Bind(wx.EVT_CLOSE, self.OnClose)
self.redraw_timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.OnRedrawTimer, self.redraw_timer)
self.redraw_timer.Start(100)
# Ivy
self.interface = IvyMessagesInterface("GVF")
self.interface.subscribe(self.message_recv)
settings = PaparazziACSettings(ac_id)
self.ke_index = None
self.kn_index = None
self.indexes_are_good = 0
self.list_of_indexes = ['gvf_ke', 'gvf_kn']
for setting_ in self.list_of_indexes:
try:
index = settings.name_lookup[setting_].index
if setting_ == 'gvf_ke':
self.ke_index = index
if setting_ == 'gvf_kn':
self.kn_index = index
self.indexes_are_good = self.indexes_are_good + 1
except Exception as e:
print(e)
print(setting_ + " setting not found, \
have you forgotten gvf.xml in your settings?")
def __init__(self, ac_id, verbose=False):
self.ac_id = ac_id
self.verbose = verbose
self._interface = None
self.auto2_index = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.auto2_index = settings.name_lookup['auto2'].index
except Exception as e:
print(e)
print("auto2 setting not found, mode change not possible.")
self._interface = IvyMessagesInterface(self.message_recv)
def __init__(self, ac_id):
self.ac_id = ac_id
self._interface = None
self.ap_mode = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.ap_mode = settings.name_lookup['mode'] # try classic name
except Exception as e:
try:
self.ap_mode = settings.name_lookup[
'ap'] # in case it is a generated autopilot
except Exception as e:
print(e)
print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("sim_rc_4ch")
def __init__(self, ac_ids):
PaparazziACSettings.__init__(self, ac_ids[0])
self.update_callback = None
self.InitIvy()
self.ac_ids = ac_ids
def __init__(self, ac_ids):
PaparazziACSettings.__init__(self, ac_ids[0])
self.update_callback = None
self.InitIvy()
self.ac_ids = ac_ids
#!/usr/bin/env python
from __future__ import print_function
import sys
from os import path, getenv
# if PAPARAZZI_SRC not set, then assume the tree containing this
# file is a reasonable substitute
PPRZ_SRC = getenv(
"PAPARAZZI_SRC",
path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../')))
sys.path.append(PPRZ_SRC + "/sw/lib/python")
from settings_xml_parse import PaparazziACSettings
if __name__ == '__main__':
ac_id = int(sys.argv[1])
command_name = sys.argv[2]
settings = PaparazziACSettings(ac_id)
print(settings.name_lookup[command_name].index)
def __init__(self, verbose=False, interface=None, quad_ids=None):
self.verbose = verbose
self._interface = interface
self.auto2_index = None
self.ac_id = quad_ids[0]
self.ids = quad_ids
self.ap_mode = None
self.rotorcrafts = [Rotorcraft(i) for i in quad_ids]
try:
settings = PaparazziACSettings(
self.ac_id) # target and follower should be checked, FIX ME !
except Exception as e:
print(e)
return
try:
self.ap_mode = settings.name_lookup['mode'] # try classic name
except Exception as e:
try:
self.ap_mode = settings.name_lookup[
'ap'] # in case it is a generated autopilot
except Exception as e:
print(e)
print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface(
"Deep Guidance is on the way...")
# bind to INS message
def ins_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "INS":
rc = self.rotorcrafts[self.ids.index(ac_id)]
i2p = 1. / 2**8 # integer to position
i2v = 1. / 2**19 # integer to velocity
rc.X[0] = float(msg['ins_x']) * i2p
rc.X[1] = float(msg['ins_y']) * i2p
rc.X[2] = float(msg['ins_z']) * i2p
rc.V[0] = float(msg['ins_xd']) * i2v
rc.V[1] = float(msg['ins_yd']) * i2v
rc.V[2] = float(msg['ins_zd']) * i2v
rc.timeout = 0
rc.initialized = True
# self._interface.subscribe(ins_cb, PprzMessage("telemetry", "INS"))
#################################################################
def rotorcraft_fp_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "ROTORCRAFT_FP":
rc = self.rotorcrafts[self.ids.index(ac_id)]
i2p = 1. / 2**8 # integer to position
i2v = 1. / 2**19 # integer to velocity
i2w = 1. / 2**12 # integer to angle
rc.X[0] = float(msg['north']) * i2p
rc.X[1] = float(msg['east']) * i2p
rc.X[2] = float(msg['up']) * i2p
rc.V[0] = float(msg['vnorth']) * i2v
rc.V[1] = float(msg['veast']) * i2v
rc.V[2] = float(msg['vup']) * i2v
rc.W[2] = float(msg['psi']) * i2w
rc.timeout = 0
rc.initialized = True
# Un-comment this if the quadrotors are providing state information to use_deep_guidance.py
self._interface.subscribe(rotorcraft_fp_cb,
PprzMessage("telemetry", "ROTORCRAFT_FP"))
# bind to GROUND_REF message : ENAC Voliere is sending LTP_ENU
def ground_ref_cb(ground_id, msg):
ac_id = int(msg['ac_id'])
if ac_id in self.ids:
rc = self.rotorcrafts[self.ids.index(ac_id)]
# X and V in NED
rc.X[0] = float(msg['pos'][1])
rc.X[1] = float(msg['pos'][0])
rc.X[2] = float(msg['pos'][2])
rc.V[0] = float(msg['speed'][1])
rc.V[1] = float(msg['speed'][0])
rc.V[2] = float(msg['speed'][2])
rc.timeout = 0
rc.initialized = True
