From ec9ee822dae04789e83e36554c9ecbfc0dfcb49e Mon Sep 17 00:00:00 2001
From: Anders Blomdell <anders.blomdell@control.lth.se>
Date: Thu, 26 May 2016 17:58:05 +0200
Subject: [PATCH] Python protocol simulator added
---
.gitignore | 3 +
simulator/.gitignore | 1 +
simulator/Makefile | 16 ++
simulator/controller.py | 116 +++++++++++
simulator/ethernet.py | 64 ++++++
simulator/extctrl2014.py | 418 +++++++++++++++++++++++++++++++++++++++
simulator/robot.py | 151 ++++++++++++++
simulator/run_test | 33 ++++
8 files changed, 802 insertions(+)
create mode 100644 .gitignore
create mode 100644 simulator/.gitignore
create mode 100644 simulator/Makefile
create mode 100755 simulator/controller.py
create mode 100644 simulator/ethernet.py
create mode 100644 simulator/extctrl2014.py
create mode 100755 simulator/robot.py
create mode 100755 simulator/run_test
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..b71676b
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+*~
+*.pyc
+__pycache__
diff --git a/simulator/.gitignore b/simulator/.gitignore
new file mode 100644
index 0000000..83841b1
--- /dev/null
+++ b/simulator/.gitignore
@@ -0,0 +1 @@
+/extctrl2014_lc.py
diff --git a/simulator/Makefile b/simulator/Makefile
new file mode 100644
index 0000000..0e9701f
--- /dev/null
+++ b/simulator/Makefile
@@ -0,0 +1,16 @@
+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)
diff --git a/simulator/controller.py b/simulator/controller.py
new file mode 100755
index 0000000..ffcc89b
--- /dev/null
+++ b/simulator/controller.py
@@ -0,0 +1,116 @@
+#!/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)
diff --git a/simulator/ethernet.py b/simulator/ethernet.py
new file mode 100644
index 0000000..259cc7d
--- /dev/null
+++ b/simulator/ethernet.py
@@ -0,0 +1,64 @@
+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))
+
+
diff --git a/simulator/extctrl2014.py b/simulator/extctrl2014.py
new file mode 100644
index 0000000..56e5631
--- /dev/null
+++ b/simulator/extctrl2014.py
@@ -0,0 +1,418 @@
+#!/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:
+ def __init__(self, extctrl):
+ self.extctrl = extctrl
+
+ def mark(self, decl, value):
+ pass
+
+ def read(self, count):
+ with self.extctrl.cond:
+ while len(self.extctrl.read_data) == 0:
+ self.extctrl.cond.wait()
+ data = self.extctrl.read_data[0:count]
+ self.extctrl.read_data = self.extctrl.read_data[count:]
+ return ''.join(data)
+
+ return Reader(self)
+
+
diff --git a/simulator/robot.py b/simulator/robot.py
new file mode 100755
index 0000000..4aec359
--- /dev/null
+++ b/simulator/robot.py
@@ -0,0 +1,151 @@
+#!/usr/bin/python
+
+import argparse
+import ethernet
+import extctrl2014
+import extctrl2014_lc
+import labcomm2014
+import sys
+import threading
+import time
+
+class Joint(extctrl2014_lc.irb2ext_joint_abs):
+
+ def __init__(self,
+ parKp=0.0,
+ parKv=0.0,
+ parKi=0.0,
+ parTrqMin=0.0,
+ parTrqMax=0.0,
+ posRawAbs=0.0,
+ posRawFb=0.0,
+ posFlt=0.0,
+ velRaw=0.0,
+ velFlt=0.0,
+ velOut=0.0,
+ trqRaw=0.0,
+ trqRefFlt=0.0,
+ posRef=0.0,
+ velRef=0.0,
+ trqFfw=0.0,
+ trqFfwGrav=0.0):
+ self.parKp = parKp
+ self.parKv = parKv
+ self.parKi = parKi
+ self.parTrqMin = parTrqMin
+ self.parTrqMax = parTrqMax
+ self.posRawAbs = posRawAbs
+ self.posRawFb = posRawFb
+ self.posFlt = posFlt
+ self.velRaw = velRaw
+ self.velFlt = velFlt
+ self.velOut = velOut
+ self.trqRaw = trqRaw
+ self.trqRefFlt = trqRefFlt
+ self.posRef = posRef
+ self.velRef = velRef
+ self.trqFfw = trqFfw
+ self.trqFfwGrav = trqFfwGrav
+
+ def __repr__(self):
+ return 'Joint(posRaw=%f posRef=%f)' % (
+ self.posRawAbs, self.posRef)
+
+class Arm(extctrl2014_lc.irb2ext_robot_net):
+
+ def __init__(self, kind, joint, mocgendata):
+ self.kind = kind
+ self.joint = joint
+ self.mocgendata = mocgendata
+
+ def __repr__(self):
+ return 'Arm(kind=%s joint=%s mocgendata=%s)' % (
+ self.kind, self.joint, self.mocgendata)
+
+class Robot(extctrl2014_lc.irb2ext_net):
+
+ def __init__(self, arms):
+ self.obtaining = False
+ self.manualMode = False
+ self.controlActive = False
+ self.robot = arms
+
+ def __repr__(self):
+ return 'Robot(obtaining=%s manual=%s active=%s arms=%s)' % (
+ self.obtaining, self.manualMode, self.controlActive, self.robot)
+
+if __name__ == '__main__':
+ optParser = argparse.ArgumentParser(usage='%(prog)s [options]')
+ optParser.add_argument('--connect',
+ type=ethernet.parse_address,
+ action='store',
+ metavar='MAC',
+ required=True,
+ help='ethernet MAC address to connect to')
+ optParser.add_argument('--channel',
+ type=lambda s: int(s, 0),
+ action='store',
+ metavar='CHANNEL',
+ required=True,
+ help='CHANNEL to connect to')
+ 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:])
+
+ print options
+ eth = ethernet.ETH(options.interface,
+ send_loss=options.send_loss,
+ recv_loss=options.recv_loss)
+ extctrl = extctrl2014.ExtCtrl(ethernet=eth,
+ channel=options.channel,
+ controller=options.connect)
+ encoder = labcomm2014.Encoder(extctrl.writer())
+ decoder = labcomm2014.Decoder(extctrl.reader())
+ print encoder, decoder
+
+ decoder.add_decl(extctrl2014_lc.ext2irb.signature)
+ encoder.add_decl(extctrl2014_lc.irb2ext_net.signature)
+ encoder.add_decl(extctrl2014_lc.force_torque_net.signature)
+
+ robot = Robot([ Arm(kind=0,
+ joint=[ Joint(), Joint() ],
+ mocgendata=[])])
+ def run_decoder():
+ while True:
+ value,decl = decoder.decode()
+ if value != None and decl == extctrl2014_lc.ext2irb_net.signature:
+ print 'FEEDBACK', value
+ robot.robot[0].joint[0].posRawFb = (
+ robot.robot[0].joint[0].posRef +
+ value.robot[0].joint[0].posOffset)
+ robot.robot[0].joint[1].posRawFb = (
+ robot.robot[0].joint[1].posRef +
+ value.robot[0].joint[1].posOffset)
+ print (robot.robot[0].joint[0].posRawFb,
+ robot.robot[0].joint[1].posRawFb)
+ t = threading.Thread(target=run_decoder)
+ t.daemon = True
+ t.start()
+ for i in range(100):
+ time.sleep(0.1)
+ encoder.encode(robot,
+ extctrl2014_lc.irb2ext_net.signature)
+ # Let connection timeout (whenever that gets implemented)
+ time.sleep(10)
diff --git a/simulator/run_test b/simulator/run_test
new file mode 100755
index 0000000..178bfcc
--- /dev/null
+++ b/simulator/run_test
@@ -0,0 +1,33 @@
+#!/bin/sh
+
+set -x
+
+CHANNEL=0x12
+
+./controller.py --interface enp4s0f0 \
+ --listen 00:15:17:79:10:89 \
+ --channel ${CHANNEL} \
+ --send-loss 0.0 \
+ --recv-loss 0.0 \
+ -v &
+#> /tmp/controller.out 2>&1 &
+CONTROLLER=$!
+sleep 0.5
+./robot.py --interface enp4s0f1 \
+ --connect 00:15:17:79:10:88 \
+ --channel ${CHANNEL} \
+ --send-loss 0.0 \
+ --recv-loss 0.0 \
+ -v &
+#> /tmp/robot.out 2>&1 &
+ROBOT=$!
+
+stop_children() {
+ kill ${CONTROLLER} ${ROBOT}
+}
+trap stop_children EXIT
+
+sleep 30
+
+
+
--
GitLab