Commit ec9ee822 authored by Anders Blomdell's avatar Anders Blomdell
Browse files

Python protocol simulator added

parent 83ed1ca1
*~
*.pyc
__pycache__
/extctrl2014_lc.py
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)
#!/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)
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))
#!/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: