opencv/ev3/Ev3Server.py

import sys
import time
import signal
import struct
import socket
import select

from threading import Thread

class LocalServer(object):
	def __init__(self, ip, port):
		self.thread = None
		self.cancel = False
		print ("Establish connection to " + ip + ':' + str(port))
		self.sockTcpEv3 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
		self.sockTcpEv3.settimeout(1.0)
		self.sockTcpEv3.connect((ip, port))
		print ("EV3 connected")
		self.sockTcpEv3.send(b'ET /target?sn=')
		data = self.sockTcpEv3.recv(1024)
		dataStr = data.decode("utf-8")
		if "Accept:EV340" not in dataStr:
			raise Exception("EV3 has not accepted connection")

		print ("EV3 has accepted connection")

	def __del__(self):
		self.stop()

	def start(self):
		if self.thread is None:
			self.thread = Thread(target=self.run)
			self.cancel = False
			self.thread.start()

	def stop(self):
		if self.thread is not None:
			self.cancel = True
			self.thread.join()
			self.thread = None

	def run(self):
		# create local socket
		localListenAddr = ('0.0.0.0', 5555)
		print ("Starting local server on " + str(localListenAddr[0]) + ':' + str(localListenAddr[1]))
		sockTcpLocal = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
		sockTcpLocal.settimeout(1.0)
		sockTcpLocal.bind(localListenAddr)
		sockTcpLocal.listen(1)
		
		print ("Listening")
		while not self.cancel:	# Listening loop	
			try:
				conn, addr = sockTcpLocal.accept()
				print ('Connection opened:' + str(addr))
				connOpen = True
				while not self.cancel and connOpen:	# Data loop
					try:
						inputready,outputready,exceptready = select.select([conn, self.sockTcpEv3], [], [], 1.0)
						for sock in inputready:
							if sock == conn:
								data = sock.recv(1024)
								if data:
#									print ("Client: Received data (" + str(len(data)) + ")")
									self.sockTcpEv3.send(data)  # send to EV3
								else:
									print ('Client: Connection closed:' + str(addr))
									connOpen = False
							if sock == self.sockTcpEv3:
								data = sock.recv(1024)
								if data:
#									print ("EV3: Received data (" + str(len(data)) + ")")
									conn.send(data)  # send to Client
								else:
									print ('EV3: Connection closed:' + str(addr))
									connOpen = False
					except:
#						print ("Receive: Timeout")
						pass

				conn.close()		
			except:
#				print ("Listening: Timeout")
				pass
		
		self.sockTcpEv3.close()
		print ("Shutdown local server on " + str(localListenAddr[0]) + ':' + str(localListenAddr[1]))

if ("__main__" == __name__):
	UDP_IP = "0.0.0.0"
	UDP_PORT = 3015
	sockUdp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
	sockUdp.settimeout(30.0)
	sockUdp.bind((UDP_IP, UDP_PORT))
	localTcpServer = None
	try:
		while True:
			try:
				data, addr = sockUdp.recvfrom(1024) # buffer size is 1024 bytes
			except (socket.timeout):
				print ("UDP: Timeout")
				if localTcpServer is not None:
					localTcpServer.stop()
					del localTcpServer
					localTcpServer = None
				continue
	#		print ("Received from " + str(addr))
	#		print ("Message (" + str(len(data)) + ") :")
			dataStr = data.decode("utf-8")
			keyValueStrList = dataStr.replace('\r', '').replace(' ', '').split('\n')

			keyValueDict = {}
			for keyValueStr in keyValueStrList:
				keyValue = keyValueStr.split(':')
				if len(keyValue) == 2:
					keyValueDict[keyValue[0]] = keyValue[1]

	#		print (keyValueDict)
			if keyValueDict['Protocol'] == 'EV3':
				if localTcpServer is None:
					sockUdp.sendto(b'A', addr)
					print ("EV3 discovered")
					try:
						time.sleep(0.2)
						localTcpServer = LocalServer(addr[0], int(keyValueDict['Port']))
						localTcpServer.start()
					except Exception as e:
						print (e)
						if localTcpServer is not None:
							del localTcpServer
							localTcpServer = None
	except (KeyboardInterrupt):
		print ("\nKeyboardInterrupt")
		
	if localTcpServer is not None:
		localTcpServer.stop()
		del localTcpServer
		localTcpServer = None