forked from semuconsulting/PyGPSClient
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathsocket_server.py
More file actions
296 lines (242 loc) · 8.5 KB
/
socket_server.py
File metadata and controls
296 lines (242 loc) · 8.5 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
"""
!!! EXPERIMENTAL - FOR TESTING ONLY !!!
Threaded GNSS TCP socket server.
Reads serial stream from GNSS receiver and outputs
raw binary data to multiple TCP socket clients.
Created on 26 Apr 2022
:author: semuadmin
:copyright: SEMU Consulting © 2022
:license: BSD 3-Clause
"""
from socketserver import ThreadingTCPServer, StreamRequestHandler
from io import BufferedReader
from threading import Thread, Event
from queue import Queue
from serial import Serial, SerialException, SerialTimeoutException
from pyubx2 import (
UBXReader,
ERR_IGNORE,
UBX_PROTOCOL,
NMEA_PROTOCOL,
RTCM3_PROTOCOL,
UBXMessageError,
UBXParseError,
)
from pynmeagps import NMEAMessageError, NMEAParseError
from pyrtcm import RTCMMessageError, RTCMParseError
from pygpsclient.globals import (
DEFAULT_BUFSIZE,
)
class SockServer(ThreadingTCPServer):
"""
Socket server class.
"""
def __init__(self, maxclients: int, msgqueue: Queue, *args, **kwargs):
"""
Constructor.
:param int maxclients: max no of clients allowed
:param Queue msgqueue: queue containing raw GNSS messages
"""
self.maxclients = maxclients
self._msgqueue = msgqueue
self.connections = 0
self._stopevent = Event()
self.clientqueues = []
# set up pool of client queues
for _ in range(self.maxclients):
self.clientqueues.append({"client": None, "queue": Queue()})
super().__init__(*args, **kwargs)
# start GNSS message reader
while not self._msgqueue.empty(): # flush queue
self._msgqueue.get()
self._stopevent.clear()
self._stream_thread = Thread(
target=self._read_thread,
args=(self._stopevent, self._msgqueue, self.clientqueues),
daemon=True,
)
self._stream_thread.start()
def _read_thread(self, stopevent: Event, msgqueue: Queue, clientqueues: dict):
"""
Read from main GNSS message queue and place
raw data on array of socket client queues.
:param Event stopevent: stop event
:param Queue msgqueue: message queue
:param Dict clientqueues: pool of queues for used by clients
"""
while not stopevent.is_set():
raw, _ = msgqueue.get()
for i in range(self.maxclients):
clientqueues[i]["queue"].put(raw)
class ClientHandler(StreamRequestHandler):
"""
Threaded TCP client connection handler class.
"""
def __init__(self, *args, **kwargs):
"""
Constructor.
"""
self._qidx = None
self._msgqueue = None
self._allowed = False
super().__init__(*args, **kwargs)
def setup(self, *args, **kwargs):
"""
Overidden client handler setup routine.
"""
# find next unused client queue in pool...
for i, clq in enumerate(self.server.clientqueues):
if clq["client"] is None:
self.server.clientqueues[i]["client"] = self.client_address[1]
self._msgqueue = clq["queue"]
while not self._msgqueue.empty(): # flush queue
self._msgqueue.get()
self._qidx = i
self._allowed = True
break
if self._qidx is None: # no available client queues in pool
print(
f"Connection rejected - maximum {self.server.maxclients} clients allowed"
)
return
if self._allowed:
self.server.connections += 1
print(
f"Client connected {self.client_address[0]}:{self.client_address[1]}",
f"; number of clients: {self.server.connections}",
)
super().setup(*args, **kwargs)
def finish(self, *args, **kwargs):
"""
Overidden client handler finish routine.
"""
if self._qidx is not None:
self.server.clientqueues[self._qidx]["client"] = None
if self._allowed:
self.server.connections -= 1
print(
f"Client disconnected {self.client_address[0]}:{self.client_address[1]}",
f"; number of clients: {self.server.connections}",
)
super().finish(*args, **kwargs)
def handle(self):
"""
Overridden main client handler.
"""
while self._allowed:
try:
raw = self._msgqueue.get()
if raw is not None:
self.wfile.write(raw)
self.wfile.flush()
except (ConnectionAbortedError, ConnectionResetError, BrokenPipeError):
break
class StreamHandler:
"""
Stream handler class.
"""
def __init__(self, serport, baud, timeout):
"""
Constructor.
"""
self._serport = serport
self._baud = baud
self._timeout = timeout
self._serial_object = None
self._serial_buffer = None
self._stream_thread = None
self._socket = None
self._msgqueue = Queue()
self._stopevent = Event()
self._maxclients = 10
@property
def msgqueue(self) -> Queue:
"""
Getter for main message queue.
:return: message queue
:rtype: Queue
"""
return self._msgqueue
def start_read_thread(self):
"""
Start the stream read thread.
"""
self._stopevent.clear()
self._stream_thread = Thread(
target=self._read_thread,
args=(self._stopevent, self.msgqueue),
daemon=True,
)
self._stream_thread.start()
def stop_read_thread(self):
"""
Stop serial reader thread.
"""
self._stopevent.set()
self._stream_thread = None
def _read_thread(self, stopevent: Event, msgqueue: Queue):
"""
THREADED PROCESS
Connects to selected data stream and starts read loop.
:param Event stopevent: thread stop event
:param Queue msgqueue: message queue
"""
try:
with Serial(
self._serport, self._baud, timeout=self._timeout
) as self._serial_object:
stream = BufferedReader(self._serial_object)
self._readloop(stopevent, msgqueue, stream)
except (SerialException, SerialTimeoutException) as err:
print(err)
def _readloop(self, stopevent: Event, msgqueue: Queue, stream: object):
"""
Read stream continously until stop event or stream error.
:param Event stopevent: thread stop event
:param Queue msgqueue: message queue
:param object stream: data stream
"""
# pylint: disable=no-self-use
ubr = UBXReader(
stream,
protfilter=NMEA_PROTOCOL | UBX_PROTOCOL | RTCM3_PROTOCOL,
quitonerror=ERR_IGNORE,
bufsize=DEFAULT_BUFSIZE,
)
raw_data = None
parsed_data = None
while not stopevent.is_set():
try:
raw_data, parsed_data = ubr.read()
if raw_data is not None:
# print(parsed_data)
msgqueue.put((raw_data, parsed_data))
except (
UBXMessageError,
UBXParseError,
NMEAMessageError,
NMEAParseError,
RTCMMessageError,
RTCMParseError,
) as err:
print(err)
continue
if __name__ == "__main__":
SERIAL = "/dev/tty.usbmodem14101"
BAUD = 9600
TIMEOUT = 0.1
HOST = "localhost"
PORT = 50007
MAXCLIENTS = 5
print(f"Creating Serial streamer on {SERIAL}@{BAUD}")
streamer = StreamHandler(SERIAL, BAUD, TIMEOUT)
print(f"Creating Socket server on {HOST}:{PORT}")
server = SockServer(MAXCLIENTS, streamer.msgqueue, (HOST, PORT), ClientHandler)
try:
print("starting serial read thread...")
streamer.start_read_thread()
print("Starting TCP server, waiting for client connections...")
server.serve_forever()
except KeyboardInterrupt:
streamer.stop_read_thread()
print("Socket server terminated by user")