Compile FrogPilot
This commit is contained in:
FrogAi
BIN
cereal/messaging/bridge
Executable file
BIN
cereal/messaging/bridge
Executable file
Binary file not shown.
@@ -1,92 +0,0 @@
|
||||
#include <algorithm>
|
||||
#include <cassert>
|
||||
#include <csignal>
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
typedef void (*sighandler_t)(int sig);
|
||||
|
||||
#include "cereal/services.h"
|
||||
#include "cereal/messaging/impl_msgq.h"
|
||||
#include "cereal/messaging/impl_zmq.h"
|
||||
|
||||
std::atomic<bool> do_exit = false;
|
||||
static void set_do_exit(int sig) {
|
||||
do_exit = true;
|
||||
}
|
||||
|
||||
void sigpipe_handler(int sig) {
|
||||
assert(sig == SIGPIPE);
|
||||
std::cout << "SIGPIPE received" << std::endl;
|
||||
}
|
||||
|
||||
static std::vector<std::string> get_services(std::string whitelist_str, bool zmq_to_msgq) {
|
||||
std::vector<std::string> service_list;
|
||||
for (const auto& it : services) {
|
||||
std::string name = it.second.name;
|
||||
bool in_whitelist = whitelist_str.find(name) != std::string::npos;
|
||||
if (name == "plusFrame" || name == "uiLayoutState" || (zmq_to_msgq && !in_whitelist)) {
|
||||
continue;
|
||||
}
|
||||
service_list.push_back(name);
|
||||
}
|
||||
return service_list;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
signal(SIGPIPE, (sighandler_t)sigpipe_handler);
|
||||
signal(SIGINT, (sighandler_t)set_do_exit);
|
||||
signal(SIGTERM, (sighandler_t)set_do_exit);
|
||||
|
||||
bool zmq_to_msgq = argc > 2;
|
||||
std::string ip = zmq_to_msgq ? argv[1] : "127.0.0.1";
|
||||
std::string whitelist_str = zmq_to_msgq ? std::string(argv[2]) : "";
|
||||
|
||||
Poller *poller;
|
||||
Context *pub_context;
|
||||
Context *sub_context;
|
||||
if (zmq_to_msgq) { // republishes zmq debugging messages as msgq
|
||||
poller = new ZMQPoller();
|
||||
pub_context = new MSGQContext();
|
||||
sub_context = new ZMQContext();
|
||||
} else {
|
||||
poller = new MSGQPoller();
|
||||
pub_context = new ZMQContext();
|
||||
sub_context = new MSGQContext();
|
||||
}
|
||||
|
||||
std::map<SubSocket*, PubSocket*> sub2pub;
|
||||
for (auto endpoint : get_services(whitelist_str, zmq_to_msgq)) {
|
||||
PubSocket * pub_sock;
|
||||
SubSocket * sub_sock;
|
||||
if (zmq_to_msgq) {
|
||||
pub_sock = new MSGQPubSocket();
|
||||
sub_sock = new ZMQSubSocket();
|
||||
} else {
|
||||
pub_sock = new ZMQPubSocket();
|
||||
sub_sock = new MSGQSubSocket();
|
||||
}
|
||||
pub_sock->connect(pub_context, endpoint);
|
||||
sub_sock->connect(sub_context, endpoint, ip, false);
|
||||
|
||||
poller->registerSocket(sub_sock);
|
||||
sub2pub[sub_sock] = pub_sock;
|
||||
}
|
||||
|
||||
while (!do_exit) {
|
||||
for (auto sub_sock : poller->poll(100)) {
|
||||
Message * msg = sub_sock->receive();
|
||||
if (msg == NULL) continue;
|
||||
int ret;
|
||||
do {
|
||||
ret = sub2pub[sub_sock]->sendMessage(msg);
|
||||
} while (ret == -1 && errno == EINTR && !do_exit);
|
||||
assert(ret >= 0 || do_exit);
|
||||
delete msg;
|
||||
|
||||
if (do_exit) break;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -1,236 +0,0 @@
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
#include <cstdlib>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <exception>
|
||||
#include <filesystem>
|
||||
|
||||
#include <unistd.h>
|
||||
#include <poll.h>
|
||||
#include <signal.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/mman.h>
|
||||
#include <sys/stat.h>
|
||||
|
||||
#include "cereal/messaging/event.h"
|
||||
|
||||
#ifndef __APPLE__
|
||||
#include <sys/eventfd.h>
|
||||
|
||||
void event_state_shm_mmap(std::string endpoint, std::string identifier, char **shm_mem, std::string *shm_path) {
|
||||
const char* op_prefix = std::getenv("OPENPILOT_PREFIX");
|
||||
|
||||
std::string full_path = "/dev/shm/";
|
||||
if (op_prefix) {
|
||||
full_path += std::string(op_prefix) + "/";
|
||||
}
|
||||
full_path += CEREAL_EVENTS_PREFIX + "/";
|
||||
if (identifier.size() > 0) {
|
||||
full_path += identifier + "/";
|
||||
}
|
||||
std::filesystem::create_directories(full_path);
|
||||
full_path += endpoint;
|
||||
|
||||
int shm_fd = open(full_path.c_str(), O_RDWR | O_CREAT, 0664);
|
||||
if (shm_fd < 0) {
|
||||
throw std::runtime_error("Could not open shared memory file.");
|
||||
}
|
||||
|
||||
int rc = ftruncate(shm_fd, sizeof(EventState));
|
||||
if (rc < 0){
|
||||
close(shm_fd);
|
||||
throw std::runtime_error("Could not truncate shared memory file.");
|
||||
}
|
||||
|
||||
char * mem = (char*)mmap(NULL, sizeof(EventState), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0);
|
||||
close(shm_fd);
|
||||
if (mem == nullptr) {
|
||||
throw std::runtime_error("Could not map shared memory file.");
|
||||
}
|
||||
|
||||
if (shm_mem != nullptr)
|
||||
*shm_mem = mem;
|
||||
if (shm_path != nullptr)
|
||||
*shm_path = full_path;
|
||||
}
|
||||
|
||||
SocketEventHandle::SocketEventHandle(std::string endpoint, std::string identifier, bool override) {
|
||||
char *mem;
|
||||
event_state_shm_mmap(endpoint, identifier, &mem, &this->shm_path);
|
||||
|
||||
this->state = (EventState*)mem;
|
||||
if (override) {
|
||||
this->state->fds[0] = eventfd(0, EFD_NONBLOCK);
|
||||
this->state->fds[1] = eventfd(0, EFD_NONBLOCK);
|
||||
}
|
||||
}
|
||||
|
||||
SocketEventHandle::~SocketEventHandle() {
|
||||
close(this->state->fds[0]);
|
||||
close(this->state->fds[1]);
|
||||
munmap(this->state, sizeof(EventState));
|
||||
unlink(this->shm_path.c_str());
|
||||
}
|
||||
|
||||
bool SocketEventHandle::is_enabled() {
|
||||
return this->state->enabled;
|
||||
}
|
||||
|
||||
void SocketEventHandle::set_enabled(bool enabled) {
|
||||
this->state->enabled = enabled;
|
||||
}
|
||||
|
||||
Event SocketEventHandle::recv_called() {
|
||||
return Event(this->state->fds[0]);
|
||||
}
|
||||
|
||||
Event SocketEventHandle::recv_ready() {
|
||||
return Event(this->state->fds[1]);
|
||||
}
|
||||
|
||||
void SocketEventHandle::toggle_fake_events(bool enabled) {
|
||||
if (enabled)
|
||||
setenv("CEREAL_FAKE", "1", true);
|
||||
else
|
||||
unsetenv("CEREAL_FAKE");
|
||||
}
|
||||
|
||||
void SocketEventHandle::set_fake_prefix(std::string prefix) {
|
||||
if (prefix.size() == 0) {
|
||||
unsetenv("CEREAL_FAKE_PREFIX");
|
||||
} else {
|
||||
setenv("CEREAL_FAKE_PREFIX", prefix.c_str(), true);
|
||||
}
|
||||
}
|
||||
|
||||
std::string SocketEventHandle::fake_prefix() {
|
||||
const char* prefix = std::getenv("CEREAL_FAKE_PREFIX");
|
||||
if (prefix == nullptr) {
|
||||
return "";
|
||||
} else {
|
||||
return std::string(prefix);
|
||||
}
|
||||
}
|
||||
|
||||
Event::Event(int fd): event_fd(fd) {}
|
||||
|
||||
void Event::set() const {
|
||||
throw_if_invalid();
|
||||
|
||||
uint64_t val = 1;
|
||||
size_t count = write(this->event_fd, &val, sizeof(uint64_t));
|
||||
assert(count == sizeof(uint64_t));
|
||||
}
|
||||
|
||||
int Event::clear() const {
|
||||
throw_if_invalid();
|
||||
|
||||
uint64_t val = 0;
|
||||
// read the eventfd to clear it
|
||||
read(this->event_fd, &val, sizeof(uint64_t));
|
||||
|
||||
return val;
|
||||
}
|
||||
|
||||
void Event::wait(int timeout_sec) const {
|
||||
throw_if_invalid();
|
||||
|
||||
int event_count;
|
||||
struct pollfd fds = { this->event_fd, POLLIN, 0 };
|
||||
struct timespec timeout = { timeout_sec, 0 };;
|
||||
|
||||
sigset_t signals;
|
||||
sigfillset(&signals);
|
||||
sigdelset(&signals, SIGALRM);
|
||||
sigdelset(&signals, SIGINT);
|
||||
sigdelset(&signals, SIGTERM);
|
||||
sigdelset(&signals, SIGQUIT);
|
||||
|
||||
event_count = ppoll(&fds, 1, timeout_sec < 0 ? nullptr : &timeout, &signals);
|
||||
|
||||
if (event_count == 0) {
|
||||
throw std::runtime_error("Event timed out pid: " + std::to_string(getpid()));
|
||||
} else if (event_count < 0) {
|
||||
throw std::runtime_error("Event poll failed, errno: " + std::to_string(errno) + " pid: " + std::to_string(getpid()));
|
||||
}
|
||||
}
|
||||
|
||||
bool Event::peek() const {
|
||||
throw_if_invalid();
|
||||
|
||||
int event_count;
|
||||
|
||||
struct pollfd fds = { this->event_fd, POLLIN, 0 };
|
||||
|
||||
// poll with timeout zero to return status immediately
|
||||
event_count = poll(&fds, 1, 0);
|
||||
|
||||
return event_count != 0;
|
||||
}
|
||||
|
||||
bool Event::is_valid() const {
|
||||
return event_fd != -1;
|
||||
}
|
||||
|
||||
int Event::fd() const {
|
||||
return event_fd;
|
||||
}
|
||||
|
||||
int Event::wait_for_one(const std::vector<Event>& events, int timeout_sec) {
|
||||
struct pollfd fds[events.size()];
|
||||
for (size_t i = 0; i < events.size(); i++) {
|
||||
fds[i] = { events[i].fd(), POLLIN, 0 };
|
||||
}
|
||||
|
||||
struct timespec timeout = { timeout_sec, 0 };
|
||||
|
||||
sigset_t signals;
|
||||
sigfillset(&signals);
|
||||
sigdelset(&signals, SIGALRM);
|
||||
sigdelset(&signals, SIGINT);
|
||||
sigdelset(&signals, SIGTERM);
|
||||
sigdelset(&signals, SIGQUIT);
|
||||
|
||||
int event_count = ppoll(fds, events.size(), timeout_sec < 0 ? nullptr : &timeout, &signals);
|
||||
|
||||
if (event_count == 0) {
|
||||
throw std::runtime_error("Event timed out pid: " + std::to_string(getpid()));
|
||||
} else if (event_count < 0) {
|
||||
throw std::runtime_error("Event poll failed, errno: " + std::to_string(errno) + " pid: " + std::to_string(getpid()));
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < events.size(); i++) {
|
||||
if (fds[i].revents & POLLIN) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
|
||||
throw std::runtime_error("Event poll failed, no events ready");
|
||||
}
|
||||
#else
|
||||
// Stub implementation for Darwin, which does not support eventfd
|
||||
void event_state_shm_mmap(std::string endpoint, std::string identifier, char **shm_mem, std::string *shm_path) {}
|
||||
|
||||
SocketEventHandle::SocketEventHandle(std::string endpoint, std::string identifier, bool override) {
|
||||
std::cerr << "SocketEventHandle not supported on macOS" << std::endl;
|
||||
assert(false);
|
||||
}
|
||||
SocketEventHandle::~SocketEventHandle() {}
|
||||
bool SocketEventHandle::is_enabled() { return this->state->enabled; }
|
||||
void SocketEventHandle::set_enabled(bool enabled) {}
|
||||
Event SocketEventHandle::recv_called() { return Event(); }
|
||||
Event SocketEventHandle::recv_ready() { return Event(); }
|
||||
void SocketEventHandle::toggle_fake_events(bool enabled) {}
|
||||
void SocketEventHandle::set_fake_prefix(std::string prefix) {}
|
||||
std::string SocketEventHandle::fake_prefix() { return ""; }
|
||||
|
||||
Event::Event(int fd): event_fd(fd) {}
|
||||
void Event::set() const {}
|
||||
int Event::clear() const { return 0; }
|
||||
void Event::wait(int timeout_sec) const {}
|
||||
bool Event::peek() const { return false; }
|
||||
bool Event::is_valid() const { return false; }
|
||||
int Event::fd() const { return this->event_fd; }
|
||||
int Event::wait_for_one(const std::vector<Event>& events, int timeout_sec) { return -1; }
|
||||
#endif
|
||||
@@ -1,58 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#define CEREAL_EVENTS_PREFIX std::string("cereal_events")
|
||||
|
||||
void event_state_shm_mmap(std::string endpoint, std::string identifier, char **shm_mem, std::string *shm_path);
|
||||
|
||||
enum EventPurpose {
|
||||
RECV_CALLED,
|
||||
RECV_READY
|
||||
};
|
||||
|
||||
struct EventState {
|
||||
int fds[2];
|
||||
bool enabled;
|
||||
};
|
||||
|
||||
class Event {
|
||||
private:
|
||||
int event_fd = -1;
|
||||
|
||||
inline void throw_if_invalid() const {
|
||||
if (!this->is_valid()) {
|
||||
throw std::runtime_error("Event does not have valid file descriptor.");
|
||||
}
|
||||
}
|
||||
public:
|
||||
Event(int fd = -1);
|
||||
|
||||
void set() const;
|
||||
int clear() const;
|
||||
void wait(int timeout_sec = -1) const;
|
||||
bool peek() const;
|
||||
bool is_valid() const;
|
||||
int fd() const;
|
||||
|
||||
static int wait_for_one(const std::vector<Event>& events, int timeout_sec = -1);
|
||||
};
|
||||
|
||||
class SocketEventHandle {
|
||||
private:
|
||||
std::string shm_path;
|
||||
EventState* state;
|
||||
public:
|
||||
SocketEventHandle(std::string endpoint, std::string identifier = "", bool override = true);
|
||||
~SocketEventHandle();
|
||||
|
||||
bool is_enabled();
|
||||
void set_enabled(bool enabled);
|
||||
Event recv_called();
|
||||
Event recv_ready();
|
||||
|
||||
static void toggle_fake_events(bool enabled);
|
||||
static void set_fake_prefix(std::string prefix);
|
||||
static std::string fake_prefix();
|
||||
};
|
||||
@@ -1,9 +0,0 @@
|
||||
#include "cereal/messaging/impl_fake.h"
|
||||
|
||||
void FakePoller::registerSocket(SubSocket *socket) {
|
||||
this->sockets.push_back(socket);
|
||||
}
|
||||
|
||||
std::vector<SubSocket*> FakePoller::poll(int timeout) {
|
||||
return this->sockets;
|
||||
}
|
||||
@@ -1,67 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <filesystem>
|
||||
|
||||
#include <sys/mman.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "cereal/messaging/event.h"
|
||||
|
||||
template<typename TSubSocket>
|
||||
class FakeSubSocket: public TSubSocket {
|
||||
private:
|
||||
Event *recv_called = nullptr;
|
||||
Event *recv_ready = nullptr;
|
||||
EventState *state = nullptr;
|
||||
|
||||
public:
|
||||
FakeSubSocket(): TSubSocket() {}
|
||||
~FakeSubSocket() {
|
||||
delete recv_called;
|
||||
delete recv_ready;
|
||||
if (state != nullptr) {
|
||||
munmap(state, sizeof(EventState));
|
||||
}
|
||||
}
|
||||
|
||||
int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true) override {
|
||||
const char* cereal_prefix = std::getenv("CEREAL_FAKE_PREFIX");
|
||||
|
||||
char* mem;
|
||||
std::string identifier = cereal_prefix != nullptr ? std::string(cereal_prefix) : "";
|
||||
event_state_shm_mmap(endpoint, identifier, &mem, nullptr);
|
||||
|
||||
this->state = (EventState*)mem;
|
||||
this->recv_called = new Event(state->fds[EventPurpose::RECV_CALLED]);
|
||||
this->recv_ready = new Event(state->fds[EventPurpose::RECV_READY]);
|
||||
|
||||
return TSubSocket::connect(context, endpoint, address, conflate, check_endpoint);
|
||||
}
|
||||
|
||||
Message *receive(bool non_blocking=false) override {
|
||||
if (this->state->enabled) {
|
||||
this->recv_called->set();
|
||||
this->recv_ready->wait();
|
||||
this->recv_ready->clear();
|
||||
}
|
||||
|
||||
return TSubSocket::receive(non_blocking);
|
||||
}
|
||||
};
|
||||
|
||||
class FakePoller: public Poller {
|
||||
private:
|
||||
std::vector<SubSocket*> sockets;
|
||||
|
||||
public:
|
||||
void registerSocket(SubSocket *socket) override;
|
||||
std::vector<SubSocket*> poll(int timeout) override;
|
||||
~FakePoller() {}
|
||||
};
|
||||
@@ -1,215 +0,0 @@
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
#include <iostream>
|
||||
#include <cstdlib>
|
||||
#include <csignal>
|
||||
#include <cerrno>
|
||||
|
||||
#include "cereal/services.h"
|
||||
#include "cereal/messaging/impl_msgq.h"
|
||||
|
||||
|
||||
volatile sig_atomic_t msgq_do_exit = 0;
|
||||
|
||||
void sig_handler(int signal) {
|
||||
assert(signal == SIGINT || signal == SIGTERM);
|
||||
msgq_do_exit = 1;
|
||||
}
|
||||
|
||||
static bool service_exists(std::string path){
|
||||
return services.count(path) > 0;
|
||||
}
|
||||
|
||||
|
||||
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, bool check_endpoint){
|
||||
assert(context);
|
||||
assert(address == "127.0.0.1");
|
||||
|
||||
if (check_endpoint && !service_exists(std::string(endpoint))){
|
||||
std::cout << "Warning, " << std::string(endpoint) << " is not in service list." << std::endl;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
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 (!non_blocking){
|
||||
std::signal(SIGINT, prev_handler_sigint);
|
||||
std::signal(SIGTERM, prev_handler_sigterm);
|
||||
}
|
||||
|
||||
errno = msgq_do_exit ? EINTR : 0;
|
||||
|
||||
if (rc > 0){
|
||||
if (msgq_do_exit){
|
||||
msgq_msg_close(&msg); // Free unused message on exit
|
||||
} else {
|
||||
r = new MSGQMessage;
|
||||
r->takeOwnership(msg.data, msg.size);
|
||||
}
|
||||
}
|
||||
|
||||
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, bool check_endpoint){
|
||||
assert(context);
|
||||
|
||||
if (check_endpoint && !service_exists(std::string(endpoint))){
|
||||
std::cout << "Warning, " << std::string(endpoint) << " is not in service list." << std::endl;
|
||||
}
|
||||
|
||||
q = new msgq_queue_t;
|
||||
int r = msgq_new_queue(q, endpoint.c_str(), DEFAULT_SEGMENT_SIZE);
|
||||
if (r != 0){
|
||||
return r;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
bool MSGQPubSocket::all_readers_updated() {
|
||||
return msgq_all_readers_updated(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;
|
||||
}
|
||||
@@ -1,67 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "cereal/messaging/msgq.h"
|
||||
|
||||
#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, bool check_endpoint=true);
|
||||
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, bool check_endpoint=true);
|
||||
int sendMessage(Message *message);
|
||||
int send(char *data, size_t size);
|
||||
bool all_readers_updated();
|
||||
~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(){}
|
||||
};
|
||||
@@ -1,162 +0,0 @@
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
#include <iostream>
|
||||
#include <cstdlib>
|
||||
#include <cerrno>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "cereal/services.h"
|
||||
#include "cereal/messaging/impl_zmq.h"
|
||||
|
||||
static int get_port(std::string endpoint) {
|
||||
return services.at(endpoint).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, bool check_endpoint){
|
||||
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 + ":";
|
||||
if (check_endpoint){
|
||||
full_endpoint += std::to_string(get_port(endpoint));
|
||||
} else {
|
||||
full_endpoint += 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, bool check_endpoint){
|
||||
sock = zmq_socket(context->getRawContext(), ZMQ_PUB);
|
||||
if (sock == NULL){
|
||||
return -1;
|
||||
}
|
||||
|
||||
full_endpoint = "tcp://*:";
|
||||
if (check_endpoint){
|
||||
full_endpoint += std::to_string(get_port(endpoint));
|
||||
} else {
|
||||
full_endpoint += endpoint;
|
||||
}
|
||||
|
||||
// ZMQ pub sockets cannot be shared between processes, so we need to ensure pid stays the same
|
||||
pid = getpid();
|
||||
|
||||
return zmq_bind(sock, full_endpoint.c_str());
|
||||
}
|
||||
|
||||
int ZMQPubSocket::sendMessage(Message *message) {
|
||||
assert(pid == getpid());
|
||||
return zmq_send(sock, message->getData(), message->getSize(), ZMQ_DONTWAIT);
|
||||
}
|
||||
|
||||
int ZMQPubSocket::send(char *data, size_t size) {
|
||||
assert(pid == getpid());
|
||||
return zmq_send(sock, data, size, ZMQ_DONTWAIT);
|
||||
}
|
||||
|
||||
bool ZMQPubSocket::all_readers_updated() {
|
||||
assert(false); // TODO not implemented
|
||||
return false;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -1,68 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <zmq.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
|
||||
#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, bool check_endpoint=true);
|
||||
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;
|
||||
int pid = -1;
|
||||
public:
|
||||
int connect(Context *context, std::string endpoint, bool check_endpoint=true);
|
||||
int sendMessage(Message *message);
|
||||
int send(char *data, size_t size);
|
||||
bool all_readers_updated();
|
||||
~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(){}
|
||||
};
|
||||
@@ -1,120 +0,0 @@
|
||||
#include <cassert>
|
||||
#include <iostream>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "cereal/messaging/impl_zmq.h"
|
||||
#include "cereal/messaging/impl_msgq.h"
|
||||
#include "cereal/messaging/impl_fake.h"
|
||||
|
||||
#ifdef __APPLE__
|
||||
const bool MUST_USE_ZMQ = true;
|
||||
#else
|
||||
const bool MUST_USE_ZMQ = false;
|
||||
#endif
|
||||
|
||||
bool messaging_use_zmq(){
|
||||
if (std::getenv("ZMQ") || MUST_USE_ZMQ) {
|
||||
if (std::getenv("OPENPILOT_PREFIX")) {
|
||||
std::cerr << "OPENPILOT_PREFIX not supported with ZMQ backend\n";
|
||||
assert(false);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool messaging_use_fake(){
|
||||
char* fake_enabled = std::getenv("CEREAL_FAKE");
|
||||
return fake_enabled != NULL;
|
||||
}
|
||||
|
||||
Context * Context::create(){
|
||||
Context * c;
|
||||
if (messaging_use_zmq()){
|
||||
c = new ZMQContext();
|
||||
} else {
|
||||
c = new MSGQContext();
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
SubSocket * SubSocket::create(){
|
||||
SubSocket * s;
|
||||
if (messaging_use_fake()) {
|
||||
if (messaging_use_zmq()) {
|
||||
s = new FakeSubSocket<ZMQSubSocket>();
|
||||
} else {
|
||||
s = new FakeSubSocket<MSGQSubSocket>();
|
||||
}
|
||||
} else {
|
||||
if (messaging_use_zmq()){
|
||||
s = new ZMQSubSocket();
|
||||
} else {
|
||||
s = new MSGQSubSocket();
|
||||
}
|
||||
}
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
SubSocket * SubSocket::create(Context * context, std::string endpoint, std::string address, bool conflate, bool check_endpoint){
|
||||
SubSocket *s = SubSocket::create();
|
||||
int r = s->connect(context, endpoint, address, conflate, check_endpoint);
|
||||
|
||||
if (r == 0) {
|
||||
return s;
|
||||
} else {
|
||||
std::cerr << "Error, failed to connect SubSocket to " << endpoint << ": " << strerror(errno) << std::endl;
|
||||
|
||||
delete s;
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
PubSocket * PubSocket::create(){
|
||||
PubSocket * s;
|
||||
if (messaging_use_zmq()){
|
||||
s = new ZMQPubSocket();
|
||||
} else {
|
||||
s = new MSGQPubSocket();
|
||||
}
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
PubSocket * PubSocket::create(Context * context, std::string endpoint, bool check_endpoint){
|
||||
PubSocket *s = PubSocket::create();
|
||||
int r = s->connect(context, endpoint, check_endpoint);
|
||||
|
||||
if (r == 0) {
|
||||
return s;
|
||||
} else {
|
||||
std::cerr << "Error, failed to bind PubSocket to " << endpoint << ": " << strerror(errno) << std::endl;
|
||||
|
||||
delete s;
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
Poller * Poller::create(){
|
||||
Poller * p;
|
||||
if (messaging_use_fake()) {
|
||||
p = new FakePoller();
|
||||
} else {
|
||||
if (messaging_use_zmq()){
|
||||
p = new ZMQPoller();
|
||||
} else {
|
||||
p = new MSGQPoller();
|
||||
}
|
||||
}
|
||||
return p;
|
||||
}
|
||||
|
||||
Poller * Poller::create(std::vector<SubSocket*> sockets){
|
||||
Poller * p = Poller::create();
|
||||
|
||||
for (auto s : sockets){
|
||||
p->registerSocket(s);
|
||||
}
|
||||
return p;
|
||||
}
|
||||
@@ -1,162 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstddef>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
#include <time.h>
|
||||
|
||||
#include <capnp/serialize.h>
|
||||
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#ifdef __APPLE__
|
||||
#define CLOCK_BOOTTIME CLOCK_MONOTONIC
|
||||
#endif
|
||||
|
||||
#define MSG_MULTIPLE_PUBLISHERS 100
|
||||
|
||||
bool messaging_use_zmq();
|
||||
|
||||
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, bool check_endpoint=true) = 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, std::string address="127.0.0.1", bool conflate=false, bool check_endpoint=true);
|
||||
virtual ~SubSocket(){}
|
||||
};
|
||||
|
||||
class PubSocket {
|
||||
public:
|
||||
virtual int connect(Context *context, std::string endpoint, bool check_endpoint=true) = 0;
|
||||
virtual int sendMessage(Message *message) = 0;
|
||||
virtual int send(char *data, size_t size) = 0;
|
||||
virtual bool all_readers_updated() = 0;
|
||||
static PubSocket * create();
|
||||
static PubSocket * create(Context * context, std::string endpoint, bool check_endpoint=true);
|
||||
static PubSocket * create(Context * context, std::string endpoint, int port, bool check_endpoint=true);
|
||||
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(){}
|
||||
};
|
||||
|
||||
class SubMaster {
|
||||
public:
|
||||
SubMaster(const std::vector<const char *> &service_list, const std::vector<const char *> &poll = {},
|
||||
const char *address = nullptr, const std::vector<const char *> &ignore_alive = {});
|
||||
void update(int timeout = 1000);
|
||||
void update_msgs(uint64_t current_time, const std::vector<std::pair<std::string, cereal::Event::Reader>> &messages);
|
||||
inline bool allAlive(const std::vector<const char *> &service_list = {}) { return all_(service_list, false, true); }
|
||||
inline bool allValid(const std::vector<const char *> &service_list = {}) { return all_(service_list, true, false); }
|
||||
inline bool allAliveAndValid(const std::vector<const char *> &service_list = {}) { return all_(service_list, true, true); }
|
||||
void drain();
|
||||
~SubMaster();
|
||||
|
||||
uint64_t frame = 0;
|
||||
bool updated(const char *name) const;
|
||||
bool alive(const char *name) const;
|
||||
bool valid(const char *name) const;
|
||||
uint64_t rcv_frame(const char *name) const;
|
||||
uint64_t rcv_time(const char *name) const;
|
||||
cereal::Event::Reader &operator[](const char *name) const;
|
||||
|
||||
private:
|
||||
bool all_(const std::vector<const char *> &service_list, bool valid, bool alive);
|
||||
Poller *poller_ = nullptr;
|
||||
struct SubMessage;
|
||||
std::map<SubSocket *, SubMessage *> messages_;
|
||||
std::map<std::string, SubMessage *> services_;
|
||||
};
|
||||
|
||||
class MessageBuilder : public capnp::MallocMessageBuilder {
|
||||
public:
|
||||
MessageBuilder() = default;
|
||||
|
||||
cereal::Event::Builder initEvent(bool valid = true) {
|
||||
cereal::Event::Builder event = initRoot<cereal::Event>();
|
||||
struct timespec t;
|
||||
clock_gettime(CLOCK_BOOTTIME, &t);
|
||||
uint64_t current_time = t.tv_sec * 1000000000ULL + t.tv_nsec;
|
||||
event.setLogMonoTime(current_time);
|
||||
event.setValid(valid);
|
||||
return event;
|
||||
}
|
||||
|
||||
kj::ArrayPtr<capnp::byte> toBytes() {
|
||||
heapArray_ = capnp::messageToFlatArray(*this);
|
||||
return heapArray_.asBytes();
|
||||
}
|
||||
|
||||
size_t getSerializedSize() {
|
||||
return capnp::computeSerializedSizeInWords(*this) * sizeof(capnp::word);
|
||||
}
|
||||
|
||||
int serializeToBuffer(unsigned char *buffer, size_t buffer_size) {
|
||||
size_t serialized_size = getSerializedSize();
|
||||
if (serialized_size > buffer_size) { return -1; }
|
||||
kj::ArrayOutputStream out(kj::ArrayPtr<capnp::byte>(buffer, buffer_size));
|
||||
capnp::writeMessage(out, *this);
|
||||
return serialized_size;
|
||||
}
|
||||
|
||||
private:
|
||||
kj::Array<capnp::word> heapArray_;
|
||||
};
|
||||
|
||||
class PubMaster {
|
||||
public:
|
||||
PubMaster(const std::vector<const char *> &service_list);
|
||||
inline int send(const char *name, capnp::byte *data, size_t size) { return sockets_.at(name)->send((char *)data, size); }
|
||||
int send(const char *name, MessageBuilder &msg);
|
||||
~PubMaster();
|
||||
|
||||
private:
|
||||
std::map<std::string, PubSocket *> sockets_;
|
||||
};
|
||||
|
||||
class AlignedBuffer {
|
||||
public:
|
||||
kj::ArrayPtr<const capnp::word> align(const char *data, const size_t size) {
|
||||
words_size = size / sizeof(capnp::word) + 1;
|
||||
if (aligned_buf.size() < words_size) {
|
||||
aligned_buf = kj::heapArray<capnp::word>(words_size < 512 ? 512 : words_size);
|
||||
}
|
||||
memcpy(aligned_buf.begin(), data, size);
|
||||
return aligned_buf.slice(0, words_size);
|
||||
}
|
||||
inline kj::ArrayPtr<const capnp::word> align(Message *m) {
|
||||
return align(m->getData(), m->getSize());
|
||||
}
|
||||
private:
|
||||
kj::Array<capnp::word> aligned_buf;
|
||||
size_t words_size;
|
||||
};
|
||||
17670
cereal/messaging/messaging_pyx.cpp
Normal file
17670
cereal/messaging/messaging_pyx.cpp
Normal file
File diff suppressed because it is too large
Load Diff
BIN
cereal/messaging/messaging_pyx.so
Executable file
BIN
cereal/messaging/messaging_pyx.so
Executable file
Binary file not shown.
@@ -1,468 +0,0 @@
|
||||
#include <iostream>
|
||||
#include <cassert>
|
||||
#include <cerrno>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <cstdint>
|
||||
#include <chrono>
|
||||
#include <algorithm>
|
||||
#include <cstdlib>
|
||||
#include <csignal>
|
||||
#include <random>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
|
||||
#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 "cereal/messaging/msgq.h"
|
||||
|
||||
void sigusr2_handler(int signal) {
|
||||
assert(signal == SIGUSR2);
|
||||
}
|
||||
|
||||
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());
|
||||
|
||||
#ifdef __APPLE__
|
||||
// TODO: this doesn't work
|
||||
uint64_t uid = distribution(rd) << 32 | getpid();
|
||||
#else
|
||||
uint64_t uid = distribution(rd) << 32 | syscall(SYS_gettid);
|
||||
#endif
|
||||
|
||||
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){
|
||||
// wait for subscriber
|
||||
}
|
||||
|
||||
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(SIGUSR2, sigusr2_handler);
|
||||
|
||||
std::string full_path = "/dev/shm/";
|
||||
const char* prefix = std::getenv("OPENPILOT_PREFIX");
|
||||
if (prefix) {
|
||||
full_path += std::string(prefix) + "/";
|
||||
}
|
||||
full_path += path;
|
||||
|
||||
auto fd = open(full_path.c_str(), O_RDWR | O_CREAT, 0664);
|
||||
if (fd < 0) {
|
||||
std::cout << "Warning, could not open: " << full_path << std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
int rc = ftruncate(fd, size + sizeof(msgq_header_t));
|
||||
if (rc < 0){
|
||||
close(fd);
|
||||
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;
|
||||
}
|
||||
|
||||
static void thread_signal(uint32_t tid) {
|
||||
#ifndef SYS_tkill
|
||||
// TODO: this won't work for multithreaded programs
|
||||
kill(tid, SIGUSR2);
|
||||
#else
|
||||
syscall(SYS_tkill, tid, SIGUSR2);
|
||||
#endif
|
||||
}
|
||||
|
||||
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
|
||||
thread_signal(old_uid & 0xFFFFFFFF);
|
||||
}
|
||||
|
||||
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];
|
||||
thread_signal(reader_uid & 0xFFFFFFFF);
|
||||
}
|
||||
|
||||
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]);
|
||||
UNUSED(read_cycles);
|
||||
|
||||
uint32_t write_cycles, write_pointer;
|
||||
UNPACK64(write_cycles, write_pointer, *q->write_pointer);
|
||||
UNUSED(write_cycles);
|
||||
|
||||
// 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);
|
||||
UNUSED(write_cycles);
|
||||
|
||||
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){
|
||||
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;
|
||||
}
|
||||
|
||||
bool msgq_all_readers_updated(msgq_queue_t *q) {
|
||||
uint64_t num_readers = *q->num_readers;
|
||||
for (uint64_t i = 0; i < num_readers; i++) {
|
||||
if (*q->read_valids[i] && *q->write_pointer != *q->read_pointers[i]) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return num_readers > 0;
|
||||
}
|
||||
@@ -1,70 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <atomic>
|
||||
|
||||
#define DEFAULT_SEGMENT_SIZE (10 * 1024 * 1024)
|
||||
#define NUM_READERS 12
|
||||
#define ALIGN(n) ((n + (8 - 1)) & -8)
|
||||
|
||||
#define UNUSED(x) (void)x
|
||||
#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);
|
||||
|
||||
bool msgq_all_readers_updated(msgq_queue_t *q);
|
||||
@@ -1,210 +0,0 @@
|
||||
#include <time.h>
|
||||
#include <assert.h>
|
||||
#include <stdlib.h>
|
||||
#include <string>
|
||||
#include <mutex>
|
||||
|
||||
#include "cereal/services.h"
|
||||
#include "cereal/messaging/messaging.h"
|
||||
|
||||
const bool SIMULATION = (getenv("SIMULATION") != nullptr) && (std::string(getenv("SIMULATION")) == "1");
|
||||
|
||||
static inline uint64_t nanos_since_boot() {
|
||||
struct timespec t;
|
||||
clock_gettime(CLOCK_BOOTTIME, &t);
|
||||
return t.tv_sec * 1000000000ULL + t.tv_nsec;
|
||||
}
|
||||
|
||||
static inline bool inList(const std::vector<const char *> &list, const char *value) {
|
||||
for (auto &v : list) {
|
||||
if (strcmp(value, v) == 0) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
class MessageContext {
|
||||
public:
|
||||
MessageContext() : ctx_(nullptr) {}
|
||||
~MessageContext() { delete ctx_; }
|
||||
inline Context *context() {
|
||||
std::call_once(init_flag, [=]() { ctx_ = Context::create(); });
|
||||
return ctx_;
|
||||
}
|
||||
private:
|
||||
Context *ctx_;
|
||||
std::once_flag init_flag;
|
||||
};
|
||||
|
||||
MessageContext message_context;
|
||||
|
||||
struct SubMaster::SubMessage {
|
||||
std::string name;
|
||||
SubSocket *socket = nullptr;
|
||||
int freq = 0;
|
||||
bool updated = false, alive = false, valid = true, ignore_alive;
|
||||
uint64_t rcv_time = 0, rcv_frame = 0;
|
||||
void *allocated_msg_reader = nullptr;
|
||||
bool is_polled = false;
|
||||
capnp::FlatArrayMessageReader *msg_reader = nullptr;
|
||||
AlignedBuffer aligned_buf;
|
||||
cereal::Event::Reader event;
|
||||
};
|
||||
|
||||
SubMaster::SubMaster(const std::vector<const char *> &service_list, const std::vector<const char *> &poll,
|
||||
const char *address, const std::vector<const char *> &ignore_alive) {
|
||||
poller_ = Poller::create();
|
||||
for (auto name : service_list) {
|
||||
assert(services.count(std::string(name)) > 0);
|
||||
|
||||
service serv = services.at(std::string(name));
|
||||
SubSocket *socket = SubSocket::create(message_context.context(), name, address ? address : "127.0.0.1", true);
|
||||
assert(socket != 0);
|
||||
bool is_polled = inList(poll, name) || poll.empty();
|
||||
if (is_polled) poller_->registerSocket(socket);
|
||||
SubMessage *m = new SubMessage{
|
||||
.name = name,
|
||||
.socket = socket,
|
||||
.freq = serv.frequency,
|
||||
.ignore_alive = inList(ignore_alive, name),
|
||||
.allocated_msg_reader = malloc(sizeof(capnp::FlatArrayMessageReader)),
|
||||
.is_polled = is_polled};
|
||||
m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader({});
|
||||
messages_[socket] = m;
|
||||
services_[name] = m;
|
||||
}
|
||||
}
|
||||
|
||||
void SubMaster::update(int timeout) {
|
||||
for (auto &kv : messages_) kv.second->updated = false;
|
||||
|
||||
auto sockets = poller_->poll(timeout);
|
||||
|
||||
// add non-polled sockets for non-blocking receive
|
||||
for (auto &kv : messages_) {
|
||||
SubMessage *m = kv.second;
|
||||
SubSocket *s = kv.first;
|
||||
if (!m->is_polled) sockets.push_back(s);
|
||||
}
|
||||
|
||||
uint64_t current_time = nanos_since_boot();
|
||||
|
||||
std::vector<std::pair<std::string, cereal::Event::Reader>> messages;
|
||||
|
||||
for (auto s : sockets) {
|
||||
Message *msg = s->receive(true);
|
||||
if (msg == nullptr) continue;
|
||||
|
||||
SubMessage *m = messages_.at(s);
|
||||
|
||||
m->msg_reader->~FlatArrayMessageReader();
|
||||
capnp::ReaderOptions options;
|
||||
options.traversalLimitInWords = kj::maxValue; // Don't limit
|
||||
m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader(m->aligned_buf.align(msg), options);
|
||||
delete msg;
|
||||
messages.push_back({m->name, m->msg_reader->getRoot<cereal::Event>()});
|
||||
}
|
||||
|
||||
update_msgs(current_time, messages);
|
||||
}
|
||||
|
||||
void SubMaster::update_msgs(uint64_t current_time, const std::vector<std::pair<std::string, cereal::Event::Reader>> &messages){
|
||||
if (++frame == UINT64_MAX) frame = 1;
|
||||
|
||||
for (auto &kv : messages) {
|
||||
auto m_find = services_.find(kv.first);
|
||||
if (m_find == services_.end()){
|
||||
continue;
|
||||
}
|
||||
SubMessage *m = m_find->second;
|
||||
m->event = kv.second;
|
||||
m->updated = true;
|
||||
m->rcv_time = current_time;
|
||||
m->rcv_frame = frame;
|
||||
m->valid = m->event.getValid();
|
||||
if (SIMULATION) m->alive = true;
|
||||
}
|
||||
|
||||
if (!SIMULATION) {
|
||||
for (auto &kv : messages_) {
|
||||
SubMessage *m = kv.second;
|
||||
m->alive = (m->freq <= (1e-5) || ((current_time - m->rcv_time) * (1e-9)) < (10.0 / m->freq));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool SubMaster::all_(const std::vector<const char *> &service_list, bool valid, bool alive) {
|
||||
int found = 0;
|
||||
for (auto &kv : messages_) {
|
||||
SubMessage *m = kv.second;
|
||||
if (service_list.size() == 0 || inList(service_list, m->name.c_str())) {
|
||||
found += (!valid || m->valid) && (!alive || (m->alive || m->ignore_alive));
|
||||
}
|
||||
}
|
||||
return service_list.size() == 0 ? found == messages_.size() : found == service_list.size();
|
||||
}
|
||||
|
||||
void SubMaster::drain() {
|
||||
while (true) {
|
||||
auto polls = poller_->poll(0);
|
||||
if (polls.size() == 0)
|
||||
break;
|
||||
|
||||
for (auto sock : polls) {
|
||||
Message *msg = sock->receive(true);
|
||||
delete msg;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool SubMaster::updated(const char *name) const {
|
||||
return services_.at(name)->updated;
|
||||
}
|
||||
|
||||
bool SubMaster::alive(const char *name) const {
|
||||
return services_.at(name)->alive;
|
||||
}
|
||||
|
||||
bool SubMaster::valid(const char *name) const {
|
||||
return services_.at(name)->valid;
|
||||
}
|
||||
|
||||
uint64_t SubMaster::rcv_frame(const char *name) const {
|
||||
return services_.at(name)->rcv_frame;
|
||||
}
|
||||
|
||||
uint64_t SubMaster::rcv_time(const char *name) const {
|
||||
return services_.at(name)->rcv_time;
|
||||
}
|
||||
|
||||
cereal::Event::Reader &SubMaster::operator[](const char *name) const {
|
||||
return services_.at(name)->event;
|
||||
}
|
||||
|
||||
SubMaster::~SubMaster() {
|
||||
delete poller_;
|
||||
for (auto &kv : messages_) {
|
||||
SubMessage *m = kv.second;
|
||||
m->msg_reader->~FlatArrayMessageReader();
|
||||
free(m->allocated_msg_reader);
|
||||
delete m->socket;
|
||||
delete m;
|
||||
}
|
||||
}
|
||||
|
||||
PubMaster::PubMaster(const std::vector<const char *> &service_list) {
|
||||
for (auto name : service_list) {
|
||||
assert(services.count(name) > 0);
|
||||
PubSocket *socket = PubSocket::create(message_context.context(), name);
|
||||
assert(socket);
|
||||
sockets_[name] = socket;
|
||||
}
|
||||
}
|
||||
|
||||
int PubMaster::send(const char *name, MessageBuilder &msg) {
|
||||
auto bytes = msg.toBytes();
|
||||
return send(name, bytes.begin(), bytes.size());
|
||||
}
|
||||
|
||||
PubMaster::~PubMaster() {
|
||||
for (auto s : sockets_) delete s.second;
|
||||
}
|
||||
Reference in New Issue
Block a user