Vehicle Researcher
5 years ago
commit
5087c7684e
35 changed files with 19871 additions and 71 deletions
@ -0,0 +1 @@ |
||||
.sconsign.dblite |
||||
@ -0,0 +1,19 @@ |
||||
from ubuntu:16.04 |
||||
|
||||
RUN apt-get update && apt-get install -y libzmq3-dev clang wget git autoconf libtool curl make build-essential libssl-dev zlib1g-dev libbz2-dev libreadline-dev libsqlite3-dev llvm libncurses5-dev libncursesw5-dev xz-utils tk-dev libffi-dev liblzma-dev python-openssl |
||||
|
||||
RUN curl -L https://github.com/pyenv/pyenv-installer/raw/master/bin/pyenv-installer | bash |
||||
ENV PATH="/root/.pyenv/bin:/root/.pyenv/shims:${PATH}" |
||||
RUN pyenv install 3.7.3 |
||||
RUN pyenv global 3.7.3 |
||||
RUN pyenv rehash |
||||
RUN pip3 install pyyaml==5.1.2 Cython==0.29.14 scons==3.1.1 pycapnp==0.6.4 |
||||
|
||||
WORKDIR /project/cereal |
||||
COPY install_capnp.sh . |
||||
RUN ./install_capnp.sh |
||||
|
||||
ENV PYTHONPATH=/project |
||||
|
||||
COPY . . |
||||
RUN scons -c && scons -j$(nproc) |
||||
@ -1,62 +0,0 @@ |
||||
PWD := $(shell pwd)
|
||||
|
||||
SRCS := log.capnp car.capnp
|
||||
|
||||
GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
|
||||
JS := gen/js/car.capnp.js gen/js/log.capnp.js
|
||||
|
||||
UNAME_M ?= $(shell uname -m)
|
||||
|
||||
GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h
|
||||
|
||||
ifeq ($(UNAME_M),x86_64) |
||||
|
||||
ifneq (, $(shell which capnpc-java)) |
||||
GENS += gen/java/Car.java gen/java/Log.java
|
||||
else |
||||
$(warning capnpc-java not found, skipping java build) |
||||
endif |
||||
|
||||
endif |
||||
|
||||
|
||||
ifeq ($(UNAME_M),aarch64) |
||||
CAPNPC=PATH=$(PWD)/../phonelibs/capnp-cpp/aarch64/bin/:$$PATH capnpc
|
||||
else |
||||
CAPNPC=capnpc
|
||||
endif |
||||
|
||||
.PHONY: all |
||||
all: $(GENS) |
||||
js: $(JS) |
||||
|
||||
.PHONY: clean |
||||
clean: |
||||
rm -rf gen
|
||||
rm -rf node_modules
|
||||
rm -rf package-lock.json
|
||||
|
||||
gen/c/%.capnp.c: %.capnp |
||||
@echo "[ CAPNPC C ] $@"
|
||||
mkdir -p gen/c/
|
||||
$(CAPNPC) '$<' -o c:gen/c/
|
||||
|
||||
gen/js/%.capnp.js: %.capnp |
||||
@echo "[ CAPNPC JavaScript ] $@"
|
||||
mkdir -p gen/js/
|
||||
sh ./generate_javascript.sh
|
||||
|
||||
gen/cpp/%.capnp.c++: %.capnp |
||||
@echo "[ CAPNPC C++ ] $@"
|
||||
mkdir -p gen/cpp/
|
||||
$(CAPNPC) '$<' -o c++:gen/cpp/
|
||||
|
||||
gen/java/Car.java gen/java/Log.java: $(SRCS) |
||||
@echo "[ CAPNPC java ] $@"
|
||||
mkdir -p gen/java/
|
||||
$(CAPNPC) $^ -o java:gen/java
|
||||
|
||||
# c-capnproto needs some empty headers
|
||||
gen/c/include/c++.capnp.h gen/c/include/java.capnp.h: |
||||
mkdir -p gen/c/include
|
||||
touch '$@'
|
||||
@ -0,0 +1,68 @@ |
||||
Import('env', 'arch', 'zmq') |
||||
|
||||
gen_dir = Dir('gen') |
||||
messaging_dir = Dir('messaging') |
||||
|
||||
# TODO: remove src-prefix and cereal from command string. can we set working directory? |
||||
env.Command(["gen/c/include/c++.capnp.h", "gen/c/include/java.capnp.h"], [], "mkdir -p " + gen_dir.path + "/c/include && touch $TARGETS") |
||||
env.Command( |
||||
['gen/c/car.capnp.c', 'gen/c/log.capnp.c', 'gen/c/car.capnp.h', 'gen/c/log.capnp.h'], |
||||
['car.capnp', 'log.capnp'], |
||||
'capnpc $SOURCES --src-prefix=cereal -o c:' + gen_dir.path + '/c/') |
||||
env.Command( |
||||
['gen/cpp/car.capnp.c++', 'gen/cpp/log.capnp.c++', 'gen/cpp/car.capnp.h', 'gen/cpp/log.capnp.h'], |
||||
['car.capnp', 'log.capnp'], |
||||
'capnpc $SOURCES --src-prefix=cereal -o c++:' + gen_dir.path + '/cpp/') |
||||
import shutil |
||||
if shutil.which('capnpc-java'): |
||||
env.Command( |
||||
['gen/java/Car.java', 'gen/java/Log.java'], |
||||
['car.capnp', 'log.capnp'], |
||||
'capnpc $SOURCES --src-prefix=cereal -o java:' + gen_dir.path + '/java/') |
||||
|
||||
# TODO: remove non shared cereal and messaging |
||||
cereal_objects = env.SharedObject([ |
||||
'gen/c/car.capnp.c', |
||||
'gen/c/log.capnp.c', |
||||
'gen/cpp/car.capnp.c++', |
||||
'gen/cpp/log.capnp.c++', |
||||
]) |
||||
|
||||
env.Library('cereal', cereal_objects) |
||||
env.SharedLibrary('cereal_shared', cereal_objects) |
||||
|
||||
cereal_dir = Dir('.') |
||||
services_h = env.Command( |
||||
['services.h'], |
||||
['service_list.yaml', 'services.py'], |
||||
'python3 ' + cereal_dir.path + '/services.py > $TARGET') |
||||
|
||||
messaging_objects = env.SharedObject([ |
||||
'messaging/messaging.cc', |
||||
'messaging/impl_zmq.cc', |
||||
'messaging/impl_msgq.cc', |
||||
'messaging/msgq.cc', |
||||
]) |
||||
|
||||
messaging_lib = env.Library('messaging', messaging_objects) |
||||
Depends('messaging/impl_zmq.cc', services_h) |
||||
|
||||
# note, this rebuilds the deps shared, zmq is statically linked to make APK happy |
||||
# TODO: get APK to load system zmq to remove the static link |
||||
shared_lib_shared_lib = [zmq, 'm', 'stdc++'] + ["gnustl_shared"] if arch == "aarch64" else [] |
||||
env.SharedLibrary('messaging_shared', messaging_objects, LIBS=shared_lib_shared_lib) |
||||
|
||||
env.Program('messaging/bridge', ['messaging/bridge.cc'], LIBS=[messaging_lib, 'zmq']) |
||||
Depends('messaging/bridge.cc', services_h) |
||||
|
||||
# different target? |
||||
#env.Program('messaging/demo', ['messaging/demo.cc'], LIBS=[messaging_lib, 'zmq']) |
||||
|
||||
|
||||
env.Command(['messaging/messaging_pyx.so'], |
||||
[messaging_lib, 'messaging/messaging_pyx_setup.py', 'messaging/messaging_pyx.pyx', 'messaging/messaging.pxd'], |
||||
"cd " + messaging_dir.path + " && python3 messaging_pyx_setup.py build_ext --inplace") |
||||
|
||||
|
||||
if GetOption('test'): |
||||
env.Program('messaging/test_runner', ['messaging/test_runner.cc', 'messaging/msgq_tests.cc'], LIBS=[messaging_lib]) |
||||
@ -0,0 +1,49 @@ |
||||
import os |
||||
import subprocess |
||||
|
||||
zmq = 'zmq' |
||||
arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() |
||||
|
||||
cereal_dir = Dir('.') |
||||
|
||||
cpppath = [ |
||||
cereal_dir, |
||||
'/usr/lib/include', |
||||
] |
||||
|
||||
AddOption('--test', |
||||
action='store_true', |
||||
help='build test files') |
||||
|
||||
AddOption('--asan', |
||||
action='store_true', |
||||
help='turn on ASAN') |
||||
|
||||
ccflags_asan = ["-fsanitize=address", "-fno-omit-frame-pointer"] if GetOption('asan') else [] |
||||
ldflags_asan = ["-fsanitize=address"] if GetOption('asan') else [] |
||||
|
||||
env = Environment( |
||||
ENV=os.environ, |
||||
CC='clang', |
||||
CXX='clang++', |
||||
CCFLAGS=[ |
||||
"-g", |
||||
"-fPIC", |
||||
"-O2", |
||||
"-Werror=implicit-function-declaration", |
||||
"-Werror=incompatible-pointer-types", |
||||
"-Werror=int-conversion", |
||||
"-Werror=return-type", |
||||
"-Werror=format-extra-args", |
||||
] + ccflags_asan, |
||||
LDFLAGS=ldflags_asan, |
||||
LINKFLAGS=ldflags_asan, |
||||
|
||||
CFLAGS="-std=gnu11", |
||||
CXXFLAGS="-std=c++14", |
||||
CPPPATH=cpppath, |
||||
) |
||||
|
||||
|
||||
Export('env', 'zmq', 'arch') |
||||
SConscript(['SConscript']) |
||||
@ -0,0 +1,14 @@ |
||||
pr: none |
||||
|
||||
pool: |
||||
vmImage: 'ubuntu-16.04' |
||||
|
||||
steps: |
||||
- script: | |
||||
set -e |
||||
docker build -t cereal . |
||||
docker run cereal bash -c "scons --test --asan -j$(nproc) && messaging/test_runner" |
||||
docker run cereal bash -c "ZMQ=1 python -m unittest discover ." |
||||
docker run cereal bash -c "MSGQ=1 python -m unittest discover ." |
||||
|
||||
displayName: 'Run Tests' |
||||
@ -0,0 +1,10 @@ |
||||
demo |
||||
bridge |
||||
test_runner |
||||
*.o |
||||
*.os |
||||
*.d |
||||
*.a |
||||
*.so |
||||
messaging_pyx.cpp |
||||
build/ |
||||
@ -0,0 +1,219 @@ |
||||
# must be build with scons |
||||
from .messaging_pyx import Context, Poller, SubSocket, PubSocket # pylint: disable=no-name-in-module, import-error |
||||
from .messaging_pyx import MultiplePublishersError, MessagingError # pylint: disable=no-name-in-module, import-error |
||||
|
||||
assert MultiplePublishersError |
||||
assert MessagingError |
||||
|
||||
from cereal import log |
||||
from cereal.services import service_list |
||||
|
||||
# sec_since_boot is faster, but allow to run standalone too |
||||
try: |
||||
from common.realtime import sec_since_boot |
||||
except ImportError: |
||||
import time |
||||
sec_since_boot = time.time |
||||
print("Warning, using python time.time() instead of faster sec_since_boot") |
||||
|
||||
context = Context() |
||||
|
||||
def new_message(): |
||||
dat = log.Event.new_message() |
||||
dat.logMonoTime = int(sec_since_boot() * 1e9) |
||||
dat.valid = True |
||||
return dat |
||||
|
||||
def pub_sock(endpoint): |
||||
sock = PubSocket() |
||||
sock.connect(context, endpoint) |
||||
return sock |
||||
|
||||
def sub_sock(endpoint, poller=None, addr="127.0.0.1", conflate=False, timeout=None): |
||||
sock = SubSocket() |
||||
addr = addr.encode('utf8') |
||||
sock.connect(context, endpoint, addr, conflate) |
||||
|
||||
if timeout is not None: |
||||
sock.setTimeout(timeout) |
||||
|
||||
if poller is not None: |
||||
poller.registerSocket(sock) |
||||
return sock |
||||
|
||||
|
||||
def drain_sock_raw(sock, wait_for_one=False): |
||||
"""Receive all message currently available on the queue""" |
||||
ret = [] |
||||
while 1: |
||||
if wait_for_one and len(ret) == 0: |
||||
dat = sock.receive() |
||||
else: |
||||
dat = sock.receive(non_blocking=True) |
||||
|
||||
if dat is None: |
||||
break |
||||
|
||||
ret.append(dat) |
||||
|
||||
return ret |
||||
|
||||
def drain_sock(sock, wait_for_one=False): |
||||
"""Receive all message currently available on the queue""" |
||||
ret = [] |
||||
while 1: |
||||
if wait_for_one and len(ret) == 0: |
||||
dat = sock.receive() |
||||
else: |
||||
dat = sock.receive(non_blocking=True) |
||||
|
||||
if dat is None: # Timeout hit |
||||
break |
||||
|
||||
dat = log.Event.from_bytes(dat) |
||||
ret.append(dat) |
||||
|
||||
return ret |
||||
|
||||
|
||||
# TODO: print when we drop packets? |
||||
def recv_sock(sock, wait=False): |
||||
"""Same as drain sock, but only returns latest message. Consider using conflate instead.""" |
||||
dat = None |
||||
|
||||
while 1: |
||||
if wait and dat is None: |
||||
rcv = sock.receive() |
||||
else: |
||||
rcv = sock.receive(non_blocking=True) |
||||
|
||||
if rcv is None: # Timeout hit |
||||
break |
||||
|
||||
dat = rcv |
||||
|
||||
if dat is not None: |
||||
dat = log.Event.from_bytes(dat) |
||||
|
||||
return dat |
||||
|
||||
def recv_one(sock): |
||||
dat = sock.receive() |
||||
if dat is not None: |
||||
dat = log.Event.from_bytes(dat) |
||||
return dat |
||||
|
||||
def recv_one_or_none(sock): |
||||
dat = sock.receive(non_blocking=True) |
||||
if dat is not None: |
||||
dat = log.Event.from_bytes(dat) |
||||
return dat |
||||
|
||||
def recv_one_retry(sock): |
||||
"""Keep receiving until we get a message""" |
||||
while True: |
||||
dat = sock.receive() |
||||
if dat is not None: |
||||
return log.Event.from_bytes(dat) |
||||
|
||||
def get_one_can(logcan): |
||||
while True: |
||||
can = recv_one_retry(logcan) |
||||
if len(can.can) > 0: |
||||
return can |
||||
|
||||
class SubMaster(): |
||||
def __init__(self, services, ignore_alive=None, addr="127.0.0.1"): |
||||
self.poller = Poller() |
||||
self.frame = -1 |
||||
self.updated = {s : False for s in services} |
||||
self.rcv_time = {s : 0. for s in services} |
||||
self.rcv_frame = {s : 0 for s in services} |
||||
self.alive = {s : False for s in services} |
||||
self.sock = {} |
||||
self.freq = {} |
||||
self.data = {} |
||||
self.logMonoTime = {} |
||||
self.valid = {} |
||||
|
||||
if ignore_alive is not None: |
||||
self.ignore_alive = ignore_alive |
||||
else: |
||||
self.ignore_alive = [] |
||||
|
||||
for s in services: |
||||
if addr is not None: |
||||
self.sock[s] = sub_sock(s, poller=self.poller, addr=addr, conflate=True) |
||||
self.freq[s] = service_list[s].frequency |
||||
|
||||
data = new_message() |
||||
if s in ['can', 'sensorEvents', 'liveTracks', 'sendCan', |
||||
'ethernetData', 'cellInfo', 'wifiScan', |
||||
'trafficEvents', 'orbObservation', 'carEvents']: |
||||
data.init(s, 0) |
||||
else: |
||||
data.init(s) |
||||
self.data[s] = getattr(data, s) |
||||
self.logMonoTime[s] = 0 |
||||
self.valid[s] = data.valid |
||||
|
||||
def __getitem__(self, s): |
||||
return self.data[s] |
||||
|
||||
def update(self, timeout=1000): |
||||
msgs = [] |
||||
for sock in self.poller.poll(timeout): |
||||
msgs.append(recv_one_or_none(sock)) |
||||
self.update_msgs(sec_since_boot(), msgs) |
||||
|
||||
def update_msgs(self, cur_time, msgs): |
||||
# TODO: add optional input that specify the service to wait for |
||||
self.frame += 1 |
||||
self.updated = dict.fromkeys(self.updated, False) |
||||
for msg in msgs: |
||||
if msg is None: |
||||
continue |
||||
|
||||
s = msg.which() |
||||
self.updated[s] = True |
||||
self.rcv_time[s] = cur_time |
||||
self.rcv_frame[s] = self.frame |
||||
self.data[s] = getattr(msg, s) |
||||
self.logMonoTime[s] = msg.logMonoTime |
||||
self.valid[s] = msg.valid |
||||
|
||||
for s in self.data: |
||||
# arbitrary small number to avoid float comparison. If freq is 0, we can skip the check |
||||
if self.freq[s] > 1e-5: |
||||
# alive if delay is within 10x the expected frequency |
||||
self.alive[s] = (cur_time - self.rcv_time[s]) < (10. / self.freq[s]) |
||||
else: |
||||
self.alive[s] = True |
||||
|
||||
def all_alive(self, service_list=None): |
||||
if service_list is None: # check all |
||||
service_list = self.alive.keys() |
||||
return all(self.alive[s] for s in service_list if s not in self.ignore_alive) |
||||
|
||||
def all_valid(self, service_list=None): |
||||
if service_list is None: # check all |
||||
service_list = self.valid.keys() |
||||
return all(self.valid[s] for s in service_list) |
||||
|
||||
def all_alive_and_valid(self, service_list=None): |
||||
if service_list is None: # check all |
||||
service_list = self.alive.keys() |
||||
return self.all_alive(service_list=service_list) and self.all_valid(service_list=service_list) |
||||
|
||||
|
||||
class PubMaster(): |
||||
def __init__(self, services): |
||||
self.sock = {} |
||||
for s in services: |
||||
self.sock[s] = pub_sock(s) |
||||
|
||||
def send(self, s, dat): |
||||
# accept either bytes or capnp builder |
||||
if not isinstance(dat, bytes): |
||||
dat = dat.to_bytes() |
||||
self.sock[s].send(dat) |
||||
@ -0,0 +1,62 @@ |
||||
#include <iostream> |
||||
#include <string> |
||||
#include <cassert> |
||||
#include <csignal> |
||||
#include <map> |
||||
|
||||
#include "services.h" |
||||
|
||||
#include "impl_msgq.hpp" |
||||
#include "impl_zmq.hpp" |
||||
|
||||
void sigpipe_handler(int sig) { |
||||
assert(sig == SIGPIPE); |
||||
std::cout << "SIGPIPE received" << std::endl; |
||||
} |
||||
|
||||
static std::vector<std::string> get_services() { |
||||
std::vector<std::string> name_list; |
||||
|
||||
for (const auto& it : services) { |
||||
std::string name = it.name; |
||||
if (name == "plusFrame" || name == "uiLayoutState") continue; |
||||
name_list.push_back(name); |
||||
} |
||||
|
||||
return name_list; |
||||
} |
||||
|
||||
|
||||
int main(void){ |
||||
signal(SIGPIPE, (sighandler_t)sigpipe_handler); |
||||
|
||||
auto endpoints = get_services(); |
||||
|
||||
std::map<SubSocket*, PubSocket*> sub2pub; |
||||
|
||||
Context *zmq_context = new ZMQContext(); |
||||
Context *msgq_context = new MSGQContext(); |
||||
Poller *poller = new MSGQPoller(); |
||||
|
||||
for (auto endpoint: endpoints){ |
||||
SubSocket * msgq_sock = new MSGQSubSocket(); |
||||
msgq_sock->connect(msgq_context, endpoint, "127.0.0.1", false); |
||||
poller->registerSocket(msgq_sock); |
||||
|
||||
PubSocket * zmq_sock = new ZMQPubSocket(); |
||||
zmq_sock->connect(zmq_context, endpoint); |
||||
|
||||
sub2pub[msgq_sock] = zmq_sock; |
||||
} |
||||
|
||||
|
||||
while (true){ |
||||
for (auto sub_sock : poller->poll(100)){ |
||||
Message * msg = sub_sock->receive(); |
||||
if (msg == NULL) continue; |
||||
sub2pub[sub_sock]->sendMessage(msg); |
||||
delete msg; |
||||
} |
||||
} |
||||
return 0; |
||||
} |
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,50 @@ |
||||
#include <iostream> |
||||
#include <cstddef> |
||||
#include <chrono> |
||||
#include <thread> |
||||
#include <cassert> |
||||
|
||||
#include "messaging.hpp" |
||||
#include "impl_zmq.hpp" |
||||
|
||||
#define MSGS 1e5 |
||||
|
||||
int main() { |
||||
Context * c = Context::create(); |
||||
SubSocket * sub_sock = SubSocket::create(c, "controlsState"); |
||||
PubSocket * pub_sock = PubSocket::create(c, "controlsState"); |
||||
|
||||
char data[8]; |
||||
|
||||
Poller * poller = Poller::create({sub_sock}); |
||||
|
||||
auto start = std::chrono::steady_clock::now(); |
||||
|
||||
for (uint64_t i = 0; i < MSGS; i++){ |
||||
*(uint64_t*)data = i; |
||||
pub_sock->send(data, 8); |
||||
|
||||
auto r = poller->poll(100); |
||||
|
||||
for (auto p : r){ |
||||
Message * m = p->receive(); |
||||
uint64_t ii = *(uint64_t*)m->getData(); |
||||
assert(i == ii); |
||||
delete m; |
||||
} |
||||
} |
||||
|
||||
|
||||
auto end = std::chrono::steady_clock::now(); |
||||
double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / 1e9; |
||||
double throughput = ((double) MSGS / (double) elapsed); |
||||
std::cout << throughput << " msg/s" << std::endl; |
||||
|
||||
delete poller; |
||||
delete sub_sock; |
||||
delete pub_sock; |
||||
delete c; |
||||
|
||||
|
||||
return 0; |
||||
} |
||||
@ -0,0 +1,30 @@ |
||||
import time |
||||
|
||||
from messaging_pyx import Context, Poller, SubSocket, PubSocket # pylint: disable=no-name-in-module, import-error |
||||
|
||||
MSGS = 1e5 |
||||
|
||||
if __name__ == "__main__": |
||||
c = Context() |
||||
sub_sock = SubSocket() |
||||
pub_sock = PubSocket() |
||||
|
||||
sub_sock.connect(c, "controlsState") |
||||
pub_sock.connect(c, "controlsState") |
||||
|
||||
|
||||
poller = Poller() |
||||
poller.registerSocket(sub_sock) |
||||
|
||||
t = time.time() |
||||
for i in range(int(MSGS)): |
||||
bts = i.to_bytes(4, 'little') |
||||
pub_sock.send(bts) |
||||
|
||||
for s in poller.poll(100): |
||||
dat = s.receive() |
||||
ii = int.from_bytes(dat, 'little') |
||||
assert(i == ii) |
||||
|
||||
dt = time.time() - t |
||||
print("%.1f msg/s" % (MSGS / dt)) |
||||
@ -0,0 +1,190 @@ |
||||
#include <cassert> |
||||
#include <cstring> |
||||
#include <iostream> |
||||
#include <cstdlib> |
||||
#include <csignal> |
||||
#include <cerrno> |
||||
|
||||
|
||||
#include "impl_msgq.hpp" |
||||
|
||||
volatile sig_atomic_t msgq_do_exit = 0; |
||||
|
||||
void sig_handler(int signal) { |
||||
assert(signal == SIGINT || signal == SIGTERM); |
||||
msgq_do_exit = 1; |
||||
} |
||||
|
||||
|
||||
MSGQContext::MSGQContext() { |
||||
} |
||||
|
||||
MSGQContext::~MSGQContext() { |
||||
} |
||||
|
||||
void MSGQMessage::init(size_t sz) { |
||||
size = sz; |
||||
data = new char[size]; |
||||
} |
||||
|
||||
void MSGQMessage::init(char * d, size_t sz) { |
||||
size = sz; |
||||
data = new char[size]; |
||||
memcpy(data, d, size); |
||||
} |
||||
|
||||
void MSGQMessage::takeOwnership(char * d, size_t sz) { |
||||
size = sz; |
||||
data = d; |
||||
} |
||||
|
||||
void MSGQMessage::close() { |
||||
if (size > 0){ |
||||
delete[] data; |
||||
} |
||||
size = 0; |
||||
} |
||||
|
||||
MSGQMessage::~MSGQMessage() { |
||||
this->close(); |
||||
} |
||||
|
||||
|
||||
int MSGQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate){ |
||||
assert(context); |
||||
assert(address == "127.0.0.1"); |
||||
|
||||
q = new msgq_queue_t; |
||||
int r = msgq_new_queue(q, endpoint.c_str(), DEFAULT_SEGMENT_SIZE); |
||||
if (r != 0){ |
||||
return r; |
||||
} |
||||
|
||||
msgq_init_subscriber(q); |
||||
|
||||
if (conflate){ |
||||
q->read_conflate = true; |
||||
} |
||||
|
||||
timeout = -1; |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
|
||||
Message * MSGQSubSocket::receive(bool non_blocking){ |
||||
msgq_do_exit = 0; |
||||
|
||||
void (*prev_handler_sigint)(int); |
||||
void (*prev_handler_sigterm)(int); |
||||
if (!non_blocking){ |
||||
prev_handler_sigint = std::signal(SIGINT, sig_handler); |
||||
prev_handler_sigterm = std::signal(SIGTERM, sig_handler); |
||||
} |
||||
|
||||
msgq_msg_t msg; |
||||
|
||||
MSGQMessage *r = NULL; |
||||
r = NULL; |
||||
|
||||
int rc = msgq_msg_recv(&msg, q); |
||||
|
||||
// Hack to implement blocking read with a poller. Don't use this
|
||||
while (!non_blocking && rc == 0 && msgq_do_exit == 0){ |
||||
msgq_pollitem_t items[1]; |
||||
items[0].q = q; |
||||
|
||||
int t = (timeout != -1) ? timeout : 100; |
||||
|
||||
int n = msgq_poll(items, 1, t); |
||||
rc = msgq_msg_recv(&msg, q); |
||||
|
||||
// The poll indicated a message was ready, but the receive failed. Try again
|
||||
if (n == 1 && rc == 0){ |
||||
continue; |
||||
} |
||||
|
||||
if (timeout != -1){ |
||||
break; |
||||
} |
||||
} |
||||
|
||||
if (rc > 0){ |
||||
r = new MSGQMessage; |
||||
r->takeOwnership(msg.data, msg.size); |
||||
} |
||||
errno = msgq_do_exit ? EINTR : 0; |
||||
|
||||
if (!non_blocking){ |
||||
std::signal(SIGINT, prev_handler_sigint); |
||||
std::signal(SIGTERM, prev_handler_sigterm); |
||||
} |
||||
|
||||
return (Message*)r; |
||||
} |
||||
|
||||
void MSGQSubSocket::setTimeout(int t){ |
||||
timeout = t; |
||||
} |
||||
|
||||
MSGQSubSocket::~MSGQSubSocket(){ |
||||
if (q != NULL){ |
||||
msgq_close_queue(q); |
||||
delete q; |
||||
} |
||||
} |
||||
|
||||
int MSGQPubSocket::connect(Context *context, std::string endpoint){ |
||||
assert(context); |
||||
|
||||
q = new msgq_queue_t; |
||||
msgq_new_queue(q, endpoint.c_str(), DEFAULT_SEGMENT_SIZE); |
||||
msgq_init_publisher(q); |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
int MSGQPubSocket::sendMessage(Message *message){ |
||||
msgq_msg_t msg; |
||||
msg.data = message->getData(); |
||||
msg.size = message->getSize(); |
||||
|
||||
return msgq_msg_send(&msg, q); |
||||
} |
||||
|
||||
int MSGQPubSocket::send(char *data, size_t size){ |
||||
msgq_msg_t msg; |
||||
msg.data = data; |
||||
msg.size = size; |
||||
|
||||
return msgq_msg_send(&msg, q); |
||||
} |
||||
|
||||
MSGQPubSocket::~MSGQPubSocket(){ |
||||
if (q != NULL){ |
||||
msgq_close_queue(q); |
||||
delete q; |
||||
} |
||||
} |
||||
|
||||
|
||||
void MSGQPoller::registerSocket(SubSocket * socket){ |
||||
assert(num_polls + 1 < MAX_POLLERS); |
||||
polls[num_polls].q = (msgq_queue_t*)socket->getRawSocket(); |
||||
|
||||
sockets.push_back(socket); |
||||
num_polls++; |
||||
} |
||||
|
||||
std::vector<SubSocket*> MSGQPoller::poll(int timeout){ |
||||
std::vector<SubSocket*> r; |
||||
|
||||
msgq_poll(polls, num_polls, timeout); |
||||
for (size_t i = 0; i < num_polls; i++){ |
||||
if (polls[i].revents){ |
||||
r.push_back(sockets[i]); |
||||
} |
||||
} |
||||
|
||||
return r; |
||||
} |
||||
@ -0,0 +1,64 @@ |
||||
#pragma once |
||||
#include "messaging.hpp" |
||||
#include "msgq.hpp" |
||||
#include <zmq.h> |
||||
#include <string> |
||||
|
||||
#define MAX_POLLERS 128 |
||||
|
||||
class MSGQContext : public Context { |
||||
private: |
||||
void * context = NULL; |
||||
public: |
||||
MSGQContext(); |
||||
void * getRawContext() {return context;} |
||||
~MSGQContext(); |
||||
}; |
||||
|
||||
class MSGQMessage : public Message { |
||||
private: |
||||
char * data; |
||||
size_t size; |
||||
public: |
||||
void init(size_t size); |
||||
void init(char *data, size_t size); |
||||
void takeOwnership(char *data, size_t size); |
||||
size_t getSize(){return size;} |
||||
char * getData(){return data;} |
||||
void close(); |
||||
~MSGQMessage(); |
||||
}; |
||||
|
||||
class MSGQSubSocket : public SubSocket { |
||||
private: |
||||
msgq_queue_t * q = NULL; |
||||
int timeout; |
||||
public: |
||||
int connect(Context *context, std::string endpoint, std::string address, bool conflate=false); |
||||
void setTimeout(int timeout); |
||||
void * getRawSocket() {return (void*)q;} |
||||
Message *receive(bool non_blocking=false); |
||||
~MSGQSubSocket(); |
||||
}; |
||||
|
||||
class MSGQPubSocket : public PubSocket { |
||||
private: |
||||
msgq_queue_t * q = NULL; |
||||
public: |
||||
int connect(Context *context, std::string endpoint); |
||||
int sendMessage(Message *message); |
||||
int send(char *data, size_t size); |
||||
~MSGQPubSocket(); |
||||
}; |
||||
|
||||
class MSGQPoller : public Poller { |
||||
private: |
||||
std::vector<SubSocket*> sockets; |
||||
msgq_pollitem_t polls[MAX_POLLERS]; |
||||
size_t num_polls = 0; |
||||
|
||||
public: |
||||
void registerSocket(SubSocket *socket); |
||||
std::vector<SubSocket*> poll(int timeout); |
||||
~MSGQPoller(){}; |
||||
}; |
||||
@ -0,0 +1,155 @@ |
||||
#include <cassert> |
||||
#include <cstring> |
||||
#include <iostream> |
||||
#include <cstdlib> |
||||
#include <cerrno> |
||||
|
||||
#include <zmq.h> |
||||
|
||||
#include "services.h" |
||||
#include "impl_zmq.hpp" |
||||
|
||||
static int get_port(std::string endpoint) { |
||||
int port = -1; |
||||
for (const auto& it : services) { |
||||
std::string name = it.name; |
||||
if (name == endpoint) { |
||||
port = it.port; |
||||
break; |
||||
} |
||||
} |
||||
|
||||
assert(port >= 0); |
||||
return port; |
||||
} |
||||
|
||||
ZMQContext::ZMQContext() { |
||||
context = zmq_ctx_new(); |
||||
} |
||||
|
||||
ZMQContext::~ZMQContext() { |
||||
zmq_ctx_term(context); |
||||
} |
||||
|
||||
void ZMQMessage::init(size_t sz) { |
||||
size = sz; |
||||
data = new char[size]; |
||||
} |
||||
|
||||
void ZMQMessage::init(char * d, size_t sz) { |
||||
size = sz; |
||||
data = new char[size]; |
||||
memcpy(data, d, size); |
||||
} |
||||
|
||||
void ZMQMessage::close() { |
||||
if (size > 0){ |
||||
delete[] data; |
||||
} |
||||
size = 0; |
||||
} |
||||
|
||||
ZMQMessage::~ZMQMessage() { |
||||
this->close(); |
||||
} |
||||
|
||||
|
||||
int ZMQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate){ |
||||
sock = zmq_socket(context->getRawContext(), ZMQ_SUB); |
||||
if (sock == NULL){ |
||||
return -1; |
||||
} |
||||
|
||||
zmq_setsockopt(sock, ZMQ_SUBSCRIBE, "", 0); |
||||
|
||||
if (conflate){ |
||||
int arg = 1; |
||||
zmq_setsockopt(sock, ZMQ_CONFLATE, &arg, sizeof(int)); |
||||
} |
||||
|
||||
int reconnect_ivl = 500; |
||||
zmq_setsockopt(sock, ZMQ_RECONNECT_IVL_MAX, &reconnect_ivl, sizeof(reconnect_ivl)); |
||||
|
||||
full_endpoint = "tcp://" + address + ":"; |
||||
full_endpoint += std::to_string(get_port(endpoint)); |
||||
|
||||
return zmq_connect(sock, full_endpoint.c_str()); |
||||
} |
||||
|
||||
|
||||
Message * ZMQSubSocket::receive(bool non_blocking){ |
||||
zmq_msg_t msg; |
||||
assert(zmq_msg_init(&msg) == 0); |
||||
|
||||
int flags = non_blocking ? ZMQ_DONTWAIT : 0; |
||||
int rc = zmq_msg_recv(&msg, sock, flags); |
||||
Message *r = NULL; |
||||
|
||||
if (rc >= 0){ |
||||
// Make a copy to ensure the data is aligned
|
||||
r = new ZMQMessage; |
||||
r->init((char*)zmq_msg_data(&msg), zmq_msg_size(&msg)); |
||||
} |
||||
|
||||
zmq_msg_close(&msg); |
||||
return r; |
||||
} |
||||
|
||||
void ZMQSubSocket::setTimeout(int timeout){ |
||||
zmq_setsockopt(sock, ZMQ_RCVTIMEO, &timeout, sizeof(int)); |
||||
} |
||||
|
||||
ZMQSubSocket::~ZMQSubSocket(){ |
||||
zmq_close(sock); |
||||
} |
||||
|
||||
int ZMQPubSocket::connect(Context *context, std::string endpoint){ |
||||
sock = zmq_socket(context->getRawContext(), ZMQ_PUB); |
||||
if (sock == NULL){ |
||||
return -1; |
||||
} |
||||
|
||||
full_endpoint = "tcp://*:"; |
||||
full_endpoint += std::to_string(get_port(endpoint)); |
||||
|
||||
return zmq_bind(sock, full_endpoint.c_str()); |
||||
} |
||||
|
||||
int ZMQPubSocket::sendMessage(Message *message){ |
||||
return zmq_send(sock, message->getData(), message->getSize(), ZMQ_DONTWAIT); |
||||
} |
||||
|
||||
int ZMQPubSocket::send(char *data, size_t size){ |
||||
return zmq_send(sock, data, size, ZMQ_DONTWAIT); |
||||
} |
||||
|
||||
ZMQPubSocket::~ZMQPubSocket(){ |
||||
zmq_close(sock); |
||||
} |
||||
|
||||
|
||||
void ZMQPoller::registerSocket(SubSocket * socket){ |
||||
assert(num_polls + 1 < MAX_POLLERS); |
||||
polls[num_polls].socket = socket->getRawSocket(); |
||||
polls[num_polls].events = ZMQ_POLLIN; |
||||
|
||||
sockets.push_back(socket); |
||||
num_polls++; |
||||
} |
||||
|
||||
std::vector<SubSocket*> ZMQPoller::poll(int timeout){ |
||||
std::vector<SubSocket*> r; |
||||
|
||||
int rc = zmq_poll(polls, num_polls, timeout); |
||||
if (rc < 0){ |
||||
return r; |
||||
} |
||||
|
||||
for (size_t i = 0; i < num_polls; i++){ |
||||
if (polls[i].revents){ |
||||
r.push_back(sockets[i]); |
||||
} |
||||
} |
||||
|
||||
return r; |
||||
} |
||||
@ -0,0 +1,63 @@ |
||||
#pragma once |
||||
#include "messaging.hpp" |
||||
#include <zmq.h> |
||||
#include <string> |
||||
|
||||
#define MAX_POLLERS 128 |
||||
|
||||
class ZMQContext : public Context { |
||||
private: |
||||
void * context = NULL; |
||||
public: |
||||
ZMQContext(); |
||||
void * getRawContext() {return context;} |
||||
~ZMQContext(); |
||||
}; |
||||
|
||||
class ZMQMessage : public Message { |
||||
private: |
||||
char * data; |
||||
size_t size; |
||||
public: |
||||
void init(size_t size); |
||||
void init(char *data, size_t size); |
||||
size_t getSize(){return size;} |
||||
char * getData(){return data;} |
||||
void close(); |
||||
~ZMQMessage(); |
||||
}; |
||||
|
||||
class ZMQSubSocket : public SubSocket { |
||||
private: |
||||
void * sock; |
||||
std::string full_endpoint; |
||||
public: |
||||
int connect(Context *context, std::string endpoint, std::string address, bool conflate=false); |
||||
void setTimeout(int timeout); |
||||
void * getRawSocket() {return sock;} |
||||
Message *receive(bool non_blocking=false); |
||||
~ZMQSubSocket(); |
||||
}; |
||||
|
||||
class ZMQPubSocket : public PubSocket { |
||||
private: |
||||
void * sock; |
||||
std::string full_endpoint; |
||||
public: |
||||
int connect(Context *context, std::string endpoint); |
||||
int sendMessage(Message *message); |
||||
int send(char *data, size_t size); |
||||
~ZMQPubSocket(); |
||||
}; |
||||
|
||||
class ZMQPoller : public Poller { |
||||
private: |
||||
std::vector<SubSocket*> sockets; |
||||
zmq_pollitem_t polls[MAX_POLLERS]; |
||||
size_t num_polls = 0; |
||||
|
||||
public: |
||||
void registerSocket(SubSocket *socket); |
||||
std::vector<SubSocket*> poll(int timeout); |
||||
~ZMQPoller(){}; |
||||
}; |
||||
@ -0,0 +1,117 @@ |
||||
#include "messaging.hpp" |
||||
#include "impl_zmq.hpp" |
||||
#include "impl_msgq.hpp" |
||||
|
||||
Context * Context::create(){ |
||||
Context * c; |
||||
if (std::getenv("MSGQ")){ |
||||
c = new MSGQContext(); |
||||
} else { |
||||
c = new ZMQContext(); |
||||
} |
||||
return c; |
||||
} |
||||
|
||||
SubSocket * SubSocket::create(){ |
||||
SubSocket * s; |
||||
if (std::getenv("MSGQ")){ |
||||
s = new MSGQSubSocket(); |
||||
} else { |
||||
s = new ZMQSubSocket(); |
||||
} |
||||
return s; |
||||
} |
||||
|
||||
SubSocket * SubSocket::create(Context * context, std::string endpoint){ |
||||
SubSocket *s = SubSocket::create(); |
||||
int r = s->connect(context, endpoint, "127.0.0.1"); |
||||
|
||||
if (r == 0) { |
||||
return s; |
||||
} else { |
||||
delete s; |
||||
return NULL; |
||||
} |
||||
} |
||||
|
||||
SubSocket * SubSocket::create(Context * context, std::string endpoint, std::string address){ |
||||
SubSocket *s = SubSocket::create(); |
||||
int r = s->connect(context, endpoint, address); |
||||
|
||||
if (r == 0) { |
||||
return s; |
||||
} else { |
||||
delete s; |
||||
return NULL; |
||||
} |
||||
} |
||||
|
||||
SubSocket * SubSocket::create(Context * context, std::string endpoint, std::string address, bool conflate){ |
||||
SubSocket *s = SubSocket::create(); |
||||
int r = s->connect(context, endpoint, address, conflate); |
||||
|
||||
if (r == 0) { |
||||
return s; |
||||
} else { |
||||
delete s; |
||||
return NULL; |
||||
} |
||||
} |
||||
|
||||
PubSocket * PubSocket::create(){ |
||||
PubSocket * s; |
||||
if (std::getenv("MSGQ")){ |
||||
s = new MSGQPubSocket(); |
||||
} else { |
||||
s = new ZMQPubSocket(); |
||||
} |
||||
return s; |
||||
} |
||||
|
||||
PubSocket * PubSocket::create(Context * context, std::string endpoint){ |
||||
PubSocket *s = PubSocket::create(); |
||||
int r = s->connect(context, endpoint); |
||||
|
||||
if (r == 0) { |
||||
return s; |
||||
} else { |
||||
delete s; |
||||
return NULL; |
||||
} |
||||
} |
||||
|
||||
Poller * Poller::create(){ |
||||
Poller * p; |
||||
if (std::getenv("MSGQ")){ |
||||
p = new MSGQPoller(); |
||||
} else { |
||||
p = new ZMQPoller(); |
||||
} |
||||
return p; |
||||
} |
||||
|
||||
Poller * Poller::create(std::vector<SubSocket*> sockets){ |
||||
Poller * p = Poller::create(); |
||||
|
||||
for (auto s : sockets){ |
||||
p->registerSocket(s); |
||||
} |
||||
return p; |
||||
} |
||||
|
||||
extern "C" Context * messaging_context_create() { |
||||
return Context::create(); |
||||
} |
||||
|
||||
extern "C" SubSocket * messaging_subsocket_create(Context* context, const char* endpoint) { |
||||
return SubSocket::create(context, std::string(endpoint)); |
||||
} |
||||
|
||||
extern "C" PubSocket * messaging_pubsocket_create(Context* context, const char* endpoint) { |
||||
return PubSocket::create(context, std::string(endpoint)); |
||||
} |
||||
|
||||
extern "C" Poller * messaging_poller_create(SubSocket** sockets, int size) { |
||||
std::vector<SubSocket*> socketsVec(sockets, sockets + size); |
||||
return Poller::create(socketsVec); |
||||
} |
||||
@ -0,0 +1,56 @@ |
||||
#pragma once |
||||
#include <cstddef> |
||||
#include <vector> |
||||
#include <string> |
||||
|
||||
#define MSG_MULTIPLE_PUBLISHERS 100 |
||||
|
||||
class Context { |
||||
public: |
||||
virtual void * getRawContext() = 0; |
||||
static Context * create(); |
||||
virtual ~Context(){}; |
||||
}; |
||||
|
||||
class Message { |
||||
public: |
||||
virtual void init(size_t size) = 0; |
||||
virtual void init(char * data, size_t size) = 0; |
||||
virtual void close() = 0; |
||||
virtual size_t getSize() = 0; |
||||
virtual char * getData() = 0; |
||||
virtual ~Message(){}; |
||||
}; |
||||
|
||||
|
||||
class SubSocket { |
||||
public: |
||||
virtual int connect(Context *context, std::string endpoint, std::string address, bool conflate=false) = 0; |
||||
virtual void setTimeout(int timeout) = 0; |
||||
virtual Message *receive(bool non_blocking=false) = 0; |
||||
virtual void * getRawSocket() = 0; |
||||
static SubSocket * create(); |
||||
static SubSocket * create(Context * context, std::string endpoint); |
||||
static SubSocket * create(Context * context, std::string endpoint, std::string address); |
||||
static SubSocket * create(Context * context, std::string endpoint, std::string address, bool conflate); |
||||
virtual ~SubSocket(){}; |
||||
}; |
||||
|
||||
class PubSocket { |
||||
public: |
||||
virtual int connect(Context *context, std::string endpoint) = 0; |
||||
virtual int sendMessage(Message *message) = 0; |
||||
virtual int send(char *data, size_t size) = 0; |
||||
static PubSocket * create(); |
||||
static PubSocket * create(Context * context, std::string endpoint); |
||||
virtual ~PubSocket(){}; |
||||
}; |
||||
|
||||
class Poller { |
||||
public: |
||||
virtual void registerSocket(SubSocket *socket) = 0; |
||||
virtual std::vector<SubSocket*> poll(int timeout) = 0; |
||||
static Poller * create(); |
||||
static Poller * create(std::vector<SubSocket*> sockets); |
||||
virtual ~Poller(){}; |
||||
}; |
||||
@ -0,0 +1,39 @@ |
||||
# distutils: language = c++ |
||||
#cython: language_level=3 |
||||
|
||||
from libcpp.string cimport string |
||||
from libcpp.vector cimport vector |
||||
from libcpp cimport bool |
||||
|
||||
|
||||
cdef extern from "messaging.hpp": |
||||
cdef cppclass Context: |
||||
@staticmethod |
||||
Context * create() |
||||
|
||||
cdef cppclass Message: |
||||
void init(size_t) |
||||
void init(char *, size_t) |
||||
void close() |
||||
size_t getSize() |
||||
char *getData() |
||||
|
||||
cdef cppclass SubSocket: |
||||
@staticmethod |
||||
SubSocket * create() |
||||
int connect(Context *, string, string, bool) |
||||
Message * receive(bool) |
||||
void setTimeout(int) |
||||
|
||||
cdef cppclass PubSocket: |
||||
@staticmethod |
||||
PubSocket * create() |
||||
int connect(Context *, string) |
||||
int sendMessage(Message *) |
||||
int send(char *, size_t) |
||||
|
||||
cdef cppclass Poller: |
||||
@staticmethod |
||||
Poller * create() |
||||
void registerSocket(SubSocket *) |
||||
vector[SubSocket*] poll(int) nogil |
||||
@ -0,0 +1,151 @@ |
||||
# distutils: language = c++ |
||||
# cython: c_string_encoding=ascii, language_level=3 |
||||
|
||||
import sys |
||||
from libcpp.string cimport string |
||||
from libcpp cimport bool |
||||
from libc cimport errno |
||||
|
||||
|
||||
from messaging cimport Context as cppContext |
||||
from messaging cimport SubSocket as cppSubSocket |
||||
from messaging cimport PubSocket as cppPubSocket |
||||
from messaging cimport Poller as cppPoller |
||||
from messaging cimport Message as cppMessage |
||||
|
||||
|
||||
class MessagingError(Exception): |
||||
pass |
||||
|
||||
|
||||
class MultiplePublishersError(MessagingError): |
||||
pass |
||||
|
||||
|
||||
cdef class Context: |
||||
cdef cppContext * context |
||||
|
||||
def __cinit__(self): |
||||
self.context = cppContext.create() |
||||
|
||||
def term(self): |
||||
del self.context |
||||
self.context = NULL |
||||
|
||||
def __dealloc__(self): |
||||
pass |
||||
# Deleting the context will hang if sockets are still active |
||||
# TODO: Figure out a way to make sure the context is closed last |
||||
# del self.context |
||||
|
||||
|
||||
cdef class Poller: |
||||
cdef cppPoller * poller |
||||
cdef list sub_sockets |
||||
|
||||
def __cinit__(self): |
||||
self.sub_sockets = [] |
||||
self.poller = cppPoller.create() |
||||
|
||||
def __dealloc__(self): |
||||
del self.poller |
||||
|
||||
def registerSocket(self, SubSocket socket): |
||||
self.sub_sockets.append(socket) |
||||
self.poller.registerSocket(socket.socket) |
||||
|
||||
def poll(self, timeout): |
||||
sockets = [] |
||||
cdef int t = timeout |
||||
|
||||
with nogil: |
||||
result = self.poller.poll(t) |
||||
|
||||
for s in result: |
||||
socket = SubSocket() |
||||
socket.setPtr(s) |
||||
sockets.append(socket) |
||||
|
||||
return sockets |
||||
|
||||
cdef class SubSocket: |
||||
cdef cppSubSocket * socket |
||||
cdef bool is_owner |
||||
|
||||
def __cinit__(self): |
||||
self.socket = cppSubSocket.create() |
||||
self.is_owner = True |
||||
|
||||
if self.socket == NULL: |
||||
raise MessagingError |
||||
|
||||
def __dealloc__(self): |
||||
if self.is_owner: |
||||
del self.socket |
||||
|
||||
cdef setPtr(self, cppSubSocket * ptr): |
||||
if self.is_owner: |
||||
del self.socket |
||||
|
||||
self.is_owner = False |
||||
self.socket = ptr |
||||
|
||||
def connect(self, Context context, string endpoint, string address=b"127.0.0.1", bool conflate=False): |
||||
r = self.socket.connect(context.context, endpoint, address, conflate) |
||||
|
||||
if r != 0: |
||||
if errno.errno == errno.EADDRINUSE: |
||||
raise MultiplePublishersError |
||||
else: |
||||
raise MessagingError |
||||
|
||||
def setTimeout(self, int timeout): |
||||
self.socket.setTimeout(timeout) |
||||
|
||||
def receive(self, bool non_blocking=False): |
||||
msg = self.socket.receive(non_blocking) |
||||
|
||||
if msg == NULL: |
||||
# If a blocking read returns no message check errno if SIGINT was caught in the C++ code |
||||
if errno.errno == errno.EINTR: |
||||
print("SIGINT received, exiting") |
||||
sys.exit(1) |
||||
|
||||
return None |
||||
else: |
||||
sz = msg.getSize() |
||||
m = msg.getData()[:sz] |
||||
del msg |
||||
|
||||
return m |
||||
|
||||
|
||||
cdef class PubSocket: |
||||
cdef cppPubSocket * socket |
||||
|
||||
def __cinit__(self): |
||||
self.socket = cppPubSocket.create() |
||||
if self.socket == NULL: |
||||
raise MessagingError |
||||
|
||||
def __dealloc__(self): |
||||
del self.socket |
||||
|
||||
def connect(self, Context context, string endpoint): |
||||
r = self.socket.connect(context.context, endpoint) |
||||
|
||||
if r != 0: |
||||
if errno.errno == errno.EADDRINUSE: |
||||
raise MultiplePublishersError |
||||
else: |
||||
raise MessagingError |
||||
|
||||
def send(self, string data): |
||||
length = len(data) |
||||
r = self.socket.send(<char*>data.c_str(), length) |
||||
|
||||
if r != length: |
||||
if errno.errno == errno.EADDRINUSE: |
||||
raise MultiplePublishersError |
||||
else: |
||||
raise MessagingError |
||||
@ -0,0 +1,56 @@ |
||||
import os |
||||
import subprocess |
||||
import sysconfig |
||||
from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module |
||||
|
||||
from Cython.Build import cythonize |
||||
from Cython.Distutils import build_ext |
||||
|
||||
|
||||
def get_ext_filename_without_platform_suffix(filename): |
||||
name, ext = os.path.splitext(filename) |
||||
ext_suffix = sysconfig.get_config_var('EXT_SUFFIX') |
||||
|
||||
if ext_suffix == ext: |
||||
return filename |
||||
|
||||
ext_suffix = ext_suffix.replace(ext, '') |
||||
idx = name.find(ext_suffix) |
||||
|
||||
if idx == -1: |
||||
return filename |
||||
else: |
||||
return name[:idx] + ext |
||||
|
||||
|
||||
class BuildExtWithoutPlatformSuffix(build_ext): |
||||
def get_ext_filename(self, ext_name): |
||||
filename = super().get_ext_filename(ext_name) |
||||
return get_ext_filename_without_platform_suffix(filename) |
||||
|
||||
|
||||
sourcefiles = ['messaging_pyx.pyx'] |
||||
extra_compile_args = ["-std=c++11"] |
||||
libraries = ['zmq'] |
||||
ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg |
||||
|
||||
if ARCH == "aarch64": |
||||
extra_compile_args += ["-Wno-deprecated-register"] |
||||
libraries += ['gnustl_shared'] |
||||
|
||||
setup(name='CAN parser', |
||||
cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, |
||||
ext_modules=cythonize( |
||||
Extension( |
||||
"messaging_pyx", |
||||
language="c++", |
||||
sources=sourcefiles, |
||||
extra_compile_args=extra_compile_args, |
||||
libraries=libraries, |
||||
extra_objects=[ |
||||
os.path.join(os.path.dirname(os.path.realpath(__file__)), '../', 'libmessaging.a'), |
||||
] |
||||
) |
||||
), |
||||
nthreads=4, |
||||
) |
||||
@ -0,0 +1,441 @@ |
||||
#include <iostream> |
||||
#include <cassert> |
||||
#include <cerrno> |
||||
#include <cmath> |
||||
#include <cstring> |
||||
#include <cstdint> |
||||
#include <chrono> |
||||
#include <algorithm> |
||||
#include <cstdlib> |
||||
#include <csignal> |
||||
#include <random> |
||||
|
||||
#include <poll.h> |
||||
#include <sys/ioctl.h> |
||||
#include <sys/mman.h> |
||||
#include <sys/stat.h> |
||||
#include <sys/types.h> |
||||
#include <sys/syscall.h> |
||||
#include <fcntl.h> |
||||
#include <unistd.h> |
||||
|
||||
#include <stdio.h> |
||||
|
||||
#include "msgq.hpp" |
||||
|
||||
void sigusr1_handler(int signal) { |
||||
assert(signal == SIGUSR1); |
||||
} |
||||
|
||||
uint64_t msgq_get_uid(void){ |
||||
std::random_device rd("/dev/urandom"); |
||||
std::uniform_int_distribution<uint64_t> distribution(0,std::numeric_limits<uint32_t>::max()); |
||||
|
||||
uint64_t uid = distribution(rd) << 32 | syscall(SYS_gettid); |
||||
return uid; |
||||
} |
||||
|
||||
int msgq_msg_init_size(msgq_msg_t * msg, size_t size){ |
||||
msg->size = size; |
||||
msg->data = new(std::nothrow) char[size]; |
||||
|
||||
return (msg->data == NULL) ? -1 : 0; |
||||
} |
||||
|
||||
|
||||
int msgq_msg_init_data(msgq_msg_t * msg, char * data, size_t size) { |
||||
int r = msgq_msg_init_size(msg, size); |
||||
|
||||
if (r == 0) |
||||
memcpy(msg->data, data, size); |
||||
|
||||
return r; |
||||
} |
||||
|
||||
int msgq_msg_close(msgq_msg_t * msg){ |
||||
if (msg->size > 0) |
||||
delete[] msg->data; |
||||
|
||||
msg->size = 0; |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
void msgq_reset_reader(msgq_queue_t * q){ |
||||
int id = q->reader_id; |
||||
q->read_valids[id]->store(true); |
||||
q->read_pointers[id]->store(*q->write_pointer); |
||||
} |
||||
|
||||
void msgq_wait_for_subscriber(msgq_queue_t *q){ |
||||
while (*q->num_readers == 0){ |
||||
; |
||||
} |
||||
|
||||
return; |
||||
} |
||||
|
||||
|
||||
|
||||
int msgq_new_queue(msgq_queue_t * q, const char * path, size_t size){ |
||||
assert(size < 0xFFFFFFFF); // Buffer must be smaller than 2^32 bytes
|
||||
|
||||
std::signal(SIGUSR1, sigusr1_handler); |
||||
|
||||
const char * prefix = "/dev/shm/"; |
||||
char * full_path = new char[strlen(path) + strlen(prefix) + 1]; |
||||
strcpy(full_path, prefix); |
||||
strcat(full_path, path); |
||||
|
||||
auto fd = open(full_path, O_RDWR | O_CREAT, 0777); |
||||
delete[] full_path; |
||||
|
||||
if (fd < 0) |
||||
return -1; |
||||
|
||||
int rc = ftruncate(fd, size + sizeof(msgq_header_t)); |
||||
if (rc < 0) |
||||
return -1; |
||||
|
||||
char * mem = (char*)mmap(NULL, size + sizeof(msgq_header_t), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); |
||||
close(fd); |
||||
|
||||
if (mem == NULL) |
||||
return -1; |
||||
|
||||
q->mmap_p = mem; |
||||
|
||||
msgq_header_t *header = (msgq_header_t *)mem; |
||||
|
||||
// Setup pointers to header segment
|
||||
q->num_readers = reinterpret_cast<std::atomic<uint64_t>*>(&header->num_readers); |
||||
q->write_pointer = reinterpret_cast<std::atomic<uint64_t>*>(&header->write_pointer); |
||||
q->write_uid = reinterpret_cast<std::atomic<uint64_t>*>(&header->write_uid); |
||||
|
||||
for (size_t i = 0; i < NUM_READERS; i++){ |
||||
q->read_pointers[i] = reinterpret_cast<std::atomic<uint64_t>*>(&header->read_pointers[i]); |
||||
q->read_valids[i] = reinterpret_cast<std::atomic<uint64_t>*>(&header->read_valids[i]); |
||||
q->read_uids[i] = reinterpret_cast<std::atomic<uint64_t>*>(&header->read_uids[i]); |
||||
} |
||||
|
||||
q->data = mem + sizeof(msgq_header_t); |
||||
q->size = size; |
||||
q->reader_id = -1; |
||||
|
||||
q->endpoint = path; |
||||
q->read_conflate = false; |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
void msgq_close_queue(msgq_queue_t *q){ |
||||
if (q->mmap_p != NULL){ |
||||
munmap(q->mmap_p, q->size + sizeof(msgq_header_t)); |
||||
} |
||||
} |
||||
|
||||
|
||||
void msgq_init_publisher(msgq_queue_t * q) { |
||||
std::cout << "Starting publisher" << std::endl; |
||||
uint64_t uid = msgq_get_uid(); |
||||
|
||||
*q->write_uid = uid; |
||||
*q->num_readers = 0; |
||||
|
||||
for (size_t i = 0; i < NUM_READERS; i++){ |
||||
*q->read_valids[i] = false; |
||||
*q->read_uids[i] = 0; |
||||
} |
||||
|
||||
q->write_uid_local = uid; |
||||
} |
||||
|
||||
void msgq_init_subscriber(msgq_queue_t * q) { |
||||
assert(q != NULL); |
||||
assert(q->num_readers != NULL); |
||||
|
||||
uint64_t uid = msgq_get_uid(); |
||||
|
||||
// Get reader id
|
||||
while (true){ |
||||
uint64_t cur_num_readers = *q->num_readers; |
||||
uint64_t new_num_readers = cur_num_readers + 1; |
||||
|
||||
// No more slots available. Reset all subscribers to kick out inactive ones
|
||||
if (new_num_readers > NUM_READERS){ |
||||
std::cout << "Warning, evicting all subscribers!" << std::endl; |
||||
*q->num_readers = 0; |
||||
|
||||
for (size_t i = 0; i < NUM_READERS; i++){ |
||||
*q->read_valids[i] = false; |
||||
|
||||
uint64_t old_uid = *q->read_uids[i]; |
||||
*q->read_uids[i] = 0; |
||||
|
||||
// Wake up reader in case they are in a poll
|
||||
syscall(SYS_tkill, old_uid & 0xFFFFFFFF, SIGUSR1); |
||||
} |
||||
|
||||
continue; |
||||
} |
||||
|
||||
// Use atomic compare and swap to handle race condition
|
||||
// where two subscribers start at the same time
|
||||
if (std::atomic_compare_exchange_strong(q->num_readers, |
||||
&cur_num_readers, |
||||
new_num_readers)){ |
||||
q->reader_id = cur_num_readers; |
||||
q->read_uid_local = uid; |
||||
|
||||
// We start with read_valid = false,
|
||||
// on the first read the read pointer will be synchronized with the write pointer
|
||||
*q->read_valids[cur_num_readers] = false; |
||||
*q->read_pointers[cur_num_readers] = 0; |
||||
*q->read_uids[cur_num_readers] = uid; |
||||
break; |
||||
} |
||||
} |
||||
|
||||
std::cout << "New subscriber id: " << q->reader_id << " uid: " << q->read_uid_local << " " << q->endpoint << std::endl; |
||||
msgq_reset_reader(q); |
||||
} |
||||
|
||||
int msgq_msg_send(msgq_msg_t * msg, msgq_queue_t *q){ |
||||
// Die if we are no longer the active publisher
|
||||
if (q->write_uid_local != *q->write_uid){ |
||||
std::cout << "Killing old publisher: " << q->endpoint << std::endl; |
||||
errno = EADDRINUSE; |
||||
return -1; |
||||
} |
||||
|
||||
uint64_t total_msg_size = ALIGN(msg->size + sizeof(int64_t)); |
||||
|
||||
// We need to fit at least three messages in the queue,
|
||||
// then we can always safely access the last message
|
||||
assert(3 * total_msg_size <= q->size); |
||||
|
||||
uint64_t num_readers = *q->num_readers; |
||||
|
||||
uint32_t write_cycles, write_pointer; |
||||
UNPACK64(write_cycles, write_pointer, *q->write_pointer); |
||||
|
||||
char *p = q->data + write_pointer; // add base offset
|
||||
|
||||
// Check remaining space
|
||||
// Always leave space for a wraparound tag for the next message, including alignment
|
||||
int64_t remaining_space = q->size - write_pointer - total_msg_size - sizeof(int64_t); |
||||
if (remaining_space <= 0){ |
||||
// Write -1 size tag indicating wraparound
|
||||
*(int64_t*)p = -1; |
||||
|
||||
// Invalidate all readers that are beyond the write pointer
|
||||
// TODO: should we handle the case where a new reader shows up while this is running?
|
||||
for (uint64_t i = 0; i < num_readers; i++){ |
||||
uint64_t read_pointer = *q->read_pointers[i]; |
||||
uint64_t read_cycles = read_pointer >> 32; |
||||
read_pointer &= 0xFFFFFFFF; |
||||
|
||||
if ((read_pointer > write_pointer) && (read_cycles != write_cycles)) { |
||||
*q->read_valids[i] = false; |
||||
} |
||||
} |
||||
|
||||
// Update global and local copies of write pointer and write_cycles
|
||||
write_pointer = 0; |
||||
write_cycles = write_cycles + 1; |
||||
PACK64(*q->write_pointer, write_cycles, write_pointer); |
||||
|
||||
// Set actual pointer to the beginning of the data segment
|
||||
p = q->data; |
||||
} |
||||
|
||||
// Invalidate readers that are in the area that will be written
|
||||
uint64_t start = write_pointer; |
||||
uint64_t end = ALIGN(start + sizeof(int64_t) + msg->size); |
||||
|
||||
for (uint64_t i = 0; i < num_readers; i++){ |
||||
uint32_t read_cycles, read_pointer; |
||||
UNPACK64(read_cycles, read_pointer, *q->read_pointers[i]); |
||||
|
||||
if ((read_pointer >= start) && (read_pointer < end) && (read_cycles != write_cycles)) { |
||||
*q->read_valids[i] = false; |
||||
} |
||||
} |
||||
|
||||
|
||||
// Write size tag
|
||||
std::atomic<int64_t> *size_p = reinterpret_cast<std::atomic<int64_t>*>(p); |
||||
*size_p = msg->size; |
||||
|
||||
// Copy data
|
||||
memcpy(p + sizeof(int64_t), msg->data, msg->size); |
||||
__sync_synchronize(); |
||||
|
||||
// Update write pointer
|
||||
uint32_t new_ptr = ALIGN(write_pointer + msg->size + sizeof(int64_t)); |
||||
PACK64(*q->write_pointer, write_cycles, new_ptr); |
||||
|
||||
// Notify readers
|
||||
for (uint64_t i = 0; i < num_readers; i++){ |
||||
uint64_t reader_uid = *q->read_uids[i]; |
||||
|
||||
syscall(SYS_tkill, reader_uid & 0xFFFFFFFF, SIGUSR1); |
||||
} |
||||
|
||||
return msg->size; |
||||
} |
||||
|
||||
|
||||
int msgq_msg_ready(msgq_queue_t * q){ |
||||
start: |
||||
int id = q->reader_id; |
||||
assert(id >= 0); // Make sure subscriber is initialized
|
||||
|
||||
if (q->read_uid_local != *q->read_uids[id]){ |
||||
std::cout << q->endpoint << ": Reader was evicted, reconnecting" << std::endl; |
||||
msgq_init_subscriber(q); |
||||
goto start; |
||||
} |
||||
|
||||
// Check valid
|
||||
if (!*q->read_valids[id]){ |
||||
msgq_reset_reader(q); |
||||
goto start; |
||||
} |
||||
|
||||
uint32_t read_cycles, read_pointer; |
||||
UNPACK64(read_cycles, read_pointer, *q->read_pointers[id]); |
||||
|
||||
uint32_t write_cycles, write_pointer; |
||||
UNPACK64(write_cycles, write_pointer, *q->write_pointer); |
||||
|
||||
// Check if new message is available
|
||||
return (read_pointer != write_pointer); |
||||
} |
||||
|
||||
int msgq_msg_recv(msgq_msg_t * msg, msgq_queue_t * q){ |
||||
start: |
||||
int id = q->reader_id; |
||||
assert(id >= 0); // Make sure subscriber is initialized
|
||||
|
||||
if (q->read_uid_local != *q->read_uids[id]){ |
||||
std::cout << q->endpoint << ": Reader was evicted, reconnecting" << std::endl; |
||||
msgq_init_subscriber(q); |
||||
goto start; |
||||
} |
||||
|
||||
// Check valid
|
||||
if (!*q->read_valids[id]){ |
||||
msgq_reset_reader(q); |
||||
goto start; |
||||
} |
||||
|
||||
uint32_t read_cycles, read_pointer; |
||||
UNPACK64(read_cycles, read_pointer, *q->read_pointers[id]); |
||||
|
||||
uint32_t write_cycles, write_pointer; |
||||
UNPACK64(write_cycles, write_pointer, *q->write_pointer); |
||||
|
||||
char * p = q->data + read_pointer; |
||||
|
||||
// Check if new message is available
|
||||
if (read_pointer == write_pointer) { |
||||
msg->size = 0; |
||||
return 0; |
||||
} |
||||
|
||||
// Read potential message size
|
||||
std::atomic<int64_t> *size_p = reinterpret_cast<std::atomic<int64_t>*>(p); |
||||
std::int64_t size = *size_p; |
||||
|
||||
// Check if the size that was read is valid
|
||||
if (!*q->read_valids[id]){ |
||||
msgq_reset_reader(q); |
||||
goto start; |
||||
} |
||||
|
||||
// If size is -1 the buffer was full, and we need to wrap around
|
||||
if (size == -1){ |
||||
read_cycles++; |
||||
PACK64(*q->read_pointers[id], read_cycles, 0); |
||||
goto start; |
||||
} |
||||
|
||||
// crashing is better than passing garbage data to the consumer
|
||||
// the size will have weird value if it was overwritten by data accidentally
|
||||
assert((uint64_t)size < q->size); |
||||
assert(size > 0); |
||||
|
||||
uint32_t new_read_pointer = ALIGN(read_pointer + sizeof(std::int64_t) + size); |
||||
|
||||
// If conflate is true, check if this is the latest message, else start over
|
||||
if (q->read_conflate){ |
||||
if (new_read_pointer != write_pointer){ |
||||
// Update read pointer
|
||||
PACK64(*q->read_pointers[id], read_cycles, new_read_pointer); |
||||
goto start; |
||||
} |
||||
} |
||||
|
||||
// Copy message
|
||||
if (msgq_msg_init_size(msg, size) < 0) |
||||
return -1; |
||||
|
||||
__sync_synchronize(); |
||||
memcpy(msg->data, p + sizeof(int64_t), size); |
||||
__sync_synchronize(); |
||||
|
||||
// Update read pointer
|
||||
PACK64(*q->read_pointers[id], read_cycles, new_read_pointer); |
||||
|
||||
// Check if the actual data that was copied is valid
|
||||
if (!*q->read_valids[id]){ |
||||
msgq_msg_close(msg); |
||||
msgq_reset_reader(q); |
||||
goto start; |
||||
} |
||||
|
||||
|
||||
return msg->size; |
||||
} |
||||
|
||||
|
||||
|
||||
int msgq_poll(msgq_pollitem_t * items, size_t nitems, int timeout){ |
||||
assert(timeout >= 0); |
||||
|
||||
int num = 0; |
||||
|
||||
// Check if messages ready
|
||||
for (size_t i = 0; i < nitems; i++) { |
||||
items[i].revents = msgq_msg_ready(items[i].q); |
||||
if (items[i].revents) num++; |
||||
} |
||||
|
||||
int ms = (timeout == -1) ? 100 : timeout; |
||||
struct timespec ts; |
||||
ts.tv_sec = ms / 1000; |
||||
ts.tv_nsec = (ms % 1000) * 1000 * 1000; |
||||
|
||||
|
||||
while (num == 0) { |
||||
int ret; |
||||
|
||||
ret = nanosleep(&ts, &ts); |
||||
|
||||
// Check if messages ready
|
||||
for (size_t i = 0; i < nitems; i++) { |
||||
if (items[i].revents == 0 && msgq_msg_ready(items[i].q)){ |
||||
num += 1; |
||||
items[i].revents = 1; |
||||
} |
||||
} |
||||
|
||||
// exit if we had a timeout and the sleep finished
|
||||
if (timeout != -1 && ret == 0){ |
||||
break; |
||||
} |
||||
} |
||||
|
||||
return num; |
||||
} |
||||
@ -0,0 +1,66 @@ |
||||
#pragma once |
||||
#include <cstdint> |
||||
#include <cstring> |
||||
#include <string> |
||||
#include <atomic> |
||||
|
||||
#define DEFAULT_SEGMENT_SIZE (10 * 1024 * 1024) |
||||
#define NUM_READERS 8 |
||||
#define ALIGN(n) ((n + (8 - 1)) & -8) |
||||
|
||||
#define UNPACK64(higher, lower, input) do {uint64_t tmp = input; higher = tmp >> 32; lower = tmp & 0xFFFFFFFF;} while (0) |
||||
#define PACK64(output, higher, lower) output = ((uint64_t)higher << 32 ) | ((uint64_t)lower & 0xFFFFFFFF) |
||||
|
||||
struct msgq_header_t { |
||||
uint64_t num_readers; |
||||
uint64_t write_pointer; |
||||
uint64_t write_uid; |
||||
uint64_t read_pointers[NUM_READERS]; |
||||
uint64_t read_valids[NUM_READERS]; |
||||
uint64_t read_uids[NUM_READERS]; |
||||
}; |
||||
|
||||
struct msgq_queue_t { |
||||
std::atomic<uint64_t> *num_readers; |
||||
std::atomic<uint64_t> *write_pointer; |
||||
std::atomic<uint64_t> *write_uid; |
||||
std::atomic<uint64_t> *read_pointers[NUM_READERS]; |
||||
std::atomic<uint64_t> *read_valids[NUM_READERS]; |
||||
std::atomic<uint64_t> *read_uids[NUM_READERS]; |
||||
char * mmap_p; |
||||
char * data; |
||||
size_t size; |
||||
int reader_id; |
||||
uint64_t read_uid_local; |
||||
uint64_t write_uid_local; |
||||
|
||||
bool read_conflate; |
||||
std::string endpoint; |
||||
}; |
||||
|
||||
struct msgq_msg_t { |
||||
size_t size; |
||||
char * data; |
||||
}; |
||||
|
||||
struct msgq_pollitem_t { |
||||
msgq_queue_t *q; |
||||
int revents; |
||||
}; |
||||
|
||||
void msgq_wait_for_subscriber(msgq_queue_t *q); |
||||
void msgq_reset_reader(msgq_queue_t *q); |
||||
|
||||
int msgq_msg_init_size(msgq_msg_t *msg, size_t size); |
||||
int msgq_msg_init_data(msgq_msg_t *msg, char * data, size_t size); |
||||
int msgq_msg_close(msgq_msg_t *msg); |
||||
|
||||
int msgq_new_queue(msgq_queue_t * q, const char * path, size_t size); |
||||
void msgq_close_queue(msgq_queue_t *q); |
||||
void msgq_init_publisher(msgq_queue_t * q); |
||||
void msgq_init_subscriber(msgq_queue_t * q); |
||||
|
||||
int msgq_msg_send(msgq_msg_t *msg, msgq_queue_t *q); |
||||
int msgq_msg_recv(msgq_msg_t *msg, msgq_queue_t *q); |
||||
int msgq_msg_ready(msgq_queue_t * q); |
||||
int msgq_poll(msgq_pollitem_t * items, size_t nitems, int timeout); |
||||
@ -0,0 +1,56 @@ |
||||
# MSGQ: A lock free single producer multi consumer message queue |
||||
|
||||
[](https://dev.azure.com/commaai/default/_build/latest?definitionId=21&branchName=master) |
||||
|
||||
## What is MSGQ? |
||||
MSGQ is a system to pass messages from a single producer to multiple consumers. All the consumers need to be able to receive all the messages. It is designed to be a high performance replacement for ZMQ-like SUB/PUB patterns. It uses a ring buffer in shared memory to efficiently read and write data. Each read requires a copy. Writing can be done without a copy, as long as the size of the data is known in advance. |
||||
|
||||
## Storage |
||||
The storage for the queue consists of an area of metadata, and the actual buffer. The metadata contains: |
||||
|
||||
1. A counter to the number of readers that are active |
||||
2. A pointer to the head of the queue for writing. From now on referred to as *write pointer* |
||||
3. A cycle counter for the writer. This counter is incremented when the writer wraps around |
||||
4. N pointers, pointing to the current read position for all the readers. From now on referred to as *read pointer* |
||||
5. N counters, counting the number of cycles for all the readers |
||||
6. N booleans, indicating validity for all the readers. From now on referred to as *validity flag* |
||||
|
||||
The counter and the pointer are both 32 bit values, packed into 64 bit so they can be read and written atomically. |
||||
|
||||
The data buffer is a ring buffer. All messages are prefixed by an 8 byte size field, followed by the data. A size of -1 indicates a wrap-around, and means the next message is stored at the beginning of the buffer. |
||||
|
||||
|
||||
## Writing |
||||
Writing involves the following steps: |
||||
|
||||
1. Check if the area that is to be written overlaps with any of the read pointers, mark those readers as invalid by clearing the validity flag. |
||||
2. Write the message |
||||
3. Increase the write pointer by the size of the message |
||||
|
||||
In case there is not enough space at the end of the buffer, a special empty message with a prefix of -1 is written. The cycle counter is incremented by one. In this case step 1 will check there are no read pointers pointing to the remainder of the buffer. Then another write cycle will start with the actual message. |
||||
|
||||
There always needs to be 8 bytes of empty space at the end of the buffer. By doing this there is always space to write the -1. |
||||
|
||||
## Reset reader |
||||
When the reader is lagging too much behind the read pointer becomes invalid and no longer points to the beginning of a valid message. To reset a reader to the current write pointer, the following steps are performed: |
||||
|
||||
1. Set valid flag |
||||
2. Set read cycle counter to that of the writer |
||||
3. Set read pointer to write pointer |
||||
|
||||
## Reading |
||||
Reading involves the following steps: |
||||
|
||||
1. Read the size field at the current read pointer |
||||
2. Read the validity flag |
||||
3. Copy the data out of the buffer |
||||
4. Increase the read pointer by the size of the message |
||||
5. Check the validity flag again |
||||
|
||||
Before starting the copy, the valid flag is checked. This is to prevent a race condition where the size prefix was invalid, and the read could read outside of the buffer. Make sure that step 1 and 2 are not reordered by your compiler or CPU. |
||||
|
||||
If a writer overwrites the data while it's being copied out, the data will be invalid. Therefore the validity flag is also checked after reading it. The order of step 4 and 5 does not matter. |
||||
|
||||
If at steps 2 or 5 the validity flag is not set, the reader is reset. Any data that was already read is discarded. After the reader is reset, the reading starts from the beginning. |
||||
|
||||
If a message with size -1 is encountered, step 3 and 4 are replaced by increasing the cycle counter and setting the read pointer to the beginning of the buffer. After that another read is performed. |
||||
@ -0,0 +1,395 @@ |
||||
#include "catch2/catch.hpp" |
||||
#include "msgq.hpp" |
||||
|
||||
TEST_CASE("ALIGN"){ |
||||
REQUIRE(ALIGN(0) == 0); |
||||
REQUIRE(ALIGN(1) == 8); |
||||
REQUIRE(ALIGN(7) == 8); |
||||
REQUIRE(ALIGN(8) == 8); |
||||
REQUIRE(ALIGN(99999) == 100000); |
||||
} |
||||
|
||||
TEST_CASE("msgq_msg_init_size"){ |
||||
const size_t msg_size = 30; |
||||
msgq_msg_t msg; |
||||
|
||||
msgq_msg_init_size(&msg, msg_size); |
||||
REQUIRE(msg.size == msg_size); |
||||
|
||||
msgq_msg_close(&msg); |
||||
} |
||||
|
||||
TEST_CASE("msgq_msg_init_data"){ |
||||
const size_t msg_size = 30; |
||||
char * data = new char[msg_size]; |
||||
|
||||
for (size_t i = 0; i < msg_size; i++){ |
||||
data[i] = i; |
||||
} |
||||
|
||||
msgq_msg_t msg; |
||||
msgq_msg_init_data(&msg, data, msg_size); |
||||
|
||||
REQUIRE(msg.size == msg_size); |
||||
REQUIRE(memcmp(msg.data, data, msg_size) == 0); |
||||
|
||||
delete[] data; |
||||
msgq_msg_close(&msg); |
||||
} |
||||
|
||||
|
||||
TEST_CASE("msgq_init_subscriber"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t q; |
||||
msgq_new_queue(&q, "test_queue", 1024); |
||||
REQUIRE(*q.num_readers == 0); |
||||
|
||||
q.reader_id = 1; |
||||
*q.read_valids[0] = false; |
||||
*q.read_pointers[0] = ((uint64_t)1 << 32); |
||||
|
||||
*q.write_pointer = 255; |
||||
|
||||
msgq_init_subscriber(&q); |
||||
REQUIRE(q.read_conflate == false); |
||||
REQUIRE(*q.read_valids[0] == true); |
||||
REQUIRE((*q.read_pointers[0] >> 32) == 0); |
||||
REQUIRE((*q.read_pointers[0] & 0xFFFFFFFF) == 255); |
||||
} |
||||
|
||||
TEST_CASE("msgq_msg_send first message"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t q; |
||||
msgq_new_queue(&q, "test_queue", 1024); |
||||
msgq_init_publisher(&q); |
||||
|
||||
REQUIRE(*q.write_pointer == 0); |
||||
|
||||
size_t msg_size = 128; |
||||
|
||||
SECTION("Aligned message size"){ |
||||
} |
||||
SECTION("Unaligned message size"){ |
||||
msg_size--; |
||||
} |
||||
|
||||
char * data = new char[msg_size]; |
||||
|
||||
for (size_t i = 0; i < msg_size; i++){ |
||||
data[i] = i; |
||||
} |
||||
|
||||
msgq_msg_t msg; |
||||
msgq_msg_init_data(&msg, data, msg_size); |
||||
|
||||
|
||||
msgq_msg_send(&msg, &q); |
||||
REQUIRE(*(int64_t*)q.data == msg_size); // Check size tag
|
||||
REQUIRE(*q.write_pointer == 128 + sizeof(int64_t)); |
||||
REQUIRE(memcmp(q.data + sizeof(int64_t), data, msg_size) == 0); |
||||
|
||||
delete[] data; |
||||
msgq_msg_close(&msg); |
||||
} |
||||
|
||||
TEST_CASE("msgq_msg_send test wraparound"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t q; |
||||
msgq_new_queue(&q, "test_queue", 1024); |
||||
msgq_init_publisher(&q); |
||||
|
||||
REQUIRE((*q.write_pointer & 0xFFFFFFFF) == 0); |
||||
REQUIRE((*q.write_pointer >> 32) == 0); |
||||
|
||||
const size_t msg_size = 120; |
||||
msgq_msg_t msg; |
||||
msgq_msg_init_size(&msg, msg_size); |
||||
|
||||
for (int i = 0; i < 8; i++) { |
||||
msgq_msg_send(&msg, &q); |
||||
} |
||||
// Check 8th message was written at the beginning
|
||||
REQUIRE((*q.write_pointer & 0xFFFFFFFF) == msg_size + sizeof(int64_t)); |
||||
|
||||
// Check cycle count
|
||||
REQUIRE((*q.write_pointer >> 32) == 1); |
||||
|
||||
// Check wraparound tag
|
||||
char * tag_location = q.data; |
||||
tag_location += 7 * (msg_size + sizeof(int64_t)); |
||||
REQUIRE(*(int64_t*)tag_location == -1); |
||||
|
||||
msgq_msg_close(&msg); |
||||
} |
||||
|
||||
TEST_CASE("msgq_msg_recv test wraparound"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t q_pub, q_sub; |
||||
msgq_new_queue(&q_pub, "test_queue", 1024); |
||||
msgq_new_queue(&q_sub, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&q_pub); |
||||
msgq_init_subscriber(&q_sub); |
||||
|
||||
REQUIRE((*q_pub.write_pointer >> 32) == 0); |
||||
REQUIRE((*q_sub.read_pointers[0] >> 32) == 0); |
||||
|
||||
const size_t msg_size = 120; |
||||
msgq_msg_t msg1; |
||||
msgq_msg_init_size(&msg1, msg_size); |
||||
|
||||
|
||||
SECTION("Check cycle counter after reset") { |
||||
for (int i = 0; i < 8; i++) { |
||||
msgq_msg_send(&msg1, &q_pub); |
||||
} |
||||
|
||||
msgq_msg_t msg2; |
||||
msgq_msg_recv(&msg2, &q_sub); |
||||
REQUIRE(msg2.size == 0); // Reader had to reset
|
||||
msgq_msg_close(&msg2); |
||||
|
||||
} |
||||
SECTION("Check cycle counter while keeping up with writer") { |
||||
for (int i = 0; i < 8; i++) { |
||||
msgq_msg_send(&msg1, &q_pub); |
||||
|
||||
msgq_msg_t msg2; |
||||
msgq_msg_recv(&msg2, &q_sub); |
||||
REQUIRE(msg2.size > 0); |
||||
msgq_msg_close(&msg2); |
||||
} |
||||
} |
||||
|
||||
REQUIRE((*q_sub.read_pointers[0] >> 32) == 1); |
||||
msgq_msg_close(&msg1); |
||||
} |
||||
|
||||
TEST_CASE("msgq_msg_send test invalidation"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t q_pub, q_sub; |
||||
msgq_new_queue(&q_pub, "test_queue", 1024); |
||||
msgq_new_queue(&q_sub, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&q_pub); |
||||
msgq_init_subscriber(&q_sub); |
||||
*q_sub.write_pointer = (uint64_t)1 << 32; |
||||
|
||||
REQUIRE(*q_sub.read_valids[0] == true); |
||||
|
||||
SECTION("read pointer in tag"){ |
||||
*q_sub.read_pointers[0] = 0; |
||||
} |
||||
SECTION("read pointer in data section"){ |
||||
*q_sub.read_pointers[0] = 64; |
||||
} |
||||
SECTION("read pointer in wraparound section"){ |
||||
*q_pub.write_pointer = ((uint64_t)1 << 32) | 1000; // Writer is one cycle ahead
|
||||
*q_sub.read_pointers[0] = 1020; |
||||
} |
||||
|
||||
msgq_msg_t msg; |
||||
msgq_msg_init_size(&msg, 128); |
||||
msgq_msg_send(&msg, &q_pub); |
||||
|
||||
REQUIRE(*q_sub.read_valids[0] == false); |
||||
|
||||
msgq_msg_close(&msg); |
||||
} |
||||
|
||||
TEST_CASE("msgq_init_subscriber init 2 subscribers"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t q1, q2; |
||||
msgq_new_queue(&q1, "test_queue", 1024); |
||||
msgq_new_queue(&q2, "test_queue", 1024); |
||||
|
||||
*q1.num_readers = 0; |
||||
|
||||
REQUIRE(*q1.num_readers == 0); |
||||
REQUIRE(*q2.num_readers == 0); |
||||
|
||||
msgq_init_subscriber(&q1); |
||||
REQUIRE(*q1.num_readers == 1); |
||||
REQUIRE(*q2.num_readers == 1); |
||||
REQUIRE(q1.reader_id == 0); |
||||
|
||||
msgq_init_subscriber(&q2); |
||||
REQUIRE(*q1.num_readers == 2); |
||||
REQUIRE(*q2.num_readers == 2); |
||||
REQUIRE(q2.reader_id == 1); |
||||
} |
||||
|
||||
|
||||
TEST_CASE("Write 1 msg, read 1 msg", "[integration]"){ |
||||
remove("/dev/shm/test_queue"); |
||||
const size_t msg_size = 128; |
||||
msgq_queue_t writer, reader; |
||||
|
||||
msgq_new_queue(&writer, "test_queue", 1024); |
||||
msgq_new_queue(&reader, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&writer); |
||||
msgq_init_subscriber(&reader); |
||||
|
||||
// Build 128 byte message
|
||||
msgq_msg_t outgoing_msg; |
||||
msgq_msg_init_size(&outgoing_msg, msg_size); |
||||
|
||||
for (size_t i = 0; i < msg_size; i++){ |
||||
outgoing_msg.data[i] = i; |
||||
} |
||||
|
||||
REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); |
||||
|
||||
msgq_msg_t incoming_msg1; |
||||
REQUIRE(msgq_msg_recv(&incoming_msg1, &reader) == msg_size); |
||||
REQUIRE(memcmp(incoming_msg1.data, outgoing_msg.data, msg_size) == 0); |
||||
|
||||
// Verify that there are no more messages
|
||||
msgq_msg_t incoming_msg2; |
||||
REQUIRE(msgq_msg_recv(&incoming_msg2, &reader) == 0); |
||||
|
||||
msgq_msg_close(&outgoing_msg); |
||||
msgq_msg_close(&incoming_msg1); |
||||
msgq_msg_close(&incoming_msg2); |
||||
} |
||||
|
||||
TEST_CASE("Write 2 msg, read 2 msg - conflate = false", "[integration]"){ |
||||
remove("/dev/shm/test_queue"); |
||||
const size_t msg_size = 128; |
||||
msgq_queue_t writer, reader; |
||||
|
||||
msgq_new_queue(&writer, "test_queue", 1024); |
||||
msgq_new_queue(&reader, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&writer); |
||||
msgq_init_subscriber(&reader); |
||||
|
||||
// Build 128 byte message
|
||||
msgq_msg_t outgoing_msg; |
||||
msgq_msg_init_size(&outgoing_msg, msg_size); |
||||
|
||||
for (size_t i = 0; i < msg_size; i++){ |
||||
outgoing_msg.data[i] = i; |
||||
} |
||||
|
||||
REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); |
||||
REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); |
||||
|
||||
msgq_msg_t incoming_msg1; |
||||
REQUIRE(msgq_msg_recv(&incoming_msg1, &reader) == msg_size); |
||||
REQUIRE(memcmp(incoming_msg1.data, outgoing_msg.data, msg_size) == 0); |
||||
|
||||
msgq_msg_t incoming_msg2; |
||||
REQUIRE(msgq_msg_recv(&incoming_msg2, &reader) == msg_size); |
||||
REQUIRE(memcmp(incoming_msg2.data, outgoing_msg.data, msg_size) == 0); |
||||
|
||||
msgq_msg_close(&outgoing_msg); |
||||
msgq_msg_close(&incoming_msg1); |
||||
msgq_msg_close(&incoming_msg2); |
||||
} |
||||
|
||||
TEST_CASE("Write 2 msg, read 2 msg - conflate = true", "[integration]"){ |
||||
remove("/dev/shm/test_queue"); |
||||
const size_t msg_size = 128; |
||||
msgq_queue_t writer, reader; |
||||
|
||||
msgq_new_queue(&writer, "test_queue", 1024); |
||||
msgq_new_queue(&reader, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&writer); |
||||
msgq_init_subscriber(&reader); |
||||
reader.read_conflate = true; |
||||
|
||||
// Build 128 byte message
|
||||
msgq_msg_t outgoing_msg; |
||||
msgq_msg_init_size(&outgoing_msg, msg_size); |
||||
|
||||
for (size_t i = 0; i < msg_size; i++){ |
||||
outgoing_msg.data[i] = i; |
||||
} |
||||
|
||||
REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); |
||||
REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); |
||||
|
||||
msgq_msg_t incoming_msg1; |
||||
REQUIRE(msgq_msg_recv(&incoming_msg1, &reader) == msg_size); |
||||
REQUIRE(memcmp(incoming_msg1.data, outgoing_msg.data, msg_size) == 0); |
||||
|
||||
// Verify that there are no more messages
|
||||
msgq_msg_t incoming_msg2; |
||||
REQUIRE(msgq_msg_recv(&incoming_msg2, &reader) == 0); |
||||
|
||||
msgq_msg_close(&outgoing_msg); |
||||
msgq_msg_close(&incoming_msg1); |
||||
msgq_msg_close(&incoming_msg2); |
||||
} |
||||
|
||||
TEST_CASE("1 publisher, 1 slow subscriber", "[integration]"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t writer, reader; |
||||
|
||||
msgq_new_queue(&writer, "test_queue", 1024); |
||||
msgq_new_queue(&reader, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&writer); |
||||
msgq_init_subscriber(&reader); |
||||
|
||||
int n_received = 0; |
||||
int n_skipped = 0; |
||||
|
||||
for (uint64_t i = 0; i < 1e5; i++) { |
||||
msgq_msg_t outgoing_msg; |
||||
msgq_msg_init_data(&outgoing_msg, (char*)&i, sizeof(uint64_t)); |
||||
msgq_msg_send(&outgoing_msg, &writer); |
||||
msgq_msg_close(&outgoing_msg); |
||||
|
||||
if (i % 10 == 0){ |
||||
msgq_msg_t msg1; |
||||
msgq_msg_recv(&msg1, &reader); |
||||
|
||||
if (msg1.size == 0){ |
||||
n_skipped++; |
||||
} else { |
||||
n_received++; |
||||
} |
||||
msgq_msg_close(&msg1); |
||||
} |
||||
} |
||||
|
||||
// TODO: verify these numbers by hand
|
||||
REQUIRE(n_received == 8572); |
||||
REQUIRE(n_skipped == 1428); |
||||
} |
||||
|
||||
TEST_CASE("1 publisher, 2 subscribers", "[integration]"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t writer, reader1, reader2; |
||||
|
||||
msgq_new_queue(&writer, "test_queue", 1024); |
||||
msgq_new_queue(&reader1, "test_queue", 1024); |
||||
msgq_new_queue(&reader2, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&writer); |
||||
msgq_init_subscriber(&reader1); |
||||
msgq_init_subscriber(&reader2); |
||||
|
||||
for (uint64_t i = 0; i < 1024 * 3; i++) { |
||||
msgq_msg_t outgoing_msg; |
||||
msgq_msg_init_data(&outgoing_msg, (char*)&i, sizeof(uint64_t)); |
||||
msgq_msg_send(&outgoing_msg, &writer); |
||||
|
||||
msgq_msg_t msg1, msg2; |
||||
msgq_msg_recv(&msg1, &reader1); |
||||
msgq_msg_recv(&msg2, &reader2); |
||||
|
||||
REQUIRE(msg1.size == sizeof(uint64_t)); |
||||
REQUIRE(msg2.size == sizeof(uint64_t)); |
||||
REQUIRE(*(uint64_t*)msg1.data == i); |
||||
REQUIRE(*(uint64_t*)msg2.data == i); |
||||
|
||||
msgq_msg_close(&outgoing_msg); |
||||
msgq_msg_close(&msg1); |
||||
msgq_msg_close(&msg2); |
||||
} |
||||
} |
||||
@ -0,0 +1,14 @@ |
||||
from messaging_pyx import Context, SubSocket, PubSocket # pylint: disable=no-name-in-module, import-error |
||||
|
||||
if __name__ == "__main__": |
||||
c = Context() |
||||
pub_sock = PubSocket() |
||||
pub_sock.connect(c, "controlsState") |
||||
|
||||
for i in range(int(1e10)): |
||||
print(i) |
||||
sub_sock = SubSocket() |
||||
sub_sock.connect(c, "controlsState") |
||||
|
||||
pub_sock.send(b'a') |
||||
print(sub_sock.receive()) |
||||
@ -0,0 +1,2 @@ |
||||
#define CATCH_CONFIG_MAIN |
||||
#include "catch2/catch.hpp" |
||||
@ -0,0 +1,142 @@ |
||||
import unittest |
||||
import time |
||||
import cereal.messaging as messaging |
||||
|
||||
import concurrent.futures |
||||
|
||||
|
||||
def poller(): |
||||
context = messaging.Context() |
||||
|
||||
p = messaging.Poller() |
||||
|
||||
sub = messaging.SubSocket() |
||||
sub.connect(context, 'controlsState') |
||||
p.registerSocket(sub) |
||||
|
||||
socks = p.poll(10000) |
||||
r = [s.receive(non_blocking=True) for s in socks] |
||||
|
||||
return r |
||||
|
||||
|
||||
class TestPoller(unittest.TestCase): |
||||
def test_poll_once(self): |
||||
context = messaging.Context() |
||||
|
||||
pub = messaging.PubSocket() |
||||
pub.connect(context, 'controlsState') |
||||
|
||||
with concurrent.futures.ThreadPoolExecutor() as e: |
||||
poll = e.submit(poller) |
||||
|
||||
time.sleep(0.1) # Slow joiner syndrome |
||||
|
||||
# Send message |
||||
pub.send("a") |
||||
|
||||
# Wait for poll result |
||||
result = poll.result() |
||||
|
||||
del pub |
||||
context.term() |
||||
|
||||
self.assertEqual(result, [b"a"]) |
||||
|
||||
def test_poll_and_create_many_subscribers(self): |
||||
context = messaging.Context() |
||||
|
||||
pub = messaging.PubSocket() |
||||
pub.connect(context, 'controlsState') |
||||
|
||||
with concurrent.futures.ThreadPoolExecutor() as e: |
||||
poll = e.submit(poller) |
||||
|
||||
time.sleep(0.1) # Slow joiner syndrome |
||||
c = messaging.Context() |
||||
for _ in range(10): |
||||
messaging.SubSocket().connect(c, 'controlsState') |
||||
|
||||
time.sleep(0.1) |
||||
|
||||
# Send message |
||||
pub.send("a") |
||||
|
||||
# Wait for poll result |
||||
result = poll.result() |
||||
|
||||
del pub |
||||
context.term() |
||||
|
||||
self.assertEqual(result, [b"a"]) |
||||
|
||||
def test_multiple_publishers_exception(self): |
||||
context = messaging.Context() |
||||
|
||||
with self.assertRaises(messaging.MultiplePublishersError): |
||||
pub1 = messaging.PubSocket() |
||||
pub1.connect(context, 'controlsState') |
||||
|
||||
pub2 = messaging.PubSocket() |
||||
pub2.connect(context, 'controlsState') |
||||
|
||||
pub1.send("a") |
||||
|
||||
del pub1 |
||||
del pub2 |
||||
context.term() |
||||
|
||||
def test_multiple_messages(self): |
||||
context = messaging.Context() |
||||
|
||||
pub = messaging.PubSocket() |
||||
pub.connect(context, 'controlsState') |
||||
|
||||
sub = messaging.SubSocket() |
||||
sub.connect(context, 'controlsState') |
||||
|
||||
time.sleep(0.1) # Slow joiner |
||||
|
||||
for i in range(100): |
||||
pub.send(str(i)) |
||||
|
||||
msg_seen = False |
||||
i = 0 |
||||
while True: |
||||
r = sub.receive(non_blocking=True) |
||||
|
||||
if r is not None: |
||||
self.assertEqual(str(i), r.decode('utf8')) |
||||
|
||||
msg_seen = True |
||||
i += 1 |
||||
|
||||
if r is None and msg_seen: # ZMQ sometimes receives nothing on the first receive |
||||
break |
||||
|
||||
del pub |
||||
del sub |
||||
context.term() |
||||
|
||||
def test_conflate(self): |
||||
context = messaging.Context() |
||||
|
||||
pub = messaging.PubSocket() |
||||
pub.connect(context, 'controlsState') |
||||
|
||||
sub = messaging.SubSocket() |
||||
sub.connect(context, 'controlsState', conflate=True) |
||||
|
||||
time.sleep(0.1) # Slow joiner |
||||
pub.send('a') |
||||
pub.send('b') |
||||
|
||||
self.assertEqual(b'b', sub.receive()) |
||||
|
||||
del pub |
||||
del sub |
||||
context.term() |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
@ -0,0 +1,163 @@ |
||||
# TODO: these port numbers are hardcoded in c, fix this |
||||
|
||||
# LogRotate: 8001 is a PUSH PULL socket between loggerd and visiond |
||||
|
||||
# all ZMQ pub sub: port, should_log, frequency, (qlog_decimation) |
||||
|
||||
# frame syncing packet |
||||
frame: [8002, true, 20., 1] |
||||
# accel, gyro, and compass |
||||
sensorEvents: [8003, true, 100., 100] |
||||
# GPS data, also global timestamp |
||||
gpsNMEA: [8004, true, 9.] # 9 msgs each sec |
||||
# CPU+MEM+GPU+BAT temps |
||||
thermal: [8005, true, 2., 1] |
||||
# List(CanData), list of can messages |
||||
can: [8006, true, 100.] |
||||
controlsState: [8007, true, 100., 100] |
||||
#liveEvent: [8008, true, 0.] |
||||
model: [8009, true, 20., 5] |
||||
features: [8010, true, 0.] |
||||
health: [8011, true, 2., 1] |
||||
radarState: [8012, true, 20.] |
||||
#liveUI: [8014, true, 0.] |
||||
encodeIdx: [8015, true, 20.] |
||||
liveTracks: [8016, true, 20.] |
||||
sendcan: [8017, true, 100.] |
||||
logMessage: [8018, true, 0.] |
||||
liveCalibration: [8019, true, 5.] |
||||
androidLog: [8020, true, 0.] |
||||
carState: [8021, true, 100., 10] |
||||
# 8022 is reserved for sshd |
||||
carControl: [8023, true, 100., 10] |
||||
plan: [8024, true, 20.] |
||||
liveLocation: [8025, true, 0.] |
||||
gpsLocation: [8026, true, 1., 1] |
||||
ethernetData: [8027, true, 0.] |
||||
navUpdate: [8028, true, 0.] |
||||
qcomGnss: [8029, true, 0.] |
||||
lidarPts: [8030, true, 0.] |
||||
procLog: [8031, true, 0.5] |
||||
gpsLocationExternal: [8032, true, 10., 1] |
||||
ubloxGnss: [8033, true, 10.] |
||||
clocks: [8034, true, 1., 1] |
||||
liveMpc: [8035, false, 20.] |
||||
liveLongitudinalMpc: [8036, false, 20.] |
||||
navStatus: [8038, true, 0.] |
||||
gpsLocationTrimble: [8039, true, 0.] |
||||
trimbleGnss: [8041, true, 0.] |
||||
ubloxRaw: [8042, true, 20.] |
||||
gpsPlannerPoints: [8043, true, 0.] |
||||
gpsPlannerPlan: [8044, true, 0.] |
||||
applanixRaw: [8046, true, 0.] |
||||
orbLocation: [8047, true, 0.] |
||||
trafficEvents: [8048, true, 0.] |
||||
liveLocationTiming: [8049, true, 0.] |
||||
orbslamCorrection: [8050, true, 0.] |
||||
liveLocationCorrected: [8051, true, 0.] |
||||
orbObservation: [8052, true, 0.] |
||||
applanixLocation: [8053, true, 0.] |
||||
liveLocationKalman: [8054, true, 0.] |
||||
uiNavigationEvent: [8055, true, 0.] |
||||
orbOdometry: [8057, true, 0.] |
||||
orbFeatures: [8058, false, 0.] |
||||
orbKeyFrame: [8059, true, 0.] |
||||
uiLayoutState: [8060, true, 0.] |
||||
frontEncodeIdx: [8061, true, 5.] |
||||
orbFeaturesSummary: [8062, true, 0.] |
||||
driverMonitoring: [8063, true, 5., 1] |
||||
liveParameters: [8064, true, 10.] |
||||
liveMapData: [8065, true, 0.] |
||||
cameraOdometry: [8066, true, 5.] |
||||
pathPlan: [8067, true, 20.] |
||||
kalmanOdometry: [8068, true, 0.] |
||||
thumbnail: [8069, true, 0.2, 1] |
||||
carEvents: [8070, true, 1., 1] |
||||
carParams: [8071, true, 0.02, 1] |
||||
frontFrame: [8072, true, 10.] |
||||
|
||||
testModel: [8040, false, 0.] |
||||
testLiveLocation: [8045, false, 0.] |
||||
testJoystick: [8056, false, 0.] |
||||
|
||||
# 8080 is reserved for slave testing daemon |
||||
# 8762 is reserved for logserver |
||||
|
||||
# manager -- base process to manage starting and stopping of all others |
||||
# subscribes: thermal |
||||
|
||||
# **** processes that communicate with the outside world **** |
||||
|
||||
# thermald -- decides when to start and stop onroad |
||||
# subscribes: health, location |
||||
# publishes: thermal |
||||
|
||||
# boardd -- communicates with the car |
||||
# subscribes: sendcan |
||||
# publishes: can, health, ubloxRaw |
||||
|
||||
# sensord -- publishes IMU and Magnetometer |
||||
# publishes: sensorEvents |
||||
|
||||
# gpsd -- publishes EON's gps |
||||
# publishes: gpsNMEA |
||||
|
||||
# visiond -- talks to the cameras, runs the model, saves the videos |
||||
# publishes: frame, model, driverMonitoring, cameraOdometry, thumbnail |
||||
|
||||
# **** stateful data transformers **** |
||||
|
||||
# plannerd -- decides where to drive the car |
||||
# subscribes: carState, model, radarState, controlsState, liveParameters |
||||
# publishes: plan, pathPlan, liveMpc, liveLongitudinalMpc |
||||
|
||||
# controlsd -- drives the car by sending CAN messages to panda |
||||
# subscribes: can, thermal, health, plan, pathPlan, driverMonitoring, liveCalibration |
||||
# publishes: carState, carControl, sendcan, controlsState, carEvents, carParams |
||||
|
||||
# radard -- processes the radar and vision data |
||||
# subscribes: can, controlsState, model, liveParameters |
||||
# publishes: radarState, liveTracks |
||||
|
||||
# params_learner -- learns vehicle params by observing the vehicle dynamics |
||||
# subscribes: controlsState, sensorEvents, cameraOdometry |
||||
# publishes: liveParameters |
||||
|
||||
# calibrationd -- reads posenet and applies a temporal filter on the frame region to look at |
||||
# subscribes: cameraOdometry |
||||
# publishes: liveCalibration |
||||
|
||||
# ubloxd -- read raw ublox data and converts them in readable format |
||||
# subscribes: ubloxRaw |
||||
# publishes: ubloxGnss |
||||
|
||||
# **** LOGGING SERVICE **** |
||||
|
||||
# loggerd |
||||
# subscribes: EVERYTHING |
||||
|
||||
# **** NON VITAL SERVICES **** |
||||
|
||||
# ui |
||||
# subscribes: thermal, model, controlsState, uiLayout, liveCalibration, radarState, liveMpc, plusFrame, liveMapData |
||||
|
||||
# uploader |
||||
# communicates through file system with loggerd |
||||
|
||||
# deleter |
||||
# communicates through file system with loggerd and uploader |
||||
|
||||
# logmessaged -- central logging service, can log to cloud |
||||
# publishes: logMessage |
||||
|
||||
# logcatd -- fetches logcat info from android |
||||
# publishes: androidLog |
||||
|
||||
# proclogd -- fetches process information |
||||
# publishes: procLog |
||||
|
||||
# tombstoned -- reports native crashes |
||||
|
||||
# athenad -- on request, open a sub socket and return the value |
||||
|
||||
# updated -- waits for network access and tries to update every hour |
||||
@ -0,0 +1,33 @@ |
||||
#!/usr/bin/env python3 |
||||
import os |
||||
import yaml |
||||
|
||||
class Service(): |
||||
def __init__(self, port, should_log, frequency, decimation=None): |
||||
self.port = port |
||||
self.should_log = should_log |
||||
self.frequency = frequency |
||||
self.decimation = decimation |
||||
|
||||
service_list_path = os.path.join(os.path.dirname(__file__), "service_list.yaml") |
||||
|
||||
service_list = {} |
||||
with open(service_list_path, "r") as f: |
||||
for k, v in yaml.safe_load(f).items(): |
||||
decimation = None |
||||
if len(v) == 4: |
||||
decimation = v[3] |
||||
|
||||
service_list[k] = Service(v[0], v[1], v[2], decimation) |
||||
|
||||
if __name__ == "__main__": |
||||
print("/* THIS IS AN AUTOGENERATED FILE, PLEASE EDIT service_list.yaml */") |
||||
print("#ifndef __SERVICES_H") |
||||
print("#define __SERVICES_H") |
||||
print("struct service { int port; bool should_log; int frequency; int decimation; char name[0x100]; };") |
||||
print("static struct service services[] = {") |
||||
for k, v in service_list.items(): |
||||
print(' { .name = "%s", .port = %d, .should_log = %s, .frequency = %d, .decimation = %d },' % (k, v.port, "true" if v.should_log else "false", v.frequency, -1 if v.decimation is None else v.decimation)) |
||||
print("};") |
||||
print("#endif") |
||||
|
||||
Loading…
Reference in new issue