started reorganizing startup + small fixes (cc689335) · Commits · Mathias Haage / GBS

catkin_ws/src/gbs/scripts/lc_client.py

+54 −31

Original line number	Original line	Diff line number	Diff line
	@@ -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):

	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):
	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):
	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):
	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):
	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):
	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):
	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):
	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)