[python] refactor messages

sw/lib/python/ivy_msg_interface.py

@@ -6,9 +6,17 @@ import os
 import sys
 import re
 
+# if PAPARAZZI_SRC not set, then assume the tree containing this
+# file is a reasonable substitute
+PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)),
+                                                                    '../../../../')))
+sys.path.append(PPRZ_SRC + "/sw/lib/python")
 
-class IvyMessagesInterface():
-    def __init__(self, callback, init=True, verbose=True, bind_regex="(.*)"):
+from pprz_msg.message import PprzMessage
+
+
+class IvyMessagesInterface(object):
+    def __init__(self, callback, init=True, verbose=False, bind_regex='(.*)'):
         self.callback = callback
         self.ivy_id = 0
         self.verbose = verbose
@@ -27,7 +35,7 @@ class IvyMessagesInterface():
         except:
             pass
 
-    def init_ivy(self, init, bind_regex):
+    def init_ivy(self, init=True, bind_regex='(.*)'):
         if init:
             IvyInit("Messages %i" % os.getpid(), "READY", 0, lambda x,y: y, lambda x,y: y)
             logging.getLogger('Ivy').setLevel(logging.WARN)
@@ -56,13 +64,16 @@ class IvyMessagesInterface():
 
         # check which message class it is
         # pass non-telemetry messages with ac_id 0
-        if data[0] in ["ground", "ground_dl", "dl"]:
+        if data[0] in ["sim", "ground_dl", "dl"]:
+            if self.verbose:
+                    print("ignoring message " + ' '.join(data))
+                    sys.stdout.flush()
+            return
+        elif data[0] in ["ground"]:
             msg_class = data[0]
             msg_name = data[1]
             ac_id = 0
             values = list(filter(None, data[2:]))
-        elif data[0] == "sim":
-            return
         else:
             try:
                 ac_id = int(data[0])
@@ -74,4 +85,6 @@ class IvyMessagesInterface():
             msg_class = "telemetry"
             msg_name = data[1]
             values = list(filter(None, data[2:]))
-        self.callback(msg_class, msg_name, ac_id, values)
+        msg = PprzMessage(msg_class, msg_name)
+        msg.set_values(values)
+        self.callback(msg_class, msg_name, ac_id, msg)

sw/lib/python/pprz_msg/message.py

@@ -0,0 +1,62 @@
+"""
+Paparazzi message representation
+
+"""
+
+from __future__ import print_function
+import sys
+import json
+import messages_xml_map
+
+
+class PprzMessageError(Exception):
+    def __init__(self, message, inner_exception=None):
+        self.message = message
+        self.inner_exception = inner_exception
+        self.exception_info = sys.exc_info()
+    def __str__(self):
+        return self.message
+
+
+class PprzMessage(object):
+    """base Paparazzi message class"""
+    def __init__(self, class_name, name):
+        self._class_name = class_name
+        self._name = name
+        self._id = messages_xml_map.get_msg_id(class_name, name)
+        self._fieldnames = messages_xml_map.get_msg_fields(class_name, name)
+        self._fieldvalues = []
+
+    def get_fieldnames(self):
+        return self._fieldnames
+
+    def get_fieldvalues(self):
+        return self._fieldvalues
+
+    def get_field(self, idx):
+        return self._fieldvalues[idx]
+
+    def set_values(self, values):
+        if len(values) == len(self._fieldnames):
+            self._fieldvalues = values
+        else:
+            raise PprzMessageError("Error: fields not matching")
+
+    def __str__(self):
+        ret = '%s.%s {' % (self._class_name, self._name)
+        for idx, f in enumerate(self._fieldnames):
+            ret += '%s : %s, ' % (f, self._fieldvalues[idx])
+        ret = ret[0:-2] + '}'
+        return ret
+
+    def to_dict(self, payload_only=False):
+        d = {}
+        if not payload_only:
+            d['msgname'] = self._name
+            d['msgclass'] = self._class_name
+        for idx, f in enumerate(self._fieldnames):
+            d[f] = self._fieldvalues[idx]
+        return d
+
+    def to_json(self, payload_only=False):
+        return json.dumps(self.to_dict(payload_only))

sw/lib/python/pprz_msg/messages_xml_map.py

@@ -7,7 +7,7 @@ import os
 # if PAPARAZZI_HOME not set, then assume the tree containing this
 # file is a reasonable substitute
 PPRZ_HOME = os.getenv("PAPARAZZI_HOME", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)),
-                                                                      '../../../')))
+                                                                      '../../../..')))
 
 default_messages_file = '%s/conf/messages.xml' % PPRZ_HOME
 
@@ -61,6 +61,30 @@ def parse_messages(messages_file=default_messages_file):
                 message_dictionary_types[class_name][message_id].append(the_field.attrib['type'])
 
 
+def get_msg_fields(msg_class, msg_name):
+    if not message_dictionary:
+        parse_messages()
+    if msg_class in message_dictionary:
+        if msg_name in message_dictionary[msg_class]:
+            return message_dictionary[msg_class][msg_name]
+        else:
+            print("Error: msg_name %s not found in msg_class %s." % (msg_name, msg_class))
+    else:
+        print("Error: msg_class %s not found." % msg_class)
+    return []
+
+
+def get_msg_id(msg_class, msg_name):
+    if not message_dictionary:
+        parse_messages()
+    try:
+        return message_dictionary_name_id[msg_class][msg_name]
+    except KeyError:
+        print("Error: msg_name %s not found in msg_class %s." % (msg_name, msg_class))
+        return 0
+
+
+
 def test():
     import argparse
     parser = argparse.ArgumentParser()