initial labcomm client based on example_client

catkin_ws/src/gbs/scripts/lc_client.py

+#!/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