jens
5ab9ce1447
git-svn-id: http://moon:8086/svn/software/trunk/projects/opencv@339 b431acfa-c32f-4a4a-93f1-934dc6c82436
137 lines
3.6 KiB
Python
Executable File
137 lines
3.6 KiB
Python
Executable File
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
|
|
self.sockTcpEv3 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
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(10.0)
|
|
sockUdp.bind((UDP_IP, UDP_PORT))
|
|
localTcpServer = None
|
|
while True:
|
|
try:
|
|
data, addr = sockUdp.recvfrom(1024) # buffer size is 1024 bytes
|
|
except:
|
|
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':
|
|
print ("EV3 discovered")
|
|
sockUdp.sendto(b'A', addr)
|
|
if localTcpServer is None:
|
|
try:
|
|
localTcpServer = LocalServer(addr[0], int(keyValueDict['Port']))
|
|
localTcpServer.start()
|
|
except Exception as e:
|
|
print (e)
|
|
if localTcpServer is not None:
|
|
localTcpServer = None
|
|
del localTcpServer
|
|
|
|
if localTcpServer is not None:
|
|
localTcpServer.stop()
|
|
del localTcpServer
|
|
localTcpServer = None
|
|
|
|
|
|
|
|
|