Made some changes to the simulator to debug controller implementation.

simulator/extctrl2014.py

@@ -32,6 +32,8 @@ import threading
 #   +
 #
 
+ETHERTYPE = 'EX'
+
 flag_MASK =           0x07
 flag_LAST_FRAG =      0x01
 flag_NEED_ACK =       0x02
@@ -130,7 +132,7 @@ class Fragmenter(Encoder):
             fragment = Encoder()
             fragment.write(self.destination)
             fragment.write('\x00\x00\x00\x00\x00\x00')
-            fragment.write('EX')
+            fragment.write(ETHERTYPE)
             if len(remaining) == 0:
                 fragment.encode_byte(self.flags | flag_LAST_FRAG)
             else:
@@ -156,7 +158,7 @@ class Packet:
         self.destination = buf.read(6)
         self.source = buf.read(6)
         self.eth_type = buf.read(2)
-        if self.eth_type == 'EX':
+        if self.eth_type == ETHERTYPE:
             flags = buf.decode_byte()
             self.flags = flags & flag_MASK
             self.kind = flags & kind_MASK
@@ -186,22 +188,50 @@ class Packet:
         self.frag_num = other.frag_num
         self.data += other.data
         return self
-        
+
     def __repr__(self):
-        return "Packet(%s %s %d index=%x flags=%x kind=%x data='%s'" % (
-            ":".join(map(lambda b: "%02.2x" % ord(b), self.destination)),
-            ":".join(map(lambda b: "%02.2x" % ord(b), self.source)),
+        macaddr2str = lambda b: ":".join(["%02.2x" % ord(byte) for byte in b])
+
+        if self.eth_type != ETHERTYPE:
+            int_ethertype = ((ord(self.eth_type[0]) << 8) |
+                             ord(self.eth_type[1]))
+            return "Packet(%s %s 0x%x)" % (
+                macaddr2str(self.destination),
+                macaddr2str(self.source),
+                int_ethertype,
+            )
+
+        flagify = lambda strlist: '(%s)' % ' | '.join(strlist)
+
+        list_flags = []
+        if self.flags & flag_LAST_FRAG:
+            list_flags.append('LAST_FRAG')
+        if self.flags & flag_NEED_ACK:
+            list_flags.append('NEED_ACK')
+        if self.flags & flag_IS_ACK:
+            list_flags.append('IS_ACK')
+
+        list_kind = []
+        if self.kind & kind_INIT:
+            list_kind.append('INIT')
+        if self.kind & kind_DATA:
+            list_kind.append('DATA')
+
+        return "Packet(%s %s channel=%d cookie=0x%x index=0x%x flags=%s kind=%s data='%s'" % (
+            macaddr2str(self.destination),
+            macaddr2str(self.source),
             self.channel,
+            self.cookie,
             self.index,
-            self.flags,
-            self.kind,
+            flagify(list_flags),
+            flagify(list_kind),
             "".join(map(lambda b: "\\x%02.2x" % ord(b), self.data)),
        )
 
 class ExtCtrl(object):
 
     def __init__(self, ethernet, channel, controller=None, robot=None,
-                 max_retries=1000, max_length=1480):
+                 max_retries=1000, max_length=1480, packetdebug=False):
         if len(filter(None, [ robot, controller])) != 1:
             raise Exception('Either robot(%s) or controller(%s) '
                             'should be defined' %
@@ -214,11 +244,13 @@ class ExtCtrl(object):
         self.robot = robot
         self.max_retries = max_retries
         self.max_length = max_length
+        self.packetdebug=packetdebug
         self.read_data = list()
         self.cond = threading.Condition()
         self.local_index = random.randint(1,2**32-1)
         self.remote_index = None
-        self.cookie = None
+        self.cookie_remote_index = None
+        self.cookie_local_index = None
         self.ack = None
         t = threading.Thread(target=self.recv_loop)
         t.daemon = True
@@ -228,6 +260,12 @@ class ExtCtrl(object):
         elif self.robot:
             self.await_INIT()
 
+    @property
+    def cookie(self):
+        if self.cookie_remote_index is None:
+            return 0
+        return self.cookie_local_index ^ self.cookie_remote_index
+
     def next_index(self):
         self.local_index = max(1, (self.local_index + 1) & 0xffffffff)
 
@@ -236,15 +274,28 @@ class ExtCtrl(object):
         data = None
         while True:
             fragment = Packet(self.ethernet.recv(2000))
+            if self.packetdebug:
+                print "Got", fragment
 
             # Sanity check fragment
-            if fragment.eth_type != 'EX':
+            if fragment.eth_type != ETHERTYPE:
+                if self.packetdebug:
+                    print "Bad eth"
                 continue
             if fragment.source != self.other:
+                if self.packetdebug:
+                    print "Bad source"
                 continue
             if fragment.channel != self.channel:
+                if self.packetdebug:
+                    print "Bad channel"
                 continue
             if fragment.cookie != 0 and fragment.cookie != self.cookie:
+                if self.packetdebug:
+                    print "Bad cookie local=%x (seed_local=%x, seed_remote=%x) remote=%x" % (
+                        self.cookie, self.cookie_local_index,
+                        self.cookie_remote_index, fragment.cookie
+                    )
                 continue
 
             # Join fragments
@@ -277,11 +328,15 @@ class ExtCtrl(object):
                     if packet.index == self.ack:
                         self.ack = None
                         self.cond.notify_all()
+                    else:
+                        if self.packetdebug:
+                            print "Unexpected ack %x (exp: %x)" % (packet.index, self.ack)
                     if packet.kind == kind_INIT and self.remote_index == None:
                         decoder = Decoder(packet.data)
                         with self.cond:
                             self.remote_index = decoder.decode_uint32()
-                            self.cookie = self.remote_index ^ self.local_index
+                            self.cookie_remote_index = self.remote_index
+                            self.cookie_local_index = self.local_index
                             self.cond.notify_all()
             elif packet.kind == kind_INIT:
                 remote_index = packet.index
@@ -289,7 +344,8 @@ class ExtCtrl(object):
                     # Save 
                     with self.cond:
                         self.remote_index = remote_index
-                        self.cookie = self.remote_index ^ self.local_index
+                        self.cookie_remote_index = self.remote_index
+                        self.cookie_local_index = self.local_index
                         self.cond.notify_all()                        
                 if remote_index == self.remote_index:
                     # ACK first INIT or its resends
@@ -322,6 +378,11 @@ class ExtCtrl(object):
             while self.cookie == None:
                 self.cond.wait(1)
 
+    def send(self, data):
+        if self.packetdebug:
+            print "Sending", Packet(data)
+        self.ethernet.send(data)
+
     def send_INIT(self):
         flags = kind_INIT | flag_NEED_ACK
         fragmenter = Fragmenter(destination=self.other,
@@ -331,7 +392,7 @@ class ExtCtrl(object):
                                 index=self.local_index)
         for i in self.retries(self.local_index):
             for f in fragmenter.fragments(max_length=self.max_length):
-                self.ethernet.send(f.getvalue())
+                self.send(f.getvalue())
         self.next_index()
 
     def send_INIT_ACK(self):
@@ -343,7 +404,7 @@ class ExtCtrl(object):
                                 index=self.remote_index)
         fragmenter.encode_uint32(self.local_index)
         for f in fragmenter.fragments(max_length=self.max_length):
-            self.ethernet.send(f.getvalue())
+            self.send(f.getvalue())
         
     def send_ACK(self, packet):
         flags = packet.kind | flag_IS_ACK
@@ -353,7 +414,7 @@ class ExtCtrl(object):
                                 cookie=packet.cookie,
                                 index=packet.index)
         for f in fragmenter.fragments(max_length=self.max_length):
-            self.ethernet.send(f.getvalue())
+            self.send(f.getvalue())
         
     def send_DATA_with_ack(self, data):
         flags = kind_DATA | flag_NEED_ACK
@@ -365,7 +426,7 @@ class ExtCtrl(object):
         fragmenter.write(data)
         for i in self.retries(self.local_index):
             for f in fragmenter.fragments(max_length=self.max_length):
-                self.ethernet.send(f.getvalue())
+                self.send(f.getvalue())
         self.next_index()
 
     def send_DATA(self, data):
@@ -377,7 +438,7 @@ class ExtCtrl(object):
                                 index=self.local_index)
         fragmenter.write(data)
         for f in fragmenter.fragments(max_length=self.max_length):
-            self.ethernet.send(f.getvalue())
+            self.send(f.getvalue())
         self.next_index()
 
     def writer(self):