started reorganizing startup + small fixes

catkin_ws/src/gbs/scripts/lc_client.py

@@ -14,6 +14,7 @@ RESP_READ_PTP    = 2
 RESP_MOVE_MEASURE= 3
 
 GBS_ERROR = -1
+GBS_OK = 0
 WHAT = 0  #placeholder to be checked/changed
 
 class ResponseMismatch(Exception):
@@ -25,31 +26,23 @@ class ResponseMismatch(Exception):
 
 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 __init__(self, host=socket.gethostname(), port=9876,version="LabComm2014.v1"):
+        self.host=host
+        self.port=port
+        self.version=version
 
-    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)
+    def connect(self):
+        server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        server.connect((self.host, self.port))
+        receiver_thread = threading.Thread(target=self.receiver, args=(client,))
+        receiver_thread.start()
+
+        self.d = labcomm2014.Decoder(labcomm2014.StreamReader(server), self.version)
+        self.e = labcomm2014.Encoder(labcomm2014.StreamWriter(server), self.version)
 
         self.e.add_decl(commands.CMD_MOVE_LIN.signature)
         self.e.add_decl(commands.CMD_MOVE_PTP.signature)
@@ -74,6 +67,25 @@ class test_client(object):
         self.cmd_seq=0
         self.run=True
 
+    def close(self):
+        self.shutdown()
+
+    def add_response(self, type, val):
+        self.response.acquire()
+        self.responses.append((type,val))
+        self.response.notifyAll()
+        self.response.release()
+
+    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 handle_COMMAND_RECEIVED(self, val):
         print "got COMMAND_RECEIVED: %s" % val
@@ -120,7 +132,7 @@ class test_client(object):
         print("... RESP_ACK: %s"%res)
         res = self.wait_response(RESP_ACK)
         print("... RESP_ACK: %s"%res)
-        return WHAT
+        return GBS_OK
 
     def CMD_MOVE_PTP(self, j, s, a):
         self.send_command(dict(seq=self.cmd_seq,
@@ -133,12 +145,15 @@ class test_client(object):
         print("... RESP_ACK: %s"%res)
         res = self.wait_response(RESP_ACK)
         print("... RESP_ACK: %s"%res)
-        return WHAT
+        return GBS_OK
 
     def CMD_NO_OPERATION(self):
         self.send_command(self.cmd_seq, commands.CMD_NO_OPERATION.signature)
+        return GBS_OK
     def CMD_READ_LIB(self):
         self.send_command(self.cmd_seq, commands.CMD_READ_LIB.signature)
+        return WHAT
+
     def CMD_READ_PTP(self):
         self.send_command(self.cmd_seq, commands.CMD_READ_PTP.signature)
         res = self.wait_response(COMMAND_RECEIVED)
@@ -153,15 +168,15 @@ class test_client(object):
         print("... COMMAND_RECEIVED: %s"%res)
         res = self.wait_response(RESP_ACK)
         print("... RESP_ACK: %s"%res)
-#
-# ---
-# float32 err
+        return GBS_OK
+
     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)
+        return GBS_OK
 
     def CMD_NAIL_LIN(self, id, bstop, start, finish, speed, acc):
         self.send_command(dict(seq=self.cmd_seq,
@@ -177,6 +192,7 @@ class test_client(object):
         print("... RESP_ACK: %s"%res)
         res = self.wait_response(RESP_ACK)
         print("... RESP_ACK: %s"%res)
+        return GBS_OK
 
     def CMD_MOVE_MEASURE(self, id, coordinate, angle, speed, acc):
         self.send_command(dict(seq=self.cmd_seq,
@@ -192,6 +208,8 @@ class test_client(object):
         print("... RESP_ACK: %s"%res)
         res = self.wait_response(RESP_MOVE_MEASURE)
         print("... RESP_MOVE_MEASURE: %s"%res)
+        ans = dict((k, res[k]) for k in ('x1', 'x2', 'y1'))
+        return ans
 
     def CMD_MOVE_ARC(self, posI, posF, speed, acc):
         self.send_command(dict(seq=self.cmd_seq,
@@ -206,23 +224,28 @@ class test_client(object):
         print("... RESP_ACK: %s"%res)
         res = self.wait_response(RESP_ACK)
         print("... RESP_ACK: %s"%res)
+        return GBS_OK
+
 
 #TODO
     def CMD_MOVE(self, moves):
         self.send_command(dict(seq=self.cmd_seq,
                                cmd=moves),
                           commands.CMD_MOVE.signature)
+        return GBS_OK
+
 
     def run_one(self):
         self.d.runOne();
         return self.run
 
-def receiver(client):
-    run = True
-    while run:
-        run = client.run_one()
+    def receiver(self):
+        run = True
+        while run:
+            run = self.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)
@@ -361,7 +384,7 @@ class ROS_client(test_client):
         except:
             ans.err = GBS_ERROR
         return ans
-# 
+#
 # ---
 # float32 err
     def handle_gbs_locktool(self, req):
@@ -372,7 +395,7 @@ class ROS_client(test_client):
         except:
             ans.err = GBS_ERROR
         return ans
-# 
+#
 # ---
 # float32 err
     def handle_gbs_unlocktool(self, req):