Changeset 522

Timestamp:

06/05/09 16:29:49 (3 years ago)

Author:

mosttw

Message:

Progress on JAUS packet serialization

Files:

• trunk/software/rb/jaus.py (modified) (7 diffs)

trunk/software/rb/jaus.py

@@ -14 +14 @@
 # along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
+from __future__ import division
 
 '''
@@ -28 +29 @@
 
 PORT = 3794
-OCU_IP = '192.168.128.1'
+
+COP_JAUS_ID = (90, 1, 1)
+COP_IP = '192.168.1.%d' % COP_ID[0]
 
 # Command codes
@@ -35 +38 @@
 CMD_SET_DISCRETE_DEV = 0x0406
 
+# JAUS message header version constants
+JAUS_2 = 0 # Version 2.0 and 2.1
+JAUS_3_0 = 1 # Version 3.0 and 3.1
+JAUS_3_2 = 2 # Version 3.2 and 3.3
+
+# Priority codes
+PRIORITY_LOW = 0
+PRIORITY_DEFAULT = 6
+PRIORITY_HIGH = 11
+PRIORITY_SAFETY_CRIT_LOW = 12
+PRIORITY_SAFETY_CRIT_HIGH = 15
+
+
+# Conversion functions for converting between "scaled" integers and
+# real floats.
+#
+# See formulas on page 4 of the JAUS Architecture Reference Specification, Volume II Part 2 (v. 3.3)
+def scaled_u16_to_real(int, min, max):
+        return int * ((max - min) / 65535) + min
+
+def real_to_scaled_u32(real, min, max):
+        return (real - min) * (65535 / (max - min))
+
+def scaled_u32_to_real(int, min, max):
+        return int * ((max - min) / 4294967295) + min
+
+def real_to_scaled_u32(real, min, max):
+        return (real - min) * (4294967295 / (max - min))
+
+def scaled_u64_to_real(int, min, max):
+        return int * ((max - min) / 18446744073709551615L) + min
+
+def real_to_scaled_u64(real, min, max):
+        return (real - min) * (18446744073709551615L / (max - min))
+
+
 
 class JAUS(object):
@@ -54 +93 @@
                 while True:
                         data, sender = sock.recvfrom(4096)
-                        #~ if sender[0] != OCU_IP:
-                                #~ log_info("JAUS: Got a packet from %s; ignoring." % sender[0])
-                                #~ continue
+                        if sender[0] != OCU_IP:
+                                log_info("JAUS: Got a packet from %s; ignoring." % sender[0])
+                                continue
                         try:
                                 msg = Message(data)
@@ -62 +101 @@
                                 log_error("JAUS: Message was malformed: %r" % e)
                                 continue
-                
                         if not self.self.validate_message(msg):
                                 continue
+                        
         
         def validate_message(self, msg):
                 '''
-                Check that 
+                Check that various header fields match the expected values.
                 '''
                 # Validate the header
@@ -135 +174 @@
 class JAUSMessageMalformed(Exception): pass
 
-class JAUSPrefixMissing(JAUSMessageMalformed):
-        'That "JAUS01.0" prefix specified in the requirements document was missing.'
-
 class JAUSReservedNotZero(JAUSMessageMalformed):
         "The reserved bits in the JAUS message properties field weren't zero."
@@ -151 +187 @@
         def __init__(self, data=None):
                 if data is not None:
-                        (jaus, self.seq_num, self.data_control, self.source_subsystem, self.source_node,
-                         self.source_component, self.source_instance, self.dest_subsystem, self.dest_node,
-                         self.dest_component, self.dest_instance, self.command_code, self.message_props
-                        ) = struct.unpack('<H<H 8B <H<H 8s', data)
-                
-                        if jaus != 'JAUS01.0':
-                                raise JAUSPrefixMissing()
+                        (message_props, self.command_code,
+                         self.source_instance, self.source_component, self.source_node, self.source_subsystem,
+                         self.dest_instance, self.dest_component, self.dest_node, self.dest_subsystem,
+                         data_control, self.seq_num) = struct.unpack('<HH 4B 4B HH', data)
                         
                         # Unpack message properties
-                        if self.message_props & 0x:
-                                raise JAUSReservedNotZero()
-                        self.mp_version = (self.message_props & 0x3f00) >> 8
-                        self.mp_experimental = bool(self.message_props & 0x80)
-                        self.mp_service_connection = bool(self.message_props & 0x40)
-                        self.mp_acknak = (self.message_props & 0x30) >> 4
-                        self.mp_priority = self.message_props & 0xF
+                        # A number of flags aren't supported so we error if we enounter them.
+                        self.mp_version = (message_props & 0x3f00) >> 8
+                        assert self.mp_version == JAUS_3_2
+                        self.mp_experimental = bool(message_props & 0x80)
+                        assert not self.mp_experimental
+                        self.mp_service_connection = bool(message_props & 0x40)
+                        assert not self.mp_service_connection
+                        self.mp_acknak = (message_props & 0x30) >> 4
+                        assert not self.mp_acknak
+                        self.mp_priority = message_props & 0xF
                         
                         # Unpack data control
-                        self.data_size = self.data_control & 0xFFF
-                        self.data_flags = self.data_control >> 12
-                        
-                        self.data = data[8 + 16:]
-                        
-                        if len(data) != self.data_size:
+                        data_size = data_control & 0xFFF
+                        self.data_flags = data_control >> 12
+                        assert self.data_flags == 0 # Not supported
+
+                        self.data = data[16:]
+                        if len(data) != data_size:
                                 raise JAUSBadDataLength()
                 else:
+                        self.mp_version = JAUS_3_2
+                        self.mp_experimental = False
+                        self.mp_service_connection = False
+                        self.mp_acknak = 0
+                        self.mp_priority = PRIORITY_DEFAULT
                         self.seq_num = 0
-                        self.command_code = 0
-                        self.dest_instance = 1
-                        self.dest_component = 40
-                        self.dest_node = 1
-                        self.dest_subsystem = 1
-                        self.source_instance = 1
-                        self.source_component = 38 # Global pose sensor
-                        self.source_node = 1
-                        self.source_subsystem = rbconfig.jaus_subsystem_id
+                        self.command_code = None
+                        self.dest_instance = 0
+                        self.dest_component = 0
+                        self.dest_node = 0
+                        self.dest_subsystem = 0 
+                        self.source_instance = 0
+                        self.source_component = 0
+                        self.source_node = 0
+                        self.source_subsystem = 0
                         self.data_flags = 0
                         self.data = ''
         
+        def _get_dest(self):
+                return (self.dest_subsystem, self.dest_node, self.dest_component)
+        def _set_dest(self, (subsystem, node, component)):
+                self.dest_subsystem = subsystem
+                self.dest_node = node
+                self.dest_component = component
+        dest = property(_get_dest, _set_dest)
+
+        def _get_src(self):
+                return (self.src_subsystem, self.src_node, self.src_component)
+        def _set_src(self, (subsystem, node, component)):
+                self.src_subsystem = subsystem
+                self.src_node = node
+                self.src_component = component
+        src = property(_get_src, _set_src)
+
         def to_bytes(self):
-                data_control = 0xFFF & len(self.data)
-                message_props = (
-                        2 << 8 | # Version
-                        6 # default priority
-                )
-                
-                return struct.pack('8s <H<H 8B <H<H 8s', 
-                        'JAUS01.0', self.seq_num, data_control, self.source_subsystem, self.source_node, 
-                        self.source_component, self.source_instance, self.dest_subsystem, self.dest_node,
-                        self.dest_component, self.dest_instance, self.command_code, message_props
-                        )
+                assert len(self.data) <= 4080
+                data_control = (self.data_flags << 12) | (len(self.data) + 16)
+                assert self.mp_priority < 16
+                message_props = self.mp_version << 8 | self.mp_priority
+        
+                assert self.mp_version = JAUS_3_2
+                assert self.mp_experimental is False
+                assert self.mp_service_connect is False
+                assert self.mp_acknak == 0
+                assert self.command_code is not None
+                assert self.dest_instance != 0
+                assert self.dest_component != 0
+                assert self.dest_node != 0
+                assert self.dest_subsystem != 0
+                assert self.src_instance != 0
+                assert self.src_component != 0
+                assert self.src_node != 0
+                assert self.src_subsystem != 0
+
+                header = struct.pack('<HH 4B 4B HH', message_props, self.command_code,
+                         self.dest_instance, self.dest_component, self.dest_node, self.dest_subsystem,
+                         self.src_instance, self.src_component, self.src_node, self.src_subsystem,
+                         data_control, self.seq_num)
+                return header + self.data 
         
         def __repr__(self):
                 return "JAUSMesage<%r>" % (self.__dict__,) # Lazy, lazy, lazy ;-)
+