diff --git a/catkin_ws/src/gbs/scripts/lc_client.py b/catkin_ws/src/gbs/scripts/lc_client.py index 398f96128ceb4fd6095dd4cfd74e6e2e18b41fe5..389ceed7e3dc517f788f97762becdab64b0a2624 100755 --- a/catkin_ws/src/gbs/scripts/lc_client.py +++ b/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):