This repository has been archived by the owner on Feb 24, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
connection.py
103 lines (86 loc) · 3.08 KB
/
connection.py
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
"""
Connections between a car and the simulator.
For sending actions and receiving camera frames.
The communication loop runs in a child process.
"""
from multiprocessing import Process, Queue
import queue
import json
import queue
import signal
import socket
import time
import numpy as np
from . import camera
TIMEOUT = 1/60
def connect(url):
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
host, port = url.split(':')
sock.connect((host, int(port)))
return sock
def send_json(sock, data):
msg_string = json.dumps(data, separators=(',', ':')) + "\n"
msg = bytes(msg_string, "utf-8")
err = sock.sendall(msg)
assert err is None, "failed sock.sendall '{:s}', with msg of length {:d}".format(err, len(msg))
def communication_loop(stop_msg, simulator_url, login_cmds, frames, commands):
signal.signal(signal.SIGINT, signal.SIG_IGN)
default_cmd = {"action": "forward", "value": 0}
with connect(simulator_url) as sock:
for login_cmd in login_cmds:
send_json(sock, login_cmd)
sock.settimeout(TIMEOUT)
while True:
try:
if stop_msg.get(block=False):
break
except queue.Empty:
pass
try:
buf = camera.read_buffer(sock)
except socket.timeout:
time.sleep(TIMEOUT)
continue
frames.put(buf, block=False)
sent_something = False
while True:
try:
cmd = commands.get(block=False)
send_json(sock, cmd)
sent_something = True
except queue.Empty:
break
if not sent_something:
send_json(sock, default_cmd)
class CarConnection:
def __init__(self, simulator_url, car_name, team_id, car_color):
self.car_id = team_id if car_name == team_id else team_id + "_" + car_name
login_cmds = [
{"name": car_name, "teamId": team_id, "color": car_color},
{"move": True},
]
self.stop_msg = Queue()
self.frame_queue = Queue()
self.cmd_queue = Queue()
self.proc = Process(
name="simulator-car-connection-{}".format(self.car_id),
target=communication_loop,
args=(self.stop_msg, simulator_url, login_cmds, self.frame_queue, self.cmd_queue))
def start(self):
self.proc.start()
def stop(self):
self.stop_msg.put(True)
self.proc.join(timeout=0.5)
if self.proc.exitcode is None:
print(self.proc.name, "did not terminate properly, killing process")
self.proc.terminate()
def read_camera_frames(self):
while True:
try:
buf = self.frame_queue.get(block=False)
yield camera.buffer_to_frame(buf)
except queue.Empty:
return
def action(self, key, value):
self.cmd_queue.put({"action": key, "value": value}, block=False)