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)
class ObstacleAdder(object):
def __init__(self, verbose=False):
self.verbose = verbose
self._interface = IvyMessagesInterface(self.message_recv)
def message_recv(self, ac_id, msg):
if self.verbose:
print("Got msg %s" % msg.name)
def shutdown(self):
print("Shutting down ivy interface...")
self._interface.shutdown()
def __del__(self):
self.shutdown()
def add_obstacle(self, obstacle_id, color, status, lat, lon, radius, alt):
msg = PprzMessage("ground", "OBSTACLE")
msg['id'] = obstacle_id
msg['color'] = color
msg['status'] = status
msg['lat'] = lat
msg['lon'] = lon
msg['radius'] = radius
msg['alt'] = alt
print("Sending message: %s" % msg)
self._interface.send(msg)
class WaypointMover(object):
def __init__(self, verbose=False):
self.verbose = verbose
self._interface = IvyMessagesInterface(self.message_recv)
def message_recv(self, ac_id, msg):
if self.verbose:
print("Got msg %s" % msg.name)
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)
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 __init__(self, redishost, redisport, verbose=False):
self.verbose = verbose
self.interface = IvyMessagesInterface(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))
class WaypointMover(object):
def __init__(self, verbose=False):
self.verbose = verbose
self._interface = IvyMessagesInterface(self.message_recv)
def message_recv(self, ac_id, msg):
if self.verbose:
print("Got msg %s" % msg.name)
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 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(self.message_recv)
self.update_timer = wx.CallLater(UPDATE_INTERVAL, self.gui_update)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class Ivy2RedisServer():
def __init__(self, redishost, redisport, verbose=False):
self.verbose = verbose
self.interface = IvyMessagesInterface(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, msg_class, msg_name, ac_id, values):
# 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_class, msg_name, ac_id)
else:
key = "{0}.{1}".format(msg_class, msg_name)
if self.verbose:
print("received message, key=%s, values=%s" %
(key, ' '.join(values)))
sys.stdout.flush()
self.r.publish(key, values)
self.r.set(key, values)
def run(self):
while self.keep_running:
time.sleep(0.1)
def stop(self):
self.keep_running = False
self.interface.shutdown()
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()
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, parent, callback, initIvy=True):
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)
self.message_interface = IvyMessagesInterface(self.msg_recv, initIvy)
class myQRParser():
def signal_handler(self, signal, frame):
print('You pressed Ctrl+C!')
self.interface.shutdown()
sys.exit(0)
def myCrapThread(self):
time.sleep(1)
myCode.decode_webcam(callback=self.codeRecognized, device='/dev/video0')
#print('Finished, please ctrl+c')
self.interface.shutdown()
sys.exit(0)
def __init__(self, ac_id, waypoint_id):
self.ac_id = ac_id[0]
self.waypoint_id = waypoint_id[0]
self.interface = IvyMessagesInterface(self, self.emptyFun)
t = threading.Thread(target=self.myCrapThread)
t.start()
signal.signal(signal.SIGINT, self.signal_handler)
print('Press Ctrl+C')
signal.pause()
self.interface.shutdown()
def emptyFun(ac_id, msg):
print "Never"
def codeRecognized (self, data):
print data
if data == '1':
coords = 1
print "Dropzone 1"
elif data == '2':
coords = 2
print "Dropzone 2"
elif data == '3':
coords = 3
print "Dropzone 3"
else:
print data
return
print('Going to: ', coords)
pprzmsg = PprzMessage("ground", "DL_SETTING")
pprzmsg.set_values([self.ac_id, self.waypoint_id, coords])
self.interface.send(pprzmsg)
#pprzmsg.set_values([self.ac_id, (self.waypoint_id+1), coords[0], coords[1], 10])
#self.interface.send(pprzmsg)
def __call__(a, b, c):
print ""
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(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(self.message_recv)
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 __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))
class CommandReader(object):
def __init__(self, verbose=False, callback=None):
self.verbose = verbose
self.callback = callback
self._interface = IvyMessagesInterface(self.message_recv)
def message_recv(self, ac_id, msg):
if (self.verbose and self.callback != None):
self.callback(ac_id, msg)
def shutdown(self):
print("Shutting down ivy interface...")
self._interface.shutdown()
def __del__(self):
self.shutdown()
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)
def __init__(self, parent, callback, initIvy=True):
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)
self.message_interface = IvyMessagesInterface(self.msg_recv, initIvy)
def __init__(self, ac_id, waypoint_id):
self.ac_id = ac_id[0]
self.waypoint_id = waypoint_id[0]
self.interface = IvyMessagesInterface(self, self.emptyFun)
t = threading.Thread(target=self.myCrapThread)
t.start()
signal.signal(signal.SIGINT, self.signal_handler)
print('Press Ctrl+C')
signal.pause()
self.interface.shutdown()
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(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")
class IntruderAdder(object):
def __init__(self, verbose=False):
self.verbose = verbose
self._interface = IvyMessagesInterface(self.message_recv)
def message_recv(self, ac_id, msg):
if self.verbose:
print("Got msg %s" % msg.name)
def shutdown(self):
print("Shutting down ivy interface...")
self._interface.shutdown()
def __del__(self):
self.shutdown()
def add_intruder(self, intruder_id, name, lat, lon, alt, course, speed, climb, itow):
msg = PprzMessage("ground", "INTRUDER")
msg['id'] = intruder_id
msg['name'] = name
msg['lat'] = lat
msg['lon'] = lon
msg['alt'] = alt
msg['course'] = course
msg['speed'] = speed
msg['climb'] = climb
msg['itow'] = 0
print("Sending message: %s" % msg)
self._interface.send(msg)
def new_intruder(self, intruder_id, name):
msg = PprzMessage("ground", "INTRUDER")
msg['id'] = intruder_id
msg['name'] = name
msg['itow'] = 0
print("Sending message: %s" % msg)
self._interface.send(msg)
class Guidance(object):
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 shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def __del__(self):
self.shutdown()
def set_guided_mode(self):
"""
change auto2 mode to GUIDED.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.auto2_index
msg['value'] = 19 # AP_MODE_GUIDED
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change auto2 mode to NAV.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.auto2_index
msg['value'] = 13 # AP_MODE_NAV
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def goto_ned(self, north, east, down, heading=0.0):
"""
goto a local NorthEastDown position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x00
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = heading
print("goto NED: %s" % msg)
# embed the message in RAW_DATALINK so that the server can log it
self._interface.send_raw_datalink(msg)
def goto_ned_relative(self, north, east, down, yaw=0.0):
"""
goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x01
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = yaw
print("goto NED relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def goto_body_relative(self, forward, right, down, yaw=0.0):
"""
goto to a position relative to current position and heading in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x03
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("goto body relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def move_at_vel(self, north=0.0, east=0.0, down=0.0, yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x70
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = yaw
print("move at vel NED: %s" % msg)
self._interface.send_raw_datalink(msg)
"%s %s CONFIG_REQ %s" % (larg[0], larg[1], larg[2]))
if __name__ == "__main__":
signal.signal(signal.SIGINT, signal_handler)
if len(sys.argv) < 2:
print "Must supply one argument (ac_id)"
sys.exit(-1)
aircraft_id = int(sys.argv[1])
dmw.init()
dmw.init_pub("ground%d" % (aircraft_id))
interface = IvyMessagesInterface(process_messages,
bind_regex='(.*)',
verbose=True)
ivyId1 = IvyBindMsg(on_aircraft_req_msg, '((\\S*) (\\S*) AIRCRAFTS_REQ)')
ivyId2 = IvyBindMsg(on_config_req_msg, '((\\S*) (\\S*) CONFIG_REQ (\\S*))')
# Subscribe to messages from gcs.
t = SubscriptionThread(interface, aircraft_id)
t.setDaemon(True)
t.start()
while True:
time.sleep(0.1)
print("Exiting app")
class PlotPanel(object):
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 SetPlotInterval(self, value):
self.plot_interval = value
self.timer.Restart(self.plot_interval)
self.timer = wx.FutureCall(self.plot_interval, self.OnTimer)
def SetAutoScale(self, value):
self.auto_scale = value
def SetMin(self, value):
self.min = value
def SetMax(self, value):
self.max = value
def Pause(self, pause):
if pause:
self.timer.Stop()
else:
self.timer = wx.FutureCall(self.plot_interval, self.OnTimer)
def ResetScale(self):
self.max = -1e32
self.min = 1e32
def OnClose(self):
self.timer.Stop()
try:
IvyStop()
except IvyIllegalStateError as e:
print(e)
def OnErase(self, event):
pass
def ShowMessagePicker(self, parent):
frame = messagepicker.MessagePicker(parent, self.BindCurve, self.ivy_interface)
frame.Show()
def OnDropText(self, data):
[ac_id, category, message, field, scale] = data.encode('ASCII').split(':')
self.BindCurve(int(ac_id), message, field, scale=float(scale))
def OnIvyMsg(self, agent, *larg):
# print(larg[0])
data = larg[0].split(' ')
ac_id = int(data[0])
message = data[1]
if ac_id not in self.plots:
return
if message not in self.plots[ac_id]:
return
for field in self.plots[ac_id][message]:
plot = self.plots[ac_id][message][field]
ix = messages_xml_map.message_dictionary["telemetry"][message].index(field)
point = float(data[ix + 2])
if self.x_axis is None or self.x_axis.id != plot.id:
if self.auto_scale:
scaled_point = (point + plot.offset) * plot.scale
self.max = max(self.max, scaled_point)
self.min = min(self.min, scaled_point)
if self.x_axis is not None:
plot.index = self.x_axis.index
plot.AddPoint(point, self.x_axis)
def BindCurve(self, ac_id, message, field, color=None, use_as_x=False, scale=1.0):
# -- add this telemetry to our list of things to plot ...
message_string = _IVY_STRING % (ac_id, message)
# print('Binding to %s' % message_string)
if ac_id not in self.plots:
self.plots[ac_id] = {}
if message not in self.plots[ac_id]:
self.plots[ac_id][message] = {}
if field in self.plots[ac_id][message]:
self.plots[ac_id][message][field].color = wx.Color(random.randint(0, 255), random.randint(0, 255),
random.randint(0, 255))
return
ivy_id = self.ivy_interface.bind_raw(self.OnIvyMsg, str(message_string))
title = '%i:%s:%s' % (ac_id, message, field)
self.plots[ac_id][message][field] = PlotData(ivy_id, title, self.plot_size, color, scale)
self.frame.AddCurve(ivy_id, title, use_as_x)
if use_as_x:
self.x_axis = self.plots[ac_id][message][field]
def CalcMinMax(self, plot):
if not self.auto_scale: return
for x in plot.data:
self.max = max(self.max, x)
self.min = min(self.min, x)
self.frame.SetMinMax(self.min, self.max)
def FindPlotName(self, ivy_id):
for ac_id in self.plots:
for msg in self.plots[ac_id]:
for field in self.plots[ac_id][msg]:
if self.plots[ac_id][msg][field].id == ivy_id:
return (ac_id, msg, field)
return (None, None, None)
def FindPlot(self, ivy_id):
(ac_id, msg, field) = self.FindPlotName(ivy_id)
if ac_id is None:
return None
return self.plots[ac_id][msg][field]
def RemovePlot(self, ivy_id):
(ac_id, msg, field) = self.FindPlotName(ivy_id)
if ac_id is None:
return
if (self.x_axis is not None) and (self.x_axis.id == ivy_id):
self.x_axis = None
self.ivy_interface.unbind(ivy_id)
del self.plots[ac_id][msg][field]
if len(self.plots[ac_id][msg]) == 0:
del self.plots[ac_id][msg]
def OffsetPlot(self, ivy_id, offset):
plot = self.FindPlot(ivy_id)
if plot is None:
return
plot.SetOffset(offset)
print('panel value: %.2f' % value)
CalcMinMax(plot)
def ScalePlot(self, ivy_id, offset):
plot = self.FindPlot(ivy_id)
if plot is None:
return
plot.SetScale(offset)
CalcMinMax(plot)
def SetRealTime(self, ivy_id, value):
plot = self.FindPlot(ivy_id)
if plot is None:
return
plot.SetRealTime(value)
def SetXAxis(self, ivy_id):
plot = self.FindPlot(ivy_id)
if plot is None:
return
self.x_axis = plot
def ClearXAxis(self):
self.x_axis = None
def OnSize(self, size):
(width, height) = size
if self.width == width and self.height == height:
return
self.pixmap = wx.EmptyBitmap(width, height)
self.width = width
self.height = height
self.plot_size = width
self.margin = min(self.height / 10, 20)
self.font = wx.Font(self.margin / 2, wx.DEFAULT, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_NORMAL)
def OnTimer(self):
self.timer.Restart(self.plot_interval)
self.frame.SetMinMax(self.min, self.max)
self.DrawFrame()
def DrawFrame(self):
dc = wx.ClientDC(self.parent)
bdc = wx.BufferedDC(dc, self.pixmap)
bdc.SetBackground(wx.Brush("White"))
bdc.Clear()
self.DrawBackground(bdc, self.width, self.height)
title_y = 2
for ac_id in self.plots:
for message in self.plots[ac_id]:
for field in self.plots[ac_id][message]:
plot = self.plots[ac_id][message][field]
if (self.x_axis is not None) and (self.x_axis.id == plot.id):
continue
title_height = plot.DrawTitle(bdc, 2, self.width, title_y)
plot.DrawCurve(bdc, self.width, self.height, self.margin, self.max, self.min, self.x_axis)
title_y += title_height + 2
def DrawBackground(self, dc, width, height):
# Time Graduations
dc.SetFont(self.font)
if self.x_axis is None:
t = self.plot_interval * width
t1 = "0.0s"
t2 = "-%.1fs" % (t / 2000.0)
t3 = "-%.1fs" % (t / 1000.0)
else:
x_max = self.x_axis.x_max
x_min = self.x_axis.x_min
t1 = "%.2f" % x_max
t2 = "%.2f" % (x_min + (x_max - x_min) / 2.0)
t3 = "%.2f" % x_min
(w, h) = dc.GetTextExtent(t1)
dc.DrawText(t1, width - w, height - h)
# (w,h) = dc.GetTextExtent(t2) #save time since h will be the same
dc.DrawText(t2, width / 2, height - h)
# (w,h) = dc.GetTextExtent(t3) #save time since h will be the same
dc.DrawText(t3, 0, height - h)
# Y graduations
if self.max == -1e32:
return
(_min_, _max_) = (self.min, self.max)
if _max_ < _min_: # prevent divide by zero or inversion
(_min_, _max_) = (-1, 1)
if _max_ == _min_:
(_min_, _max_) = (_max_ - 0.5, _max_ + 0.5)
delta = _max_ - _min_
dy = (height - self.margin * 2) / delta
scale = math.log10(delta)
d = math.pow(10.0, math.floor(scale))
u = d
if delta < 2 * d:
u = d / 5
elif delta < 5 * d:
u = d / 2
tick_min = _min_ - math.fmod(_min_, u)
for i in range(int(delta / u) + 1):
tick = tick_min + float(i) * u
s = str(tick)
(w, h) = dc.GetTextExtent(s)
y = height - self.margin - int((tick - _min_) * dy) - h / 2
dc.DrawText(s, 0, y)
class CommandSender(object):
def __init__(self, verbose=False, callback = None):
self.verbose = verbose
self.callback = callback
self._interface = IvyMessagesInterface(self.message_recv)
def message_recv(self, ac_id, msg):
if (self.verbose and self.callback != None):
self.callback(ac_id, msg)
def shutdown(self):
print("Shutting down ivy interface...")
self._interface.shutdown()
def __del__(self):
self.shutdown()
def add_mission_command(self, msg_id = "", ac_id = 5 , insert = "APPEND" , wp_lat = "", wp_lon = "", wp_alt = "", duration = "60", center_lat = "", center_lon = "", center_alt = "", radius = "60",segment_lat_1 = "", segment_lat_2 = "", segment_lon_1 = "", segment_lon_2 = "", segment_alt = "", point_lat_1 = "", point_lon_1 = "", point_lat_2 = "", point_lon_2 = "", point_lat_3 = "", point_lon_3 = "", point_lat_4 = "", point_lon_4 = "", point_lat_5 = "", point_lon_5 = "", path_alt = "", nb = "", survey_lat_1 = "", survey_lon_1 = "", survey_lat_2 = "", survey_lon_2 = "", survey_alt = ""):
msg = PprzMessage("datalink", msg_id)
msg['ac_id'] = ac_id
msg['insert'] = insert
msg['duration'] = duration
if msg_id == MISSION_GOTO_WP_LLA:
msg['wp_lat'] = wp_lat
msg['wp_lon'] = wp_lon
msg['wp_alt'] = wp_alt
elif msg_id == MISSION_CIRCLE_LLA:
msg['center_lat'] = center_lat
msg['center_lon'] = center_lon
msg['center_alt'] = center_alt
msg['radius'] = radius
elif msg_id == MISSION_SEGMENT_LLA:
msg['segment_lat_1'] = segment_lat_1
msg['segment_lon_1'] = segment_lon_1
msg['segment_lat_2'] = segment_lat_2
msg['segment_lon_2'] = segment_lon_2
elif msg_id == MISSION_PATH_LLA:
msg['point_lat_1'] = point_lat_1
msg['point_lon_1'] = point_lon_1
msg['point_lat_2'] = point_lat_2
msg['point_lon_2'] = point_lon_2
msg['point_lat_3'] = point_lat_3
msg['point_lon_3'] = point_lon_3
msg['point_lat_4'] = point_lat_4
msg['point_lon_4'] = point_lon_4
msg['point_lat_5'] = point_lat_5
msg['point_lon_5'] = point_lon_5
msg['path_alt'] = path_alt
msg['nb'] = nb
elif msg_id == MISSION_SURVEY_LLA:
msg['survey_lat_1'] = survey_lat_1
msg['survey_lon_1'] = survey_lon_1
msg['survey_lat_2'] = survey_lat_2
msg['survey_lon_2'] = survey_lon_2
msg['survey_alt'] = survey_alt
print("Sending message: %s" % msg)
self._interface.send(msg)
def add_mission_command_dict(self, ac_id, insert, msg_id, msgs):
print('hello')
msg = PprzMessage("datalink", msg_id)
print(msgs)
print(msgs.keys)
print(msgs.get('duration'))
msg['ac_id'] = ac_id
msg['insert'] = insert
msg['duration'] = msgs.get('duration')
if msg_id == 'MISSION_GOTO_WP_LLA':
msg['wp_lat'] = msgs.get('wp_lat')
msg['wp_lon'] = msgs.get('wp_lon')
msg['wp_alt'] = msgs.get('wp_alt')
elif msg_id == 'MISSION_CIRCLE_LLA':
msg['center_lat'] = msgs.get('center_lat')
msg['center_lon'] = msgs.get('center_lon')
msg['center_alt'] = msgs.get('center_alt')
msg['radius'] = msgs.get('radius')
elif msg_id == 'MISSION_SEGMENT_LLA':
msg['segment_lat_1'] = msgs.get('segment_lat_1')
msg['segment_lon_1'] = msgs.get('segment_lon_1')
msg['segment_lat_2'] = msgs.get('segment_lat_2')
msg['segment_lon_2'] = msgs.get('segment_lon_2')
elif msg_id == 'MISSION_PATH_LLA':
msg['point_lat_1'] = msgs.get('point_lat_1')
msg['point_lon_1'] = msgs.get('point_lon_1')
msg['point_lat_2'] = msgs.get('point_lat_2')
msg['point_lon_2'] = msgs.get('point_lon_2')
msg['point_lat_3'] = msgs.get('point_lat_3')
msg['point_lon_3'] = msgs.get('point_lon_3')
msg['point_lat_4'] = msgs.get('point_lat_4')
msg['point_lon_4'] = msgs.get('point_lon_4')
msg['point_lat_5'] = msgs.get('point_lat_5')
msg['point_lon_5'] = msgs.get('point_lon_5')
msg['path_alt'] = msgs.get('path_alt')
msg['nb'] = msgs.get('nb')
elif msg_id == 'MISSION_SURVEY_LLA':
msg['survey_lat_1'] = msgs.get('survey_lat_1')
msg['survey_lon_1'] = msgs.get('survey_lon_1')
msg['survey_lat_2'] = msgs.get('survey_lat_2')
msg['survey_lon_2'] = msgs.get('survey_lon_2')
msg['survey_alt'] = msgs.get('survey_alt')
print("Sending message: %s" % msg)
self._interface.send(msg)
def add_obstacle(self, obstacle_id, color, status, lat, lon, radius, alt):
msg = PprzMessage("ground", "OBSTACLE")
msg['id'] = obstacle_id
msg['color'] = color
msg['status'] = status
msg['lat'] = lat
msg['lon'] = lon
msg['radius'] = radius
msg['alt'] = alt
print("Sending message: %s" % msg)
self._interface.send(msg)
class MessagePicker(wx.Frame):
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 OnClose(self, event):
# if we have a parent (like the plotpanel) only hide instead of shutdown
if self.GetParent() is not None:
self.Hide()
else:
self.message_interface.shutdown()
self.Destroy()
def msg_recv(self, ac_id, msg):
if msg.msg_class != "telemetry":
return
self.tree.Expand(self.root)
if ac_id not in self.aircrafts:
ac_node = self.tree.AppendItem(self.root, str(ac_id))
self.aircrafts[ac_id] = Aircraft(ac_id)
self.aircrafts[ac_id].messages_book = ac_node
aircraft = self.aircrafts[ac_id]
ac_node = aircraft.messages_book
if msg.name not in aircraft.messages:
msg_node = self.tree.AppendItem(ac_node, str(msg.name))
self.tree.SortChildren(ac_node)
aircraft.messages[msg.name] = Message("telemetry", msg.name)
for field in aircraft.messages[msg.name].fieldnames:
item = self.tree.AppendItem(msg_node, field)
def OnKeyChar(self, event):
if event.GetKeyCode() != 13:
return False
node = self.tree.GetSelection()
field_name = self.tree.GetItemText(node)
parent = self.tree.GetItemParent(node)
message_name = self.tree.GetItemText(parent)
grandparent = self.tree.GetItemParent(parent)
ac_id = self.tree.GetItemText(grandparent)
if node == self.root or parent == self.root or grandparent == self.root:
# if not leaf, double click = expand
if self.tree.IsExpanded(node):
self.tree.Collapse(node)
else:
self.tree.Expand(node)
return
self.callback(int(ac_id), message_name, field_name)
def OnDoubleClick(self, event):
node = self.tree.GetSelection()
field_name = self.tree.GetItemText(node)
parent = self.tree.GetItemParent(node)
message_name = self.tree.GetItemText(parent)
grandparent = self.tree.GetItemParent(parent)
ac_id = self.tree.GetItemText(grandparent)
if node == self.root or parent == self.root or grandparent == self.root:
# if not leaf, double click = expand
if self.tree.IsExpanded(node):
self.tree.Collapse(node)
else:
self.tree.Expand(node)
return
self.callback(int(ac_id), message_name, field_name)
def on_aircrafts_msg( self, agent, *larg ):
dmw.publish( "pprz", "opaque", "%s %s AIRCRAFTS %s"%( larg[0], larg[1], larg[2] ) )
def on_config_msg( self, agent, *larg ):
dmw.publish( "pprz", "opaque", "%s %s CONFIG %s"%( larg[0], larg[1], larg[2] ) )
if __name__ == "__main__":
signal.signal(signal.SIGINT, signal_handler)
if len(sys.argv) < 2:
print "Must supply one argument (ac_id)"
sys.exit(-1)
vehicle = Vehicle( int( sys.argv[1] ) )
dmw.init()
dmw.init_pub( str(vehicle.ac_id) )
interface = IvyMessagesInterface( process_messages, verbose=True )
ivyId1 = IvyBindMsg( on_aircrafts_msg, '((\\S*) (\\S*) AIRCRAFTS (\\S*))')
ivyId1 = IvyBindMsg( on_config_msg, '((\\S*) (\\S*) CONFIG (.*))')
# Subscribe to messages from gcs.
t = SubscriptionThread( interface )
t.setDaemon( True )
t.start()
while True:
time.sleep(0.1)
class MessagesFrame(wx.Frame):
def message_recv(self, ac_id, msg):
"""Handle incoming messages
Callback function for IvyMessagesInterface
:param ac_id: aircraft id
:type ac_id: int
:param msg: message
:type msg: PprzMessage
"""
# only show messages of the requested class
if msg.msg_class != self.msg_class:
return
if ac_id in self.aircrafts and msg.name in self.aircrafts[ac_id].messages:
if time.time() - self.aircrafts[ac_id].messages[msg.name].last_seen < 0.2:
return
wx.CallAfter(self.gui_update, ac_id, msg)
def find_page(self, book, name):
if book.GetPageCount() < 1:
return 0
start = 0
end = book.GetPageCount()
while start < end:
if book.GetPageText(start) >= name:
return start
start += 1
return start
def update_leds(self):
wx.CallAfter(self.update_leds_real)
def update_leds_real(self):
for ac_id in self.aircrafts:
aircraft = self.aircrafts[ac_id]
for msg_str in aircraft.messages:
message = aircraft.messages[msg_str]
if message.last_seen + 0.2 < time.time():
aircraft.messages_book.SetPageImage(message.index, 0)
self.timer = threading.Timer(0.1, self.update_leds)
self.timer.start()
def setup_image_list(self, notebook):
imageList = wx.ImageList(24, 24)
image = wx.Image(PPRZ_HOME + "/data/pictures/gray_led24.png")
bitmap = wx.BitmapFromImage(image)
imageList.Add(bitmap)
image = wx.Image(PPRZ_HOME + "/data/pictures/green_led24.png")
bitmap = wx.BitmapFromImage(image)
imageList.Add(bitmap)
notebook.AssignImageList(imageList)
def add_new_aircraft(self, ac_id):
self.aircrafts[ac_id] = Aircraft(ac_id)
ac_panel = wx.Panel(self.notebook, -1)
self.notebook.AddPage(ac_panel, str(ac_id))
messages_book = wx.Notebook(ac_panel, style=wx.NB_LEFT)
self.setup_image_list(messages_book)
sizer = wx.BoxSizer(wx.VERTICAL)
sizer.Add(messages_book, 1, wx.EXPAND)
ac_panel.SetSizer(sizer)
sizer.Layout()
self.aircrafts[ac_id].messages_book = messages_book
def add_new_message(self, aircraft, msg_class, name):
messages_book = aircraft.messages_book
aircraft.messages[name] = Message(msg_class, name)
field_panel = wx.Panel(messages_book)
grid_sizer = wx.FlexGridSizer(len(aircraft.messages[name].fieldnames), 2)
index = self.find_page(messages_book, name)
messages_book.InsertPage(index, field_panel, name, imageId=1)
aircraft.messages[name].index = index
# update indexes of pages which are to be moved
for message_name in aircraft.messages:
aircraft.messages[message_name].index = self.find_page(messages_book, message_name)
for field_name in aircraft.messages[name].fieldnames:
name_text = wx.StaticText(field_panel, -1, field_name)
size = name_text.GetSize()
size.x = LABEL_WIDTH
name_text.SetMinSize(size)
grid_sizer.Add(name_text, 1, wx.ALL, BORDER)
value_control = wx.StaticText(field_panel, -1, "42", style=wx.EXPAND)
size = value_control.GetSize()
size.x = LABEL_WIDTH
value_control.SetMinSize(size)
grid_sizer.Add(value_control, 1, wx.ALL | wx.EXPAND, BORDER)
if wx.MAJOR_VERSION > 2:
if grid_sizer.IsColGrowable(1):
grid_sizer.AddGrowableCol(1)
else:
grid_sizer.AddGrowableCol(1)
aircraft.messages[name].field_controls.append(value_control)
field_panel.SetAutoLayout(True)
field_panel.SetSizer(grid_sizer)
field_panel.Layout()
def gui_update(self, ac_id, msg):
if ac_id not in self.aircrafts:
self.add_new_aircraft(ac_id)
aircraft = self.aircrafts[ac_id]
if msg.name not in aircraft.messages:
self.add_new_message(aircraft, msg.msg_class, msg.name)
aircraft.messages_book.SetPageImage(aircraft.messages[msg.name].index, 1)
self.aircrafts[ac_id].messages[msg.name].last_seen = time.time()
for index in range(0, len(msg.fieldvalues)):
aircraft.messages[msg.name].field_controls[index].SetLabel(msg.get_field(index))
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 OnClose(self, event):
self.timer.cancel()
self.interface.shutdown()
self.Destroy()
class Guidance(object):
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 message_recv(self, ac_id, msg):
if self.verbose:
print("Got msg %s" % msg.name)
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
def __del__(self):
self.shutdown()
def set_guided_mode(self):
"""
change auto2 mode to GUIDED.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.auto2_index
msg['value'] = 19 # AP_MODE_GUIDED
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change auto2 mode to NAV.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.auto2_index
msg['value'] = 13 # AP_MODE_NAV
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def goto_ned(self, north, east, down, heading=0.0):
"""
goto a local NorthEastDown position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x00
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = heading
print("goto NED: %s" % msg)
# embed the message in RAW_DATALINK so that the server can log it
self._interface.send_raw_datalink(msg)
def goto_ned_relative(self, north, east, down, yaw=0.0):
"""
goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x01
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = yaw
print("goto NED relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def goto_body_relative(self, forward, right, down, yaw=0.0):
"""
goto to a position relative to current position and heading in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x03
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("goto body relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def move_at_vel(self, north=0.0, east=0.0, down=0.0, yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x70
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = yaw
print("move at vel NED: %s" % msg)
self._interface.send_raw_datalink(msg)
class MessagePicker(wx.Frame):
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 OnClose(self, event):
# if we have a parent (like the plotpanel) only hide instead of shutdown
if self.GetParent() is not None:
self.Hide()
else:
self.message_interface.shutdown()
self.Destroy()
def msg_recv(self, ac_id, msg):
if msg.msg_class != "telemetry":
return
self.tree.Expand(self.root)
if ac_id not in self.aircrafts:
ac_node = self.tree.AppendItem(self.root, str(ac_id))
self.aircrafts[ac_id] = Aircraft(ac_id)
self.aircrafts[ac_id].messages_book = ac_node
aircraft = self.aircrafts[ac_id]
ac_node = aircraft.messages_book
if msg.name not in aircraft.messages:
msg_node = self.tree.AppendItem(ac_node, str(msg.name))
self.tree.SortChildren(ac_node)
aircraft.messages[msg.name] = Message("telemetry", msg.name)
for field in aircraft.messages[msg.name].fieldnames:
item = self.tree.AppendItem(msg_node, field)
def OnKeyChar(self, event):
if event.GetKeyCode() != 13:
return False
node = self.tree.GetSelection()
field_name = self.tree.GetItemText(node)
parent = self.tree.GetItemParent(node)
message_name = self.tree.GetItemText(parent)
grandparent = self.tree.GetItemParent(parent)
ac_id = self.tree.GetItemText(grandparent)
if node == self.root or parent == self.root or grandparent == self.root:
# if not leaf, double click = expand
if self.tree.IsExpanded(node):
self.tree.Collapse(node)
else:
self.tree.Expand(node)
return
self.callback(int(ac_id), message_name, field_name)
def OnDoubleClick(self, event):
node = self.tree.GetSelection()
field_name = self.tree.GetItemText(node)
parent = self.tree.GetItemParent(node)
message_name = self.tree.GetItemText(parent)
grandparent = self.tree.GetItemParent(parent)
ac_id = self.tree.GetItemText(grandparent)
if node == self.root or parent == self.root or grandparent == self.root:
# if not leaf, double click = expand
if self.tree.IsExpanded(node):
self.tree.Collapse(node)
else:
self.tree.Expand(node)
return
self.callback(int(ac_id), message_name, field_name)
def __init__(self, verbose=False, callback=None):
self.verbose = verbose
self.callback = callback
self._interface = IvyMessagesInterface(self.message_recv)
def __init__(self, verbose=False):
self.verbose = verbose
self._interface = IvyMessagesInterface(self.message_recv)
def __init__(self, verbose=False):
self.verbose = verbose
self._interface = IvyMessagesInterface("WaypointMover")
def __init__(self, verbose=False):
self.verbose = verbose
self._interface = IvyMessagesInterface(self.message_recv)
class MessagesFrame(wx.Frame):
def message_recv(self, ac_id, msg):
"""Handle incoming messages
Callback function for IvyMessagesInterface
:param ac_id: aircraft id
:type ac_id: int
:param msg: message
:type msg: PprzMessage
"""
# only show messages of the requested class
if msg.msg_class != self.msg_class:
return
if ac_id in self.aircrafts and msg.name in self.aircrafts[
ac_id].messages:
if time.time() - self.aircrafts[ac_id].messages[
msg.name].last_seen < 0.2:
return
wx.CallAfter(self.gui_update, ac_id, msg)
def find_page(self, book, name):
if book.GetPageCount() < 1:
return 0
start = 0
end = book.GetPageCount()
while start < end:
if book.GetPageText(start) >= name:
return start
start += 1
return start
def update_leds(self):
wx.CallAfter(self.update_leds_real)
def update_leds_real(self):
for ac_id in self.aircrafts:
aircraft = self.aircrafts[ac_id]
for msg_str in aircraft.messages:
message = aircraft.messages[msg_str]
if message.last_seen + 0.2 < time.time():
aircraft.messages_book.SetPageImage(message.index, 0)
self.timer = threading.Timer(0.1, self.update_leds)
self.timer.start()
def setup_image_list(self, notebook):
imageList = wx.ImageList(24, 24)
image = wx.Image(PPRZ_HOME + "/data/pictures/gray_led24.png")
bitmap = wx.BitmapFromImage(image)
imageList.Add(bitmap)
image = wx.Image(PPRZ_HOME + "/data/pictures/green_led24.png")
bitmap = wx.BitmapFromImage(image)
imageList.Add(bitmap)
notebook.AssignImageList(imageList)
def add_new_aircraft(self, ac_id):
self.aircrafts[ac_id] = Aircraft(ac_id)
ac_panel = wx.Panel(self.notebook, -1)
self.notebook.AddPage(ac_panel, str(ac_id))
messages_book = wx.Notebook(ac_panel, style=wx.NB_LEFT)
self.setup_image_list(messages_book)
sizer = wx.BoxSizer(wx.VERTICAL)
sizer.Add(messages_book, 1, wx.EXPAND)
ac_panel.SetSizer(sizer)
sizer.Layout()
self.aircrafts[ac_id].messages_book = messages_book
def add_new_message(self, aircraft, msg_class, name):
messages_book = aircraft.messages_book
aircraft.messages[name] = Message(msg_class, name)
field_panel = wx.Panel(messages_book)
grid_sizer = wx.FlexGridSizer(len(aircraft.messages[name].fieldnames),
2)
index = self.find_page(messages_book, name)
messages_book.InsertPage(index, field_panel, name, imageId=1)
aircraft.messages[name].index = index
# update indexes of pages which are to be moved
for message_name in aircraft.messages:
aircraft.messages[message_name].index = self.find_page(
messages_book, message_name)
for field_name in aircraft.messages[name].fieldnames:
name_text = wx.StaticText(field_panel, -1, field_name)
size = name_text.GetSize()
size.x = LABEL_WIDTH
name_text.SetMinSize(size)
grid_sizer.Add(name_text, 1, wx.ALL, BORDER)
value_control = wx.StaticText(field_panel,
-1,
"42",
style=wx.EXPAND)
size = value_control.GetSize()
size.x = LABEL_WIDTH
value_control.SetMinSize(size)
grid_sizer.Add(value_control, 1, wx.ALL | wx.EXPAND, BORDER)
if wx.MAJOR_VERSION > 2:
if grid_sizer.IsColGrowable(1):
grid_sizer.AddGrowableCol(1)
else:
grid_sizer.AddGrowableCol(1)
aircraft.messages[name].field_controls.append(value_control)
field_panel.SetAutoLayout(True)
field_panel.SetSizer(grid_sizer)
field_panel.Layout()
def gui_update(self, ac_id, msg):
if ac_id not in self.aircrafts:
self.add_new_aircraft(ac_id)
aircraft = self.aircrafts[ac_id]
if msg.name not in aircraft.messages:
self.add_new_message(aircraft, msg.msg_class, msg.name)
aircraft.messages_book.SetPageImage(aircraft.messages[msg.name].index,
1)
self.aircrafts[ac_id].messages[msg.name].last_seen = time.time()
for index in range(0, len(msg.fieldvalues)):
aircraft.messages[msg.name].field_controls[index].SetLabel(
msg.get_field(index))
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(self.message_recv)
def OnClose(self, event):
self.timer.cancel()
self.interface.shutdown()
self.Destroy()
class Guidance(object):
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 message_recv(self, ac_id, msg):
if self.verbose:
print("Got msg %s" % msg.name)
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
def __del__(self):
self.shutdown()
def set_guided_mode(self):
"""
change auto2 mode to GUIDED.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg["ac_id"] = self.ac_id
msg["index"] = self.auto2_index
msg["value"] = 19 # AP_MODE_GUIDED
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change auto2 mode to NAV.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg["ac_id"] = self.ac_id
msg["index"] = self.auto2_index
msg["value"] = 13 # AP_MODE_NAV
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def goto_ned(self, north, east, down, heading=0.0):
"""
goto a local NorthEastDown position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg["ac_id"] = self.ac_id
msg["flags"] = 0x00
msg["x"] = north
msg["y"] = east
msg["z"] = down
msg["yaw"] = heading
print("goto NED: %s" % msg)
# embed the message in RAW_DATALINK so that the server can log it
self._interface.send_raw_datalink(msg)
def goto_ned_relative(self, north, east, down, yaw=0.0):
"""
goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg["ac_id"] = self.ac_id
msg["flags"] = 0x01
msg["x"] = north
msg["y"] = east
msg["z"] = down
msg["yaw"] = yaw
print("goto NED relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def goto_body_relative(self, forward, right, down, yaw=0.0):
"""
goto to a position relative to current position and heading in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg["ac_id"] = self.ac_id
msg["flags"] = 0x03
msg["x"] = forward
msg["y"] = right
msg["z"] = down
msg["yaw"] = yaw
print("goto body relative: %s" % msg)
self._interface.send_raw_datalink(msg)
