def __init__(self): wx.Frame.__init__(self, id=-1, parent=None, name=u'RCWatchFrame', size=wx.Size(WIDTH, HEIGHT), title=u'RC Status') self.Bind(wx.EVT_CLOSE, self.OnClose) self.rc_statusText = wx.StaticText(self, -1, "UNKWN") pygame.mixer.init() self.alertSound = pygame.mixer.Sound("crossing.wav") self.alertChannel = pygame.mixer.Channel(False) self.setFont(self.rc_statusText) self.notification = pynotify.Notification("RC Link Warning!", "RC Link status not OK!", "dialog-warning") self.rc_status = -1 pynotify.init("RC Status") sizer = wx.BoxSizer(wx.VERTICAL) sizer.Add(self.rc_statusText, 1, wx.EXPAND) self.SetSizer(sizer) sizer.Layout() self.interface = IvyMessagesInterface("radiowatchframe") self.interface.subscribe(self.message_recv) self.update_timer = wx.CallLater(UPDATE_INTERVAL, self.gui_update)
def main(): # parse arguments import argparse as ap argp = ap.ArgumentParser(description="Environment variables provider " "for Paparazzi simulation from MesoNH data") argp.add_argument("-t", "--time-step", required=True, type=int, help="Duration of a time step between MesoNH Files.") argp.add_argument("-f", "--files", required=True, nargs='+', help="MesoNH netCDF files, in temporal ordering") argp.add_argument("-x", "--origin-x", required=False, type=float, default=0., help="Origin translation x.") argp.add_argument("-y", "--origin-y", required=False, type=float, default=0., help="Origin translation y.") argp.add_argument("-z", "--origin-z", required=False, type=float, default=0., help="Origin translation z.") arguments = argp.parse_args() print(arguments) # register signal handler for ctrl+c to stop the program signal.signal(signal.SIGINT, signal_handler) # origin for translation from paparazzi to mesoNH frame global origin origin = np.array( [0, arguments.origin_z, arguments.origin_x, arguments.origin_y]) # build atmosphere simulation source global atm atm = MesoNHAtmosphere(arguments.files, arguments.time_step, tinit=0) # init ivy and register callback for WORLD_ENV_REQ and NPS_SPEED_POS global ivy ivy = IvyMessagesInterface("MesoNH") ivy.subscribe(worldenv_cb, '(.* WORLD_ENV_REQ .*)') # wait for ivy to stop from ivy.std_api import IvyMainLoop signal.pause()
def __init__(self, parent, frame): self.parent = parent # we are drawing on our parent, so dc comes from this self.frame = frame # the frame owns any controls we might need to update parent.SetDropTarget(TextDropTarget( self)) # calls self.OnDropText when drag and drop complete self.width = 800 self.height = 200 self.margin = min(self.height / 10, 20) self.font = wx.Font(self.margin / 2, wx.DEFAULT, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_NORMAL) self.pixmap = wx.EmptyBitmap(self.width, self.height) self.plot_size = self.width self.max = -1e32 self.min = 1e32 self.plot_interval = 200 self.plots = {} self.auto_scale = True self.offset = 0.0 self.scale = 1.0 self.x_axis = None messages_xml_map.parse_messages() self.ivy_interface = IvyMessagesInterface(_IVY_APPNAME) # start the timer self.timer = wx.FutureCall(self.plot_interval, self.OnTimer)
def __init__(self, _settings): # Command line arguments self.settings = _settings # Statistics self.data = {'packets': 0, 'bytes': 0} self.w = WIDTH self.h = WIDTH # Frame wx.Frame.__init__(self, id=-1, parent=None, name=u'Distance Counter', size=wx.Size(self.w, self.h), title=u'Distance Counter') self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_CLOSE, self.OnClose) # IVY self.interface = IvyMessagesInterface("DistanceCounter") self.interface.subscribe(self.message_recv) self.msg_count = 0 self.distance = 0 self.ins_msg_x = 0 self.ins_msg_y = 0 self.ins_msg_z = 0 self.init = 0
def __init__(self): self.w = WIDTH self.h = WIDTH + BARH self.cfg = wx.Config('svinfo_conf') if self.cfg.Exists('width'): self.w = int(self.cfg.Read('width')) self.h = int(self.cfg.Read('height')) wx.Frame.__init__(self, id=-1, parent=None, name=u'SVInfoFrame', size=wx.Size(self.w, self.h), title=u'SV Info') if self.cfg.Exists('left'): self.x = int(self.cfg.Read('left')) self.y = int(self.cfg.Read('top')) self.SetPosition(wx.Point(self.x,self.y)) self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_MOVE, self.OnMove) self.Bind(wx.EVT_CLOSE, self.OnClose) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/svinfo/svinfo.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.sv = {} self.interface = IvyMessagesInterface("svinfoframe") self.interface.subscribe(self.message_recv)
def __init__(self): self.w = 900 self.h = 700 self.airspeed = 0 self.amsl = 0 self.qnh = 0 self.qfe = 0 self.alt = 0 #self.hmsl = 0; self.gspeed = 0 wx.Frame.__init__(self, id=-1, parent=None, name=u'ATC Center', size=wx.Size(self.w, self.h), title=u'ATC Center') self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_CLOSE, self.OnClose) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/atc/atc.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.interface = IvyMessagesInterface("ATC-Center") self.interface.subscribe(self.message_recv)
def __init__(self, ivyBusAddress=None): if ivyBusAddress is None: ivyBusAddress = os.getenv('IVY_BUS') if ivyBusAddress is None: ivyBusAddress == "" self.messageInterface = IvyMessagesInterface(ivy_bus=ivyBusAddress)
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 __init__(self): self.w = WIDTH self.h = WIDTH + BARH wx.Frame.__init__(self, id=-1, parent=None, name=u'EnergyMonFrame', size=wx.Size(self.w, self.h), title=u'Energy Monitoring') self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_CLOSE, self.OnClose) ico = wx.Icon( PPRZ_SRC + "/sw/ground_segment/python/energy_mon/energy_mon.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.bat = {} self.temp = {} self.cell = BatteryCell() self.interface = IvyMessagesInterface("energymonframe") self.interface.subscribe(self.message_recv)
class Ivy2RedisServer(): def __init__(self, redishost, redisport, verbose=False): self.verbose = verbose self.interface = IvyMessagesInterface("Ivy2Redis") self.interface.subscribe(self.message_recv) self.r = redis.StrictRedis(host=redishost, port=redisport, db=0) self.keep_running = True print("Connected to redis server %s on port %i" % (redishost, redisport)) def message_recv(self, ac_id, msg): # if ac_id is not 0 (i.e. telemetry from an aircraft) include it in the key # don't add it to the key for ground messages if ac_id: key = "{0}.{1}.{2}".format(msg.msg_class, msg.name, ac_id) else: key = "{0}.{1}".format(msg.msg_class, msg.name) if self.verbose: print("received message, key=%s, msg=%s" % (key, msg.to_json(payload_only=True))) sys.stdout.flush() self.r.publish(key, msg.to_json(payload_only=True)) self.r.set(key, msg.to_json(payload_only=True)) def run(self): while self.keep_running: time.sleep(0.1) def stop(self): self.keep_running = False self.interface.shutdown()
def __init__(self): self.w = WIDTH self.h = WIDTH + BARH self.cfg = wx.Config('energymon_conf') if self.cfg.Exists('width'): self.w = int(self.cfg.Read('width')) self.h = int(self.cfg.Read('height')) wx.Frame.__init__(self, id=-1, parent=None, name=u'EnergyMonFrame', size=wx.Size(self.w, self.h), title=u'Energy Monitoring') if self.cfg.Exists('left'): self.x = int(self.cfg.Read('left')) self.y = int(self.cfg.Read('top')) self.SetPosition(wx.Point(self.x,self.y), wx.SIZE_USE_EXISTING) self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_MOVE, self.OnMove) self.Bind(wx.EVT_CLOSE, self.OnClose) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/energy_mon/energy_mon.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.bat = {} self.temp = {} self.cell = BatteryCell() self.energy_prediction = EnergyPrediction(self.cell) self.interface = IvyMessagesInterface("energymonframe") self.interface.subscribe(self.message_recv)
def __init__(self, _settings): # Command line arguments self.settings = _settings # Statistics self.data = { 'packets': 0, 'bytes': 0} # Decoder if (self.settings.decoder == 'jpeg100'): self.decoder = jpeg100_decoder.ThumbNailFromPayload() else: self.decoder = MinimalDecoder() self.w = WIDTH self.h = WIDTH # Socket self.sock = socket.socket(socket.AF_INET, # Internet socket.SOCK_DGRAM) # UDP # Frame wx.Frame.__init__(self, id=-1, parent=None, name=u'Payload Forwarding', size=wx.Size(self.w, self.h), title=u'Payload Forwarding') ico = wx.Icon(os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)))) + "/payload.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_CLOSE, self.OnClose) # IVY self.interface = IvyMessagesInterface("PayloadForwarder") self.interface.subscribe(self.message_recv)
class Cx10ds2ivy: def __init__(self, verbose=False): self.verbose = verbose self.step = 0.1 # period in seconds self.button_trim = False self._cx10 = CX10DS(verbose) # Start IVY interface self._interface = IvyMessagesInterface("Cx10ds2ivy") # bind to JOYSTICK message def joystick_cb(ac_id, msg): aileron = int(msg['axis1']) elevator = int(msg['axis2']) rudder = int(msg['axis3']) if int(msg['button1']) == 255: throttle = int(msg['axis4']) # direct throttle reading else: throttle = 128 direction = int(msg['button1'])-1 throttle_incr = self._cx10.valid_range(int(msg['axis4']), 0, 127) throttle = 128 + direction * throttle_incr # up mode = int(msg['button2']) if msg['button4'] == '1' and not self.button_trim: self._cx10.set_trim() self.button_trim = (msg['button4'] == '1') self._cx10.set_cmd(aileron,elevator,rudder,throttle,mode) if self.verbose: print("Throttle {0}".format(throttle)) print("Rudder {0}".format(rudder)) print("elevator {0}".format(elevator)) print("aileron {0}".format(aileron)) print("Mode {0}".format(mode)) self._interface.subscribe(joystick_cb, PprzMessage("ground", "JOYSTICK")) def __del__(self): self.stop() def stop(self): # Stop IVY interface if self._interface is not None: self._interface.shutdown() # main loop def run(self): try: while True: # TODO: make better frequency managing self._cx10.send() sleep(self.step) except KeyboardInterrupt: if self.verbose: print("Exiting..") self.stop()
def __init__(self, redishost, redisport, verbose=False): self.verbose = verbose self.interface = IvyMessagesInterface("Ivy2Redis") self.interface.subscribe(self.message_recv) self.r = redis.StrictRedis(host=redishost, port=redisport, db=0) self.keep_running = True print("Connected to redis server %s on port %i" % (redishost, redisport))
def __init__(self, plot_results=True, verbose=False): self._interface = IvyMessagesInterface("calib_mag") self.plotter = MagPlot() self.data = [] self.flt_meas = [] self.p0 = np.array([0, 0, 0, 0, 0, 0]) self.optimization_done = False self.plot_results = plot_results
def __init__(self, ac_id, plateform_id, tag_id): # Start a ivy messages listener named PIR DRONE self._interface = IvyMessagesInterface("PIR", ivy_bus="192.168.1:2010") self.drone = FC_Rotorcraft(ac_id, tag_id, self._interface) self.plateform = Platform(plateform_id) # bind to GROUND_REF message def ground_ref_cb(ground_id, msg): ac_id = int(msg['ac_id']) if ac_id == self.drone.id: # X and V in NED self.drone.X_opti[0] = float(msg['pos'][1]) self.drone.X_opti[1] = float(msg['pos'][0]) self.drone.X_opti[2] = -float(msg['pos'][2]) self.drone.V_opti[0] = float(msg['speed'][1]) self.drone.V_opti[1] = float(msg['speed'][0]) self.drone.V_opti[2] = -float(msg['speed'][2]) self.drone.quaternions[0] = float(msg['quat'][0]) self.drone.quaternions[1] = float(msg['quat'][1]) self.drone.quaternions[2] = float(msg['quat'][2]) self.drone.quaternions[3] = float(msg['quat'][3]) self.drone.timeLastUpdate = time.time() self.drone.initialized = True if ac_id == self.plateform.id: # X and V in NED self.plateform.X[0] = float(msg['pos'][1]) self.plateform.X[1] = float(msg['pos'][0]) self.plateform.X[2] = -float(msg['pos'][2]) self.plateform.V[0] = float(msg['speed'][1]) self.plateform.V[1] = float(msg['speed'][0]) self.plateform.V[2] = -float(msg['speed'][2]) self.plateform.quaternions[0] = float(msg['quat'][0]) self.plateform.quaternions[1] = float(msg['quat'][1]) self.plateform.quaternions[2] = float(msg['quat'][2]) self.plateform.quaternions[3] = float(msg['quat'][3]) self.plateform.timeLastUpdate = time.time() self.plateform.initialized = True self._interface.subscribe(ground_ref_cb, PprzMessage("ground", "GROUND_REF")) # Recuperation du x et y via l'UWB def get_xy_uwb(uwb_id, msg): # X and V in NED #msg est un tableau contenant : d1corr, d2corr, d3corr, d1vrai, d2vrai, d3vrai, x, y, z, vx, vy, vz x = float(msg['values'][6]) y = float(msg['values'][7]) z = self.drone.X_opti[3] psi = cr.getEulerAngles( self.drone.quaternions)[2] - cr.getEulerAngles( self.plateform.quaternions)[2] #self.drone.V_uwb[0] = float(msg['values'][9]) #self.drone.V_uwb[1] = float(msg['values'][10]) self.drone.X_uwb = cr.uwb2pseudoBody(x, y, z) self._interface.subscribe(get_xy_uwb, PprzMessage("telemetry", "PAYLOAD_FLOAT"))
class RadioWatchFrame(wx.Frame): def message_recv(self, ac_id, msg): if msg.name == "ROTORCRAFT_STATUS": self.rc_status = int(msg['rc_status']) if self.rc_status != 0 and not self.alertChannel.get_busy(): self.warn_timer = wx.CallLater(5, self.rclink_alert) # else: # self.notification.close() def gui_update(self): self.rc_statusText.SetLabel(["OK", "LOST", "REALLY LOST"][self.rc_status]) self.update_timer.Restart(UPDATE_INTERVAL) def rclink_alert(self): self.alertChannel.queue(self.alertSound) self.notification.show() time.sleep(5) def setFont(self, control): font = control.GetFont() size = font.GetPointSize() font.SetPointSize(size * 1.4) control.SetFont(font) def __init__(self): wx.Frame.__init__(self, id=-1, parent=None, name=u'RCWatchFrame', size=wx.Size(WIDTH, HEIGHT), title=u'RC Status') self.Bind(wx.EVT_CLOSE, self.OnClose) self.rc_statusText = wx.StaticText(self, -1, "UNKWN") pygame.mixer.init() self.alertSound = pygame.mixer.Sound("crossing.wav") self.alertChannel = pygame.mixer.Channel(False) self.setFont(self.rc_statusText) self.notification = pynotify.Notification("RC Link Warning!", "RC Link status not OK!", "dialog-warning") self.rc_status = -1 pynotify.init("RC Status") sizer = wx.BoxSizer(wx.VERTICAL) sizer.Add(self.rc_statusText, 1, wx.EXPAND) self.SetSizer(sizer) sizer.Layout() self.interface = IvyMessagesInterface("radiowatchframe") self.interface.subscribe(self.message_recv) self.update_timer = wx.CallLater(UPDATE_INTERVAL, self.gui_update) def OnClose(self, event): self.interface.shutdown() self.Destroy()
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 __init__(self, config, verbose=False): self.verbose = verbose self.config = config self.ids = np.array(self.config['ids']) self.B = np.array(self.config['topology']) self.Zdesired = np.array(self.config['desired_intervehicle_angles']) self.list_ids = np.ndarray.tolist(self.ids) # Start IVY interface self._interface = IvyMessagesInterface("Init DCF tables")
def __init__(self, verbose=False): from pprzlink.ivy import IvyMessagesInterface # init Ivy interface self.pprzivy = IvyMessagesInterface("pyueye") # init cam related part uEyePprzlink.__init__(self, verbose) # bind to message self.pprzivy.subscribe(self.process_msg, PprzMessage("telemetry", "DC_SHOT"))
def __init__(self, src): # Create the video capture device self.cap = cv2.VideoCapture(src) # Start the ivy interface self.ivy = IvyMessagesInterface("RTPviewer", start_ivy=False) self.ivy.start() # Create a named window and add a mouse callback cv2.namedWindow('rtp') cv2.setMouseCallback('rtp', self.on_mouse)
def example2(): # Usage: ./setting.py <path_var_AC>/settings.xml <ac_id> ivy = IvyMessagesInterface("DemoSettings") try: setting_manager = PprzSettingsManager(sys.argv[1], sys.argv[2], ivy) while True: time.sleep(1) setting_manager["altitude"] = setting_manager["altitude"].value + 2 except KeyboardInterrupt: ivy.shutdown() print("Stopping on request")
def __init__(self, agent_name=None, start_ivy=True, verbose=False, ivy_bus=IVY_BUS): self.interface = IvyMessagesInterface(agent_name, start_ivy, verbose, ivy_bus) RosMessagesInterface.__init__(self) # only subscribe to telemetry messages eg. starting with AC_ID self.interface.subscribe(callback=self.to_ros, regex_or_msg='(^[0-9]+ .*)')
def __init__(self): # own data self.count_gs = 0 self.ground_gs_x = [0] * MSG_BUFFER_SIZE self.ground_gs_y = [0] * MSG_BUFFER_SIZE self.count_as = 0 self.last_heading = 0 self.airspeed = [0] * MSG_BUFFER_SIZE self.heading = [0] * MSG_BUFFER_SIZE # Click self.click_x = 0 self.click_y = 0 self.click_on = 0 # Window self.w = WIDTH self.h = WIDTH + BARH self.cfg = wx.Config('wind_conf') if self.cfg.Exists('width'): self.w = int(self.cfg.Read('width')) self.h = int(self.cfg.Read('height')) self.mid = self.w / 2 wx.Frame.__init__(self, id=-1, parent=None, name=u'WindFrame', size=wx.Size(self.w, self.h), title=u'Wind Tool') if self.cfg.Exists('left'): self.x = int(self.cfg.Read('left')) self.y = int(self.cfg.Read('top')) self.SetPosition(wx.Point(self.x, self.y), wx.SIZE_USE_EXISTING) self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_MOVE, self.OnMove) self.Bind(wx.EVT_CLOSE, self.OnClose) self.Bind(wx.EVT_LEFT_DOWN, self.OnClickD) self.Bind(wx.EVT_MOTION, self.OnClickM) self.Bind(wx.EVT_LEFT_UP, self.OnClickU) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/wind/wind.png", wx.BITMAP_TYPE_PNG) self.SetIcon(ico) self.interface = IvyMessagesInterface("windframe") self.interface.subscribe(self.message_recv)
class PprzlinkDatastream(AbstractDatastream): def initialize(self): name = 'pprzlink' self._interface = IvyMessagesInterface("pprzlink_morse") @classproperty def _type_url(cls): return "http://docs.paparazziuav.org/latest/paparazzi_messages.html#" + cls._type_name def finalize(self): if self._interface is not None: self._interface.shutdown() self._interface = None
def __init__(self, ac_id, ground_alt=0): """Init PaparazziUAV Arguments: ac_id: aircraft id. ground_alt: ground altitude above sea level. """ self.ac_id = ac_id self.ground_alt = ground_alt self.east = .0 self.north = .0 self.up = .0 self.phi = .0 # in rad self.psi = .0 # in rad self.theta = .0 # in rad self.voltage = 0. # in V self.current = 0. # in A self.power = 0. # in W self.energy = 0. # in mAh self.measured_wind_x = 0. self.measured_wind_y = 0. self.measured_wind_z = 0. self.path_queue = Queue.Queue() # format: (time, (x, y, z)) self._last_log = 0 self._log_interval = 1 # initiate ivy message interface to catch all messages self.ivy_mes = IvyMessagesInterface() self.ivy_mes.subscribe(self.ivy_callback, PaparazziUAV.BIND_REGEX) rospy.init_node('paparazziuav') self.ros_pub_state = rospy.Publisher('uav_state', UAVState, queue_size=1) self.ros_pub_measured_wind = rospy.Publisher('measured_wind', WindSample, queue_size=1) self.ros_pub_energy_consumption = rospy.Publisher('energy_consumption', EnergyConsumption, queue_size=1) rospy.Subscriber("/tick", Clock, self.on_tick_recv, queue_size=10) rospy.Subscriber('expected_uav_state_sequence', UAVStateSequence, self.on_exp_uav_state_seq_recv)
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, _settings): # Command line arguments self.settings = _settings # Statistics self.data = { 'packets': 0, 'bytes': 0} # Decoder if (self.settings.decoder is 'jpeg100'): self.decoder = jpeg100_decoder.ThumbNailFromPayload() else: self.decoder = MinimalDecoder() self.w = WIDTH self.h = WIDTH # Socket self.sock = socket.socket(socket.AF_INET, # Internet socket.SOCK_DGRAM) # UDP # Frame wx.Frame.__init__(self, id=-1, parent=None, name=u'Payload Forwarding', size=wx.Size(self.w, self.h), title=u'Payload Forwarding') ico = wx.Icon(os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)))) + "/payload.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_CLOSE, self.OnClose) # IVY self.interface = IvyMessagesInterface("PayloadForwarder") self.interface.subscribe(self.message_recv)
def __init__(self, _settings): # Command line arguments self.settings = _settings # Statistics self.data = { 'packets': 0, 'bytes': 0} self.w = WIDTH self.h = WIDTH # Frame wx.Frame.__init__(self, id=-1, parent=None, name=u'Distance Counter', size=wx.Size(self.w, self.h), title=u'Distance Counter') self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_CLOSE, self.OnClose) # IVY self.interface = IvyMessagesInterface("DistanceCounter") self.interface.subscribe(self.message_recv) self.msg_count = 0 self.distance = 0 self.ins_msg_x = 0 self.ins_msg_y = 0 self.ins_msg_z = 0 self.init = 0
def __init__(self): self.w = WIDTH self.h = WIDTH + BARH self.cfg = wx.Config('svinfo_conf') if self.cfg.Exists('width'): self.w = int(self.cfg.Read('width')) self.h = int(self.cfg.Read('height')) wx.Frame.__init__(self, id=-1, parent=None, name=u'SVInfoFrame', size=wx.Size(self.w, self.h), title=u'SV Info') if self.cfg.Exists('left'): self.x = int(self.cfg.Read('left')) self.y = int(self.cfg.Read('top')) self.SetPosition(wx.Point(self.x,self.y), wx.SIZE_USE_EXISTING) self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_MOVE, self.OnMove) self.Bind(wx.EVT_CLOSE, self.OnClose) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/svinfo/svinfo.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.sv = {} self.interface = IvyMessagesInterface("svinfoframe") self.interface.subscribe(self.message_recv)
def __init__(self, parent, frame): self.parent = parent # we are drawing on our parent, so dc comes from this self.frame = frame # the frame owns any controls we might need to update parent.SetDropTarget(TextDropTarget(self)) # calls self.OnDropText when drag and drop complete self.width = 800 self.height = 200 self.margin = min(self.height / 10, 20) self.font = wx.Font(self.margin / 2, wx.DEFAULT, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_NORMAL) self.pixmap = wx.EmptyBitmap(self.width, self.height) self.plot_size = self.width self.max = -1e32 self.min = 1e32 self.plot_interval = 200 self.plots = {} self.auto_scale = True self.offset = 0.0 self.scale = 1.0 self.x_axis = None messages_xml_map.parse_messages() self.ivy_interface = IvyMessagesInterface(_IVY_APPNAME) # start the timer self.timer = wx.FutureCall(self.plot_interval, self.OnTimer)
def __init__(self, parent, callback, ivy_interface=None): wx.Frame.__init__(self, parent, name="MessagePicker", title=u'Message Picker', size=wx.Size(320,640)) self.aircrafts = {} self.callback = callback self.tree = wx.TreeCtrl(self) self.root = self.tree.AddRoot("Telemetry") self.tree.Bind(wx.EVT_LEFT_DCLICK, self.OnDoubleClick) self.tree.Bind(wx.EVT_CHAR, self.OnKeyChar) self.Bind(wx.EVT_CLOSE, self.OnClose) if ivy_interface is None: self.message_interface = IvyMessagesInterface("MessagePicker") else: self.message_interface = ivy_interface self.message_interface.subscribe(self.msg_recv)
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("guided mode example")
def __init__(self): self.w = 900 self.h = 700 self.airspeed = 0; self.amsl = 0; self.qnh = 0; self.qfe = 0; self.alt = 0; #self.hmsl = 0; self.gspeed = 0; wx.Frame.__init__(self, id=-1, parent=None, name=u'ATC Center', size=wx.Size(self.w, self.h), title=u'ATC Center') self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_CLOSE, self.OnClose) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/atc/atc.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.interface = IvyMessagesInterface("ATC-Center") self.interface.subscribe(self.message_recv)
def __init__(self): self.w = WIDTH self.h = HEIGHT self.airspeed = 0 self.amsl = 0 self.qnh = 0 self.qfe = 0 self.alt = 0 #self.hmsl = 0; self.gspeed = 0 self.callsign = "" self.safe_to_approach = "" self.cfg = wx.Config('atc_conf') if self.cfg.Exists('width'): self.w = int(self.cfg.Read('width')) self.h = int(self.cfg.Read('height')) wx.Frame.__init__(self, id=-1, parent=None, name=u'ATC Center', size=wx.Size(self.w, self.h), title=u'ATC Center') self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_MOVE, self.OnMove) self.Bind(wx.EVT_CLOSE, self.OnClose) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/atc/atc.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.interface = IvyMessagesInterface("ATC-Center") self.interface.subscribe(self.message_recv) if self.cfg.Exists('left'): self.x = int(self.cfg.Read('left')) self.y = int(self.cfg.Read('top')) self.SetPosition(wx.Point(self.x, self.y), wx.SIZE_USE_EXISTING)
def __init__(self, ac_id, freq=100.): self.id = ac_id self.pos = np.array([[0.0], [0.0], [0.0]]) self.attitude = np.array([[0.0], [0.0], [0.0]]) self.vel = np.array([[0.0], [0.0], [0.0]]) self.step = 1. / freq self._interface = IvyMessagesInterface("Controller 1") self.target_pos = np.array([[5.0], [7.0], [10.0]]) self.target_vel = np.array([[0.0], [0.0], [0.0]]) self.error_pos = np.array([[0.0], [0.0], [0.0]]) self.error_vel = np.array([[0.0], [0.0], [0.0]]) self.U = np.array([[0.0], [0.0]]) # Accelerations # Circle self.e = 0.0 self.grad = np.array([[0.0], [0.0]]) self.hess = np.array([[2.0, 0.0], [0.0, 2.0]]) self.rot = np.array([[0.0, -1.0], [1.0, 0.0]]) self.vel_d = np.array([[0.0], [0.0]]) self.s = 1. self.c1 = 0.005 self.c2 = 7. self.c3 = 1.25 def rotorcraft_fp_callback(_id, msg): if msg.name == "ROTORCRAFT_FP": field_coefs = msg.fieldcoefs self.pos[0][0] = float(msg.get_field(0)) * field_coefs[0] self.pos[1][0] = float(msg.get_field(1)) * field_coefs[1] self.pos[2][0] = float(msg.get_field(2)) * field_coefs[2] self.vel[0][0] = float(msg.get_field(3)) * field_coefs[3] self.vel[1][0] = float(msg.get_field(4)) * field_coefs[4] self.vel[2][0] = float(msg.get_field(5)) * field_coefs[5] self.attitude[0][0] = float(msg.get_field(6)) * field_coefs[6] self.attitude[1][0] = float(msg.get_field(7)) * field_coefs[7] self.attitude[2][0] = float(msg.get_field(8)) * field_coefs[8] self._interface.subscribe(rotorcraft_fp_callback, PprzMessage("telemetry", "ROTORCRAFT_FP"))
def main(): # parse arguments import argparse as ap argp = ap.ArgumentParser(description="Environment variables provider " "for Paparazzi simulation from MesoNH data") argp.add_argument("-t", "--time-step", required=True, type=int, help="Duration of a time step between MesoNH Files.") argp.add_argument("-f", "--files", required=True, nargs='+', help="MesoNH netCDF files, in temporal ordering") argp.add_argument("-x", "--origin-x", required=False, type=float, default=0., help="Origin translation x.") argp.add_argument("-y", "--origin-y", required=False, type=float, default=0., help="Origin translation y.") argp.add_argument("-z", "--origin-z", required=False, type=float, default=0., help="Origin translation z.") arguments = argp.parse_args() print(arguments) # register signal handler for ctrl+c to stop the program signal.signal(signal.SIGINT, signal_handler) # origin for translation from paparazzi to mesoNH frame global origin origin = np.array([0, arguments.origin_z, arguments.origin_x, arguments.origin_y]) # build atmosphere simulation source global atm atm = MesoNHAtmosphere(arguments.files, arguments.time_step, tinit=0) # init ivy and register callback for WORLD_ENV_REQ and NPS_SPEED_POS global ivy ivy = IvyMessagesInterface("MesoNH"); ivy.subscribe(worldenv_cb,'(.* WORLD_ENV_REQ .*)') # wait for ivy to stop from ivy.std_api import IvyMainLoop signal.pause()
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"))
class WaypointMover(object): def __init__(self, verbose=False): self.verbose = verbose self._interface = IvyMessagesInterface("WaypointMover") def shutdown(self): print("Shutting down ivy interface...") self._interface.shutdown() def __del__(self): self.shutdown() def move_waypoint(self, ac_id, wp_id, lat, lon, alt): msg = PprzMessage("ground", "MOVE_WAYPOINT") msg['ac_id'] = ac_id msg['wp_id'] = wp_id msg['lat'] = lat msg['long'] = lon msg['alt'] = alt print("Sending message: %s" % msg) self._interface.send(msg)
class IvyRequester(object): def __init__(self, interface=None): self._interface = interface if interface is None: self._interface = IvyMessagesInterface("ivy requester") self.ac_list = [] def __del__(self): self.shutdown() def shutdown(self): if self._interface is not None: print("Shutting down ivy interface...") self._interface.shutdown() self._interface = None def get_aircrafts(self): def aircrafts_cb(ac_id, msg): self.ac_list = [int(a) for a in msg['ac_list'].split(',') if a] print("aircrafts: {}".format(self.ac_list)) self._interface.subscribe(aircrafts_cb, "(.*AIRCRAFTS .*)") sender = 'get_aircrafts' request_id = '42_1' # fake request id, should be PID_index self._interface.send("{} {} AIRCRAFTS_REQ".format(sender, request_id)) # hack: sleep briefly to wait for answer sleep(0.1) return self.ac_list
def __init__(self, msg_class="telemetry"): wx.Frame.__init__(self, id=-1, parent=None, name=u'MessagesFrame', size=wx.Size(WIDTH, HEIGHT), style=wx.DEFAULT_FRAME_STYLE, title=u'Messages') self.Bind(wx.EVT_CLOSE, self.OnClose) self.notebook = wx.Notebook(self) self.aircrafts = {} sizer = wx.BoxSizer(wx.HORIZONTAL) sizer.Add(self.notebook, 1, wx.EXPAND) self.SetSizer(sizer) sizer.Layout() self.timer = threading.Timer(0.1, self.update_leds) self.timer.start() self.msg_class = msg_class self.interface = IvyMessagesInterface("Paparazzi Messages Viewer") self.interface.subscribe(self.message_recv)
def __init__(self): # own data self.count_gs = 0 self.ground_gs_x = [0] * MSG_BUFFER_SIZE self.ground_gs_y = [0] * MSG_BUFFER_SIZE self.count_as = 0 self.last_heading = 0; self.airspeed = [0] * MSG_BUFFER_SIZE self.heading = [0] * MSG_BUFFER_SIZE # Click self.click_x = 0 self.click_y = 0 self.click_on = 0 # Window self.w = WIDTH self.h = WIDTH + BARH self.cfg = wx.Config('wind_conf') if self.cfg.Exists('width'): self.w = int(self.cfg.Read('width')) self.h = int(self.cfg.Read('height')) self.mid = self.w/2 wx.Frame.__init__(self, id=-1, parent=None, name=u'WindFrame', size=wx.Size(self.w, self.h), title=u'Wind Tool') if self.cfg.Exists('left'): self.x = int(self.cfg.Read('left')) self.y = int(self.cfg.Read('top')) self.SetPosition(wx.Point(self.x,self.y), wx.SIZE_USE_EXISTING) self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_MOVE, self.OnMove) self.Bind(wx.EVT_CLOSE, self.OnClose) self.Bind(wx.EVT_LEFT_DOWN, self.OnClickD) self.Bind(wx.EVT_MOTION, self.OnClickM) self.Bind(wx.EVT_LEFT_UP, self.OnClickU) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/wind/wind.png", wx.BITMAP_TYPE_PNG) self.SetIcon(ico) self.interface = IvyMessagesInterface("windframe") self.interface.subscribe(self.message_recv)
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("guided mode example")
def __init__(self, ac_id, verbose=False): self.ac_id = ac_id self.verbose = verbose 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'].index except Exception as e: print(e) print("ap_mode setting not found, mode change not possible.") self._interface = IvyMessagesInterface("gb2ivy")
def __init__(self, period=10., margin=1., timeout=60., verbose=False): self.period = period self.margin = margin self.timeout = timeout self.last_receive = time() self.verbose = verbose self._interface = IvyMessagesInterface("OpenSkyTraffic") self._opensky = OpenSkyApi() self.ac_list = {} self.intruder_list = {} self.running = False if self.verbose: print("Starting opensky interface...") # subscribe to FLIGHT_PARAM ground message self._interface.subscribe(self.update_ac, PprzMessage("ground", "FLIGHT_PARAM"))
def __init__(self): self.w = WIDTH self.h = WIDTH + BARH wx.Frame.__init__(self, id=-1, parent=None, name=u'SVInfoFrame', size=wx.Size(self.w, self.h), title=u'SV Info') self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_CLOSE, self.OnClose) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/svinfo/svinfo.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.sv = {} self.interface = IvyMessagesInterface("svinfoframe") self.interface.subscribe(self.message_recv)
def __init__(self): self.w = WIDTH self.h = HEIGHT self.airspeed = 0; self.amsl = 0; self.qnh = 0; self.qfe = 0; self.alt = 0; #self.hmsl = 0; self.gspeed = 0; self.callsign = "" self.safe_to_approach = ""; self.cfg = wx.Config('atc_conf') if self.cfg.Exists('width'): self.w = int(self.cfg.Read('width')) self.h = int(self.cfg.Read('height')) wx.Frame.__init__(self, id=-1, parent=None, name=u'ATC Center', size=wx.Size(self.w, self.h), title=u'ATC Center') self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_MOVE, self.OnMove) self.Bind(wx.EVT_CLOSE, self.OnClose) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/atc/atc.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.interface = IvyMessagesInterface("ATC-Center") self.interface.subscribe(self.message_recv) if self.cfg.Exists('left'): self.x = int(self.cfg.Read('left')) self.y = int(self.cfg.Read('top')) self.SetPosition(wx.Point(self.x,self.y), wx.SIZE_USE_EXISTING)
def __init__(self): self.w = WIDTH self.h = WIDTH + BARH wx.Frame.__init__(self, id=-1, parent=None, name=u'EnergyMonFrame', size=wx.Size(self.w, self.h), title=u'Energy Monitoring') self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_CLOSE, self.OnClose) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/energy_mon/energy_mon.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.bat = {} self.temp = {} self.cell = BatteryCell(); self.interface = IvyMessagesInterface("energymonframe") self.interface.subscribe(self.message_recv)