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
#!/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