Python protocol simulator added

.gitignore

+*~
+*.pyc
+__pycache__

simulator/.gitignore

+/extctrl2014_lc.py

simulator/Makefile

+AUTOGEN=extctrl2014_lc.py
+
+VPATH=../common
+
+all: $(AUTOGEN)
+
+%_lc.py: %.lc
+	labcomm2014 --python=$@ $<
+
+.PHONY: clean
+clean:
+	rm -f *~ 
+
+.PHONY: distclean
+distclean: clean
+	rm -f $(AUTOGEN)

simulator/controller.py

+#!/usr/bin/python
+
+import argparse
+import ethernet
+import extctrl2014
+import extctrl2014_lc
+import labcomm2014
+import sys
+
+
+class Joint(extctrl2014_lc.ext2irb_joint_offset):
+
+    def __init__(self,
+                 parKp=0.0,
+                 parKv=0.0,
+                 parKi=0.0,
+                 posOffset=0.0,
+                 velOffset=0.0,
+                 trqFfwOffset=0.0):
+        self.parKp = parKp
+        self.parKv = parKv
+        self.parKi = parKi
+        self.posOffset = posOffset
+        self.velOffset = velOffset
+        self.trqFfwOffset = trqFfwOffset
+
+    def __repr__(self):
+        return 'Joint(posOffset=%f posRef=%f)' % (
+            self.posOffset)
+    
+class Arm(extctrl2014_lc.ext2irb_robot_net):
+
+    def __init__(self, joint, mocgendata):
+        self.joint = joint
+        self.mocgendata = mocgendata
+
+    def __repr__(self):
+        return 'Arm(joint=%s mocgendata=%s)' % (self.joint, self.mocgendata)
+
+class Robot(extctrl2014_lc.ext2irb_net):
+
+    def __init__(self, arms):
+        self.robot = arms
+
+    def __repr__(self):
+        return 'Robot(arms=%s)' % (self.robot)
+
+if __name__ == "__main__":
+    optParser = argparse.ArgumentParser(usage='%(prog)s [options]')
+    optParser.add_argument('--listen',
+                           type=ethernet.parse_address,
+                           action='store',
+                           metavar='MAC',
+                           required=True,
+                           help='ethernet MAC address to listen for')
+    optParser.add_argument('--channel',
+                           type=lambda s: int(s, 0),
+                           action='store',
+                           metavar='CHANNEL',
+                           required=True,
+                           help='CHANNEL to listen for')
+    optParser.add_argument('--interface',
+                           action='store',
+                           metavar='INTERFACE',
+                           required=True,
+                           help='INTERFACE to use for connection')
+    optParser.add_argument('--send-loss',
+                           type=float,
+                           action='store',
+                           metavar='FRACTION',
+                           help='FRACTION of send packets to lose [0..1]')
+    optParser.add_argument('--recv-loss',
+                           type=float,
+                           action='store',
+                           metavar='FRACTION',
+                           help='FRACTION of recv packets to lose [0..1]')
+    optParser.add_argument('-v', '--verbose',
+                           action='store_true',
+                           help='be verbose')
+
+    options = optParser.parse_args(sys.argv[1:])
+
+    eth = ethernet.ETH(options.interface,
+                       send_loss=options.send_loss,
+                       recv_loss=options.recv_loss)
+    extctrl = extctrl2014.ExtCtrl(ethernet=eth,
+                                  channel=options.channel,
+                                  robot=options.listen)
+
+    encoder = labcomm2014.Encoder(extctrl.writer())
+    decoder = labcomm2014.Decoder(extctrl.reader())
+    print encoder, decoder
+    decoder.add_decl(extctrl2014_lc.irb2ext_net.signature)
+    decoder.add_decl(extctrl2014_lc.force_torque_net.signature)
+    encoder.add_decl(extctrl2014_lc.ext2irb_net.signature)
+
+    delta = 0.1
+    offset = 0.0
+    while True:
+        value,decl = decoder.decode()
+        if value != None and decl == extctrl2014_lc.irb2ext_net.signature:
+            offset += delta
+            if offset > 1.0:
+                delta = -delta
+                offset += delta
+            elif offset < -1.0:
+                delta = -delta
+                offset += delta
+            print "ROBOT", value
+            feedback = Robot([ Arm(joint=[ Joint(posOffset=offset),
+                                           Joint(posOffset=offset*2) ],
+                        mocgendata=[])])
+            encoder.encode(feedback,
+                           extctrl2014_lc.ext2irb_net.signature)
+
+    time.sleep(10)

simulator/ethernet.py

+import fcntl
+import socket
+import struct
+import sys
+import random
+
+ETH_P_ALL = 0x0003
+SIOCGIFHWADDR = 0x8927       # Get hardware address
+
+def parse_address(addr):
+    return struct.pack('!6B', *list(int(v, 16) for v in addr.split(':')))
+                       
+class ETH(object):
+
+    def __init__(self, name, send_loss=0.0, recv_loss=0.0):
+        self.name = name
+        self.send_loss = send_loss
+        self.recv_loss = recv_loss
+        self.socket = socket.socket(socket.AF_PACKET,
+                                    socket.SOCK_RAW,
+                                    socket.htons(ETH_P_ALL))
+        self.socket.bind((name, 0x0000))
+        self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 100000)
+        ifs = fcntl.ioctl(self.socket, SIOCGIFHWADDR,
+                          struct.pack("16sH14s",
+                                      self.name.encode(),
+                                      0, "".encode()))
+        self.macaddr = struct.unpack("16x2x14s", ifs)[0][0:6]
+        
+    def send(self, data):
+        u = random.uniform(0, 1.0)
+        if u >= self.send_loss:
+            data = data[0:6] + self.macaddr + data[12:]
+            self.socket.send(data)
+        else:
+            logger.log('Dropped sent packet')
+
+    def recv(self, maxlength):
+        def hexdump(data):
+            pos = 0
+            while len(data):
+                hex = ' '.join(map(lambda c: '%02.2x' % ord(c), data[0:16]))
+                ascii = ''.join(map(lambda c: '%c' %
+                                    (c > ' ' and c <= '\x7f' and c or '.'),
+                                     data[0:16]))
+                print '%04x: %-50s%s' % (pos, hex, ascii)
+                data = data[16:]
+                pos += 16
+        while True:
+            result = self.socket.recv(maxlength)
+            #hexdump(result)
+            u = random.uniform(0, 1.0)
+            if u >= self.send_loss:
+                return result
+            else:
+                logger.log('Dropped received packet')
+
+    def __repr__(self):
+        def mac_to_str(mac):
+            return ":".join(["%02x" % b for b in self.macaddr])
+                
+        return "ETH(%s, %s)" % (self.name, mac_to_str(self.macaddr))
+
+    

simulator/extctrl2014.py

+#!/usr/bin/python
+
+import random
+import struct
+import StringIO
+import time
+import threading
+
+#
+# Packet format
+#
+#   +----+----+----+----+----+----+
+#   | destination                    (6 bytes)
+#   +----+----+----+----+----+----+
+#   | source                         (6 bytes)
+#   +----+----+
+#   | eth_type                       (2 bytes 'EX')
+#   +----+
+#   | kind+flags                     (1 byte)
+#   +....+
+#   | channel                        (packed32)
+#   +----+----+----+----+
+#   | cookie                         (uint32, XOR of both first_indices)
+#   +----+----+----+----+
+#   | index                          (uint32)
+#   +....+
+#   | frag_num                       (packed32)
+#   +....+
+#   | frag_length                    (packed32)
+#   +....+
+#   | data
+#   +
+#
+
+flag_MASK =           0x07
+flag_LAST_FRAG =      0x01
+flag_NEED_ACK =       0x02
+flag_IS_ACK =         0x04
+kind_MASK =           0x30
+kind_INIT =           0x10
+kind_DATA =           0x20
+
+class UnexpectedFragment(Exception):
+
+    pass
+
+class Decoder(object):
+
+    def __init__(self, data):
+        self.buf = StringIO.StringIO(data)
+
+    def read(self, count):
+        return self.buf.read(count)
+    
+    def unpack(self, format):
+        size = struct.calcsize(format)
+        data = self.buf.read(size)
+        result = struct.unpack(format, data)
+        return result[0]
+
+    def decode_uint32(self):
+        return self.unpack("!I")
+
+    def decode_byte(self):
+        return self.unpack("!B")
+    
+    def decode_packed32(self):
+        result = 0
+        while True:
+            tmp = self.decode_byte()
+            result = (result << 7) | (tmp & 0x7f)
+            if (tmp & 0x80) == 0:
+                break
+        return result
+
+class Encoder(object):
+
+    def __init__(self):
+        self.buf = StringIO.StringIO()
+   
+    def __repr__(self):
+        return "Codec([%s])" % (" ".join(
+            map(lambda b: "0x%2.2x" % ord(b), self.getvalue())))
+
+    def getvalue(self):
+        return self.buf.getvalue()
+    
+    def write(self, data):
+        self.buf.write(data)
+        
+    def pack(self, format, *args):
+        self.buf.write(struct.pack(format, *args))
+
+    def encode_uint32(self, value):
+        self.pack("!I", value)
+
+    def encode_byte(self, value):
+        self.pack("!B", value)
+    
+    def encode_packed32(self, value):
+        value = value & 0xffffffff
+        tmp = [ value & 0x7f ]
+        value = value >> 7
+        while value:
+            tmp.append(value & 0x7f | 0x80)
+            value= value >> 7
+            pass
+        for c in reversed(tmp):
+            self.encode_byte(c) 
+            pass
+        pass
+
+class Fragmenter(Encoder):
+
+    def __init__(self, destination, flags, channel, cookie, index):
+        super(Fragmenter, self).__init__()
+        self.destination = destination
+        self.flags = flags
+        self.cookie = cookie
+        self.channel = channel
+        self.index = index
+
+    def fragments(self, max_length=1480):
+        data = self.getvalue()
+        fragment_number = 0
+        while len(data) > 0 or fragment_number == 0:
+            length = min(len(data), max(1, max_length))
+            remaining = data[length:]
+            data = data[0:length]
+            fragment = Encoder()
+            fragment.write(self.destination)
+            fragment.write('\x00\x00\x00\x00\x00\x00')
+            fragment.write('EX')
+            if len(remaining) == 0:
+                fragment.encode_byte(self.flags | flag_LAST_FRAG)
+            else:
+                fragment.encode_byte(self.flags)
+            fragment.encode_packed32(self.channel)
+            fragment.encode_uint32(self.cookie)
+            fragment.encode_uint32(self.index)
+            fragment.encode_packed32(fragment_number)
+            fragment.encode_packed32(length)
+            fragment.write(data)
+            yield fragment
+            data = remaining
+            fragment_number += 1
+            
+    def __repr__(self):
+        return "Fragmenter([%s])" % (" ".join(
+            map(lambda b: "0x%2.2x" % ord(b), self.getvalue())))
+
+class Packet:
+
+    def __init__(self, data):
+        buf = Decoder(data)
+        self.destination = buf.read(6)
+        self.source = buf.read(6)
+        self.eth_type = buf.read(2)
+        flags = buf.decode_byte()
+        self.flags = flags & flag_MASK
+        self.kind = flags & kind_MASK
+        self.channel = buf.decode_packed32()
+        self.cookie = buf.decode_uint32()
+        self.index = buf.decode_uint32()
+        self.frag_num = buf.decode_packed32()
+        frag_length = buf.decode_packed32()
+        self.data = buf.read(frag_length)
+
+    def join(self, other):
+        if self.destination != other.destination:
+            return None
+        if self.source != other.source:
+            return None
+        if self.kind != other.kind:
+            return None
+        if self.channel != other.channel:
+            return None
+        if self.cookie != other.cookie:
+            return None
+        if self.index != other.index:
+            return None
+        if self.frag_num + 1 != other.frag_num:
+            return None
+        self.flags = other.flags
+        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)),
+            self.channel,
+            self.index,
+            self.flags,
+            self.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):
+        if len(filter(None, [ robot, controller])) != 1:
+            raise Exception('Either robot(%s) or controller(%s) '
+                            'should be defined' %
+                            (robot, controller))
+        else:
+            self.other = filter(None, [ robot, controller])[0]
+        self.ethernet = ethernet
+        self.channel = channel
+        self.controller = controller
+        self.robot = robot
+        self.max_retries = max_retries
+        self.max_length = max_length
+        self.read_data = list()
+        self.cond = threading.Condition()
+        self.local_index = random.randint(0,2**32-1)
+        self.remote_index = None
+        self.cookie = None
+        self.ack = None
+        t = threading.Thread(target=self.recv_loop)
+        t.daemon = True
+        t.start()
+        if self.controller:
+            self.send_INIT()
+        elif self.robot:
+            self.await_INIT()
+        
+
+    def recv_loop(self):
+        ack = None
+        data = None
+        while True:
+            fragment = Packet(self.ethernet.recv(2000))
+
+            # Sanity check fragment
+            if fragment.eth_type != 'EX':
+                continue
+            if fragment.source != self.other:
+                continue
+            if fragment.channel != self.channel:
+                continue
+            if fragment.cookie != 0 and fragment.cookie != self.cookie:
+                continue
+
+            # Join fragments
+            if fragment.flags & flag_IS_ACK != 0:
+                if fragment.frag_num == 0:
+                    ack = fragment
+                elif ack != None:
+                    ack = ack.join(fragment)
+                else:
+                    ack = None
+                packet = ack
+            else:
+                if fragment.frag_num == 0:
+                    data = fragment
+                elif data != None:
+                    data = data.join(fragment)
+                else:
+                    data = None
+                packet = data
+
+            if packet == None:
+                # Join failed
+                continue
+            if fragment.flags & flag_LAST_FRAG == 0:
+                # Await more fragments
+                continue
+            
+            if packet.flags & flag_IS_ACK != 0:
+                with self.cond:
+                    if packet.index == self.ack:
+                        self.ack = None
+                        self.cond.notify_all()
+                    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.cond.notify_all()
+            elif packet.kind == kind_INIT:
+                remote_index = packet.index
+                if self.remote_index == None:
+                    # Save 
+                    with self.cond:
+                        self.remote_index = remote_index
+                        self.cookie = self.remote_index ^ self.local_index
+                        self.cond.notify_all()                        
+                if remote_index == self.remote_index:
+                    # ACK first INIT or its resends
+                    self.send_INIT_ACK()
+            elif packet.kind == kind_DATA:
+                if self.remote_index != packet.index:
+                    with self.cond:
+                        self.remote_index = packet.index
+                        self.read_data.extend(packet.data)
+                        self.cond.notify_all()
+                if packet.flags & flag_NEED_ACK:
+                    self.send_ACK(packet)
+
+    def retries(self, index, timeout=1):
+        with self.cond:
+            self.ack = index
+        i = 0
+        while i < self.max_retries:
+            yield i
+            i += 1
+            with self.cond:
+                if self.ack == index:
+                    self.cond.wait(timeout)
+                if self.ack != index:
+                    raise StopIteration()
+        raise Exception()
+
+    def await_INIT(self):
+        with self.cond:
+            while self.cookie == None:
+                self.cond.wait(1)
+
+    def send_INIT(self):
+        flags = kind_INIT | flag_NEED_ACK
+        fragmenter = Fragmenter(destination=self.other,
+                                channel=self.channel,
+                                flags=flags,
+                                cookie=0,
+                                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.local_index += 1
+
+    def send_INIT_ACK(self):
+        flags = kind_INIT | flag_IS_ACK
+        fragmenter = Fragmenter(destination=self.other,
+                                channel=self.channel,
+                                flags=flags,
+                                cookie=0,
+                                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())
+        
+    def send_ACK(self, packet):
+        flags = packet.kind | flag_IS_ACK
+        fragmenter = Fragmenter(destination=self.other,
+                                channel=self.channel,
+                                flags=flags,
+                                cookie=packet.cookie,
+                                index=packet.index)
+        for f in fragmenter.fragments(max_length=self.max_length):
+            self.ethernet.send(f.getvalue())
+        
+    def send_DATA_with_ack(self, data):
+        flags = kind_DATA | flag_NEED_ACK
+        fragmenter = Fragmenter(destination=self.other,
+                                channel=self.channel,
+                                flags=flags,
+                                cookie=self.cookie,
+                                index=self.local_index)
+        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.local_index += 1
+
+    def send_DATA(self, data):
+        flags = kind_DATA
+        fragmenter = Fragmenter(destination=self.other,
+                                channel=self.channel,
+                                flags=flags,
+                                cookie=self.cookie,
+                                index=self.local_index)
+        fragmenter.write(data)
+        for f in fragmenter.fragments(max_length=self.max_length):
+            self.ethernet.send(f.getvalue())
+        self.local_index += 1
+
+    def writer(self):
+        class Writer:
+            def __init__(self, extctrl):
+                self.extctrl = extctrl
+                
+            def mark_begin(self, decl, value):
+                self.data = ""
+
+            def mark_end(self, decl, value):
+                if value == None:
+                    self.extctrl.send_DATA_with_ack(data=self.data)
+                else:
+                    self.extctrl.send_DATA(data=self.data)
+                
+            def write(self, data):
+                self.data += data
+                pass
+                
+        return Writer(self)
+    
+    def reader(self):
+        class Reader:
+