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)
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()
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 ""
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)
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 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)
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 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)
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("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)
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)
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 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()
