Commit 8b73f0a0 authored by Sven Gestegård Robertz's avatar Sven Gestegård Robertz
Browse files

initial labcomm client based on example_client

parent 884ce343
#!/usr/bin/python
import labcomm2014
import commands
import sys,traceback
import collections
import threading
import socket
import rwsocket
COMMAND_RECEIVED = 0
RESP_ACK = 1
RESP_READ_PTP = 2
RESP_MOVE_MEASURE= 3
GBS_ERROR = -1
WHAT = 0 #placeholder to be checked/changed
class ResponseMismatch(Exception):
def __init__(self, expected, received):
self.expected = expected
self.received = received
def __str__(self):
return "Unexpected response: expected %s, got %s" % (self.expected, self.received)
class test_client(object):
def add_response(self, type, val):
self.response.acquire()
self.responses.append((type,val))
self.response.notifyAll()
self.response.release()
raise NotImplementedError()
def wait_response(self, type):
self.response.acquire()
while len(self.responses) < 1:
self.response.wait()
(t,res) = self.responses.popleft()
self.response.release()
if t == type:
return res
else:
raise ResponseMismatch(type,t)
def __init__(self, server):
#conditions for awaiting GBS responses
self.response = threading.Condition()
self.responses = collections.deque()
self.d = labcomm2014.Decoder(labcomm2014.StreamReader(server), version)
self.e = labcomm2014.Encoder(labcomm2014.StreamWriter(server), version)
self.e.add_decl(commands.CMD_MOVE_LIN.signature)
self.e.add_decl(commands.CMD_MOVE_PTP.signature)
self.e.add_decl(commands.CMD_NO_OPERATION.signature)
self.e.add_decl(commands.CMD_READ_LIB.signature)
self.e.add_decl(commands.CMD_READ_PTP.signature)
self.e.add_decl(commands.CMD_LCK_TL.signature)
self.e.add_decl(commands.CMD_UNLCK_TL.signature)
self.e.add_decl(commands.CMD_NAIL_LIN.signature)
self.e.add_decl(commands.CMD_MOVE_MEASURE.signature)
self.e.add_decl(commands.CMD_MOVE_ARC.signature)
self.e.add_decl(commands.CMD_MOVE.signature)
self.d.register_handler(commands.COMMAND_RECEIVED.signature, self.handle_COMMAND_RECEIVED)
self.d.register_handler(commands.RESP_ACK.signature, self.handle_RESP_ACK)
self.d.register_handler(commands.RESP_READ_PTP.signature, self.handle_RESP_READ_PTP)
self.d.register_handler(commands.RESP_MOVE_MEASURE.signature, self.handle_RESP_MOVE_MEASURE)
self.e.add_decl(commands.shutdown.signature)
self.d.register_handler(commands.shutdown.signature, self.handle_shutdown)
self.cmd_seq=0
self.run=True
def handle_COMMAND_RECEIVED(self, val):
print "got COMMAND_RECEIVED: %s" % val
self.add_response(COMMAND_RECEIVED, val)
def handle_RESP_NAK(self, val):
print "got RESP_NAK: %s" % val
self.add_response(RESP_NAK, val)
def handle_RESP_ACK(self, val):
print "got RESP_ACK: %s" % val
self.add_response(RESP_ACK, val)
def handle_RESP_READ_PTP(self, val):
print "got RESP_READ_PTP: %s" % val
self.add_response(RESP_READ_PTP, val)
def handle_RESP_MOVE_MEASURE(self, val):
print "got RESP_MOVE_MEASURE: %s" % val
self.add_response(RESP_MOVE_MEASURE, val)
def handle_shutdown(self, val):
print "got shutdown: %s" % val
self.run=False
def send_command(self, cmd, sig):
self.e.encode(cmd, sig)
self.cmd_seq+=1
def shutdown(self):
self.send_command(self.cmd_seq, commands.shutdown.signature)
def CMD_MOVE_LIN(self, coordinate, angle, speed, acc):
self.send_command(dict(seq=self.cmd_seq,
cmd=dict(pos=dict(
pos=coordinate,
rot=angle),
S_Ac=dict(speed=speed, acc=acc))),
commands.CMD_MOVE_LIN.signature)
res = self.wait_response(COMMAND_RECEIVED)
print("... COMMAND_RECEIVED: %s"%res)
res = self.wait_response(RESP_ACK)
print("... RESP_ACK: %s"%res)
res = self.wait_response(RESP_ACK)
print("... RESP_ACK: %s"%res)
return WHAT
def CMD_MOVE_PTP(self, j, s, a):
self.send_command(dict(seq=self.cmd_seq,
cmd=dict(j=j,
S_Ac=dict(speed=s, acc=a))),
commands.CMD_MOVE_PTP.signature)
res = self.wait_response(COMMAND_RECEIVED)
print("... COMMAND_RECEIVED: %s"%res)
res = self.wait_response(RESP_ACK)
print("... RESP_ACK: %s"%res)
res = self.wait_response(RESP_ACK)
print("... RESP_ACK: %s"%res)
return WHAT
def CMD_NO_OPERATION(self):
self.send_command(self.cmd_seq, commands.CMD_NO_OPERATION.signature)
def CMD_READ_LIB(self):
self.send_command(self.cmd_seq, commands.CMD_READ_LIB.signature)
def CMD_READ_PTP(self):
self.send_command(self.cmd_seq, commands.CMD_READ_PTP.signature)
res = self.wait_response(COMMAND_RECEIVED)
print("... COMMAND_RECEIVED: %s"%res)
res = self.wait_response(RESP_READ_PTP)
print("... RESP_READ_PTP: %s"%res)
return res.val
def CMD_LCK_TL(self):
self.send_command(self.cmd_seq, commands.CMD_LCK_TL.signature)
res = self.wait_response(COMMAND_RECEIVED)
print("... COMMAND_RECEIVED: %s"%res)
res = self.wait_response(RESP_ACK)
print("... RESP_ACK: %s"%res)
#
# ---
# float32 err
def CMD_UNLCK_TL(self):
self.send_command(self.cmd_seq, commands.CMD_UNLCK_TL.signature)
res = self.wait_response(COMMAND_RECEIVED)
print("... COMMAND_RECEIVED: %s"%res)
res = self.wait_response(RESP_ACK)
print("... RESP_ACK: %s"%res)
def CMD_NAIL_LIN(self, id, bstop, start, finish, speed, acc):
self.send_command(dict(seq=self.cmd_seq,
id=id,
bstop=bstop,
S_Ac=dict(speed=speed, acc=acc),
start=start,
finish=finish),
commands.CMD_NAIL_LIN.signature)
res = self.wait_response(COMMAND_RECEIVED)
print("... COMMAND_RECEIVED: %s"%res)
res = self.wait_response(RESP_ACK)
print("... RESP_ACK: %s"%res)
res = self.wait_response(RESP_ACK)
print("... RESP_ACK: %s"%res)
def CMD_MOVE_MEASURE(self, id, coordinate, angle, speed, acc):
self.send_command(dict(seq=self.cmd_seq,
id=id,
pos=dict(pos=coordinate, rot=angle),
S_Ac=dict(speed=speed, acc=acc)),
commands.CMD_MOVE_MEASURE.signature)
res = self.wait_response(COMMAND_RECEIVED)
print("... COMMAND_RECEIVED: %s"%res)
res = self.wait_response(RESP_ACK)
print("... RESP_ACK: %s"%res)
res = self.wait_response(RESP_ACK)
print("... RESP_ACK: %s"%res)
res = self.wait_response(RESP_MOVE_MEASURE)
print("... RESP_MOVE_MEASURE: %s"%res)
def CMD_MOVE_ARC(self, posI, posF, speed, acc):
self.send_command(dict(seq=self.cmd_seq,
cmd=dict(
posI=posI,
posF=posF,
S_Ac=dict(speed=speed, acc=acc))),
commands.CMD_MOVE_ARC.signature)
res = self.wait_response(COMMAND_RECEIVED)
print("... COMMAND_RECEIVED: %s"%res)
res = self.wait_response(RESP_ACK)
print("... RESP_ACK: %s"%res)
res = self.wait_response(RESP_ACK)
print("... RESP_ACK: %s"%res)
#TODO
def CMD_MOVE(self, moves):
self.send_command(dict(seq=self.cmd_seq,
cmd=moves),
commands.CMD_MOVE.signature)
def run_one(self):
self.d.runOne();
return self.run
def receiver(client):
run = True
while run:
run = client.run_one()
if __name__ == "__main__":
port = sys.argv[1] if len(sys.argv) >= 2 else 9876
version = sys.argv[2] if len(sys.argv) == 3 else "LabComm2014.v1"
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server.connect((socket.gethostname(), port))
client = test_client(server)
t = threading.Thread(target=receiver, args=(client,))
t.start()
try:
client.CMD_UNLCK_TL()
client.CMD_LCK_TL()
coord = dict(x=0,y=1,z=2)
rot = dict(c=0,b=0,a=0)
client.CMD_MOVE_LIN(coord, rot, 1, 0.1)
client.CMD_MOVE_PTP(range(6), 1, 0.1)
client.CMD_READ_PTP()
coord = dict(x=0,y=1,z=2)
rot = dict(c=0,b=0,a=0)
client.CMD_MOVE_MEASURE(1, coord, rot, 1, 0.1)
start = dict(pos=dict(x=0,y=1,z=2),
rot=dict(c=0,b=0,a=0))
finish = dict(pos=dict(x=0,y=1,z=2),
rot=dict(c=0,b=0,a=0))
client.CMD_NAIL_LIN(1, False, start, finish, 1, 0.1)
posI = dict(pos=dict(x=0,y=1,z=2),
rot=dict(c=0,b=0,a=0))
posF = dict(pos=dict(x=0,y=1,z=2),
rot=dict(c=0,b=0,a=0))
client.CMD_MOVE_ARC(posI, posF, 1, 0.1)
#linear
move1 = dict(pos=dict(pos=dict(x=0,y=1,z=2),
rot=dict(c=0,b=0,a=0)),
S_Ac=dict(speed=1,
acc=2))
move2 = dict(pos=dict(pos=dict(x=5,y=1,z=0),
rot=dict(c=0,b=0,a=0)),
S_Ac=dict(speed=1,
acc=2))
#ptp
move3 = dict(j=[0, 0, 0, 0, 0, 0],
S_Ac = dict (speed=0.1, acc=0.5))
#arc
move4 = dict(posI=dict(pos=dict(x=2,y=4,z=5),
rot=dict(c=0,b=0,a=1)),
posF=dict(pos=dict(x=0,y=1,z=0.5),
rot=dict(c=0,b=1,a=0)),
S_Ac = dict(speed=0.4, acc=1))
#example of how to use variable length lists to make a "union"
moves = [dict(move_lin=[move1],move_ptp=[],move_arc=[]),
dict(move_lin=[move2],move_ptp=[],move_arc=[]),
dict(move_lin=[],move_ptp=[move3],move_arc=[]),
dict(move_lin=[],move_ptp=[],move_arc=[move4]),]
client.CMD_MOVE(moves)
client.shutdown()
except EOFError:
print "got EOF"
except Exception, e:
print "got Exception"
print e
traceback.print_exc()
class ROS_client(test_client):
#GBSPose p
#float32 s
#float32 ac
#---
#float32 err1
#float32 err2
def handle_gbs_linmove(self, req):
ans = GBSLinMoveResponse()
coordinate = req.p.pos
angle = req.p.ori
speed = req.s
acc = req.ac
try:
res = self.CMD_MOVE_LIN(self, req.p.pos, req.p.ori, req.s, req.ac)
ans.err1 = 0 # XXX
ans.err2 = 0 # XXX
except:
ans.err1 = GBS_ERROR
ans.err2 = 0 # XXX
return ans
# GBSJointPose p
# float32 s
# float32 ac
# ---
# float32 err1
# float32 err2
def handle_gbs_ptpmove(self, req):
ans = GBSPTPMoveResponse()
j = [req.p.j1, req.p.j2, req.p.j3, req.p.j4, req.p.j5, req.p.j6]
try:
res = self.CMD_MOVE_PTP(j, req.s, req.ac)
ans.err1 = 0 # XXX
ans.err2 = 0 # XXX
except:
err1 = GBS_ERROR
err2 = 0 # XXX
return ans
#
# ---
#float32 err
#GBSJointPose p
def handle_gbs_readcurjointpose(self, req):
ans = GBSReadCurJointPoseResponse()
try:
res = self.CMD_READ_PTP()
ans.err = 0 # XXX
ans.p.j1 = res[0]
ans.p.j2 = res[1]
ans.p.j3 = res[2]
ans.p.j4 = res[3]
ans.p.j5 = res[4]
ans.p.j6 = res[5]
except:
ans.err = GBS_ERROR
return ans
#
# ---
# float32 err
def handle_gbs_locktool(self, req):
ans = GBSLockToolResponse()
try:
res = self.CMD_LCK_TL()
ans.err = 0 # XXX
except:
ans.err = GBS_ERROR
return ans
#
# ---
# float32 err
def handle_gbs_unlocktool(self, req):
ans = GBSUnlockToolResponse()
try:
res = self.CMD_UNLCK_TL()
ans.err = 0 # XXX
except:
ans.err = GBS_ERROR
return ans
# GBSPose pi
# GBSPose pf
# float32 s
# float32 ac
# ---
# float32 err1
# float32 err2
def handle_gbs_circmove(self, req):
posI = dict(pos=req.pi.pos, rot=req.pi.ori)
posF = dict(pos=req.pf.pos, rot=req.pf.ori)
ans = GBSCircMoveResponse()
try:
res = CMD_MOVE_ARC(posI, posF, req.s, req.ac)
ans.err1 = 0 # XXX
ans.err2 = 0 # XXX
except:
ans.err1 = GBS_ERROR
ans.err2 = GBS_ERROR
return ans
# TODO
def handle_gbs_combmove(self, req):
raise NotImplementedError()
# int32 id
# GBSPose p
# float32 s
# float32 ac
# ---
# float32 err1
# float32 err2
def handle_gbs_measuremove(self, req):
ans = GBSMeasureMoveResponse()
coord = req.p.pos
angle = req.p.ori
try:
res = CMD_MOVE_MEASURE(req.id, coord, angle, req.s, req.ac)
ans.err1 = 0 #XXX
ans.err2 = 0 #XXX
except:
ans.err1 = GBS_ERROR
ans.err2 = GBS_ERROR
return ans
# int32 id
# bool bstop
# float32 s
# float32 ac
# GBSPose p1
# GBSPose p2
# ---
# float32 err1
# float32 err2
def handle_gbs_naillinmove(self, req):
ans = GBSNailLinMoveResponse()
start = dict(pos=req.p1.pos, rot=req.p1.ori)
finish = dict(pos=req.p2.pos, rot=req.p2.ori)
try:
res = CMD_NAIL_LIN(req.id, req.bstop, start, finish, req.s, req.ac)
ans.err1 = 0 #XXX
ans.err2 = 0 #XXX
except:
ans.err1 = GBS_ERROR
ans.err2 = GBS_ERROR
return ans
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment