started reorganizing startup + small fixes

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