IQ.Pilot Release Commit @ b6534c0

iqpilot/cereal/messaging/__init__.py

@@ -0,0 +1,269 @@
+# must be built with scons
+from msgq import fake_event_handle, drain_sock_raw, MultiplePublishersError, IpcError, \
+                 Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \
+                 set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event
+import msgq
+import os
+import capnp
+import time
+
+from typing import Optional, List, Union, Dict
+
+from iqpilot.cereal import log
+from iqpilot.cereal.services import SERVICE_LIST
+from iqpilot.common.utils import MovingAverage
+
+NO_TRAVERSAL_LIMIT = 2**64-1
+
+
+def pub_sock(endpoint: str) -> PubSocket:
+  service = SERVICE_LIST.get(endpoint)
+  segment_size = service.queue_size if service else 0
+  return msgq.pub_sock(endpoint, segment_size)
+
+
+def sub_sock(endpoint: str, poller: Optional[Poller] = None, addr: str = "127.0.0.1",
+             conflate: bool = False, timeout: Optional[int] = None) -> SubSocket:
+  service = SERVICE_LIST.get(endpoint)
+  segment_size = service.queue_size if service else 0
+  return msgq.sub_sock(endpoint, poller=poller, addr=addr, conflate=conflate,
+                       timeout=timeout, segment_size=segment_size)
+
+
+def reset_context():
+  msgq.context = Context()
+
+
+def log_from_bytes(dat: bytes, struct: capnp.lib.capnp._StructModule = log.Event) -> capnp.lib.capnp._DynamicStructReader:
+  with struct.from_bytes(dat, traversal_limit_in_words=NO_TRAVERSAL_LIMIT) as msg:
+    return msg
+
+
+def new_message(service: Optional[str], size: Optional[int] = None, **kwargs) -> capnp.lib.capnp._DynamicStructBuilder:
+  args = {
+    'valid': False,
+    'logMonoTime': int(time.monotonic() * 1e9),
+    **kwargs
+  }
+  dat = log.Event.new_message(**args)
+  if service is not None:
+    if size is None:
+      dat.init(service)
+    else:
+      dat.init(service, size)
+  return dat
+
+
+def drain_sock(sock: SubSocket, wait_for_one: bool = False) -> List[capnp.lib.capnp._DynamicStructReader]:
+  """Receive all message currently available on the queue"""
+  msgs = drain_sock_raw(sock, wait_for_one=wait_for_one)
+  return [log_from_bytes(m) for m in msgs]
+
+
+# TODO: print when we drop packets?
+def recv_sock(sock: SubSocket, wait: bool = False) -> Optional[capnp.lib.capnp._DynamicStructReader]:
+  """Same as drain sock, but only returns latest message. Consider using conflate instead."""
+  dat = None
+
+  while 1:
+    if wait and dat is None:
+      recv = sock.receive()
+    else:
+      recv = sock.receive(non_blocking=True)
+
+    if recv is None:  # Timeout hit
+      break
+
+    dat = recv
+
+  if dat is not None:
+    dat = log_from_bytes(dat)
+
+  return dat
+
+
+def recv_one(sock: SubSocket) -> Optional[capnp.lib.capnp._DynamicStructReader]:
+  dat = sock.receive()
+  if dat is not None:
+    dat = log_from_bytes(dat)
+  return dat
+
+
+def recv_one_or_none(sock: SubSocket) -> Optional[capnp.lib.capnp._DynamicStructReader]:
+  dat = sock.receive(non_blocking=True)
+  if dat is not None:
+    dat = log_from_bytes(dat)
+  return dat
+
+
+def recv_one_retry(sock: SubSocket) -> capnp.lib.capnp._DynamicStructReader:
+  """Keep receiving until we get a message"""
+  while True:
+    dat = sock.receive()
+    if dat is not None:
+      return log_from_bytes(dat)
+
+
+class FrequencyTracker:
+  def __init__(self, service_freq: float, update_freq: float, is_poll: bool):
+    freq = max(min(service_freq, update_freq), 1.)
+    if is_poll:
+      min_freq = max_freq = freq
+    else:
+      max_freq = min(freq, update_freq)
+      if service_freq >= 2 * update_freq:
+        min_freq = update_freq
+      elif update_freq >= 2* service_freq:
+        min_freq = freq
+      else:
+        min_freq = min(freq, freq / 2.)
+
+    self.min_freq = min_freq * 0.8
+    self.max_freq = max_freq * 1.2
+    self.avg_dt = MovingAverage(int(10 * freq))
+    self.recent_avg_dt = MovingAverage(int(freq))
+    self.prev_time = 0.0
+
+  def record_recv_time(self, cur_time: float) -> None:
+    # TODO: Handle case where cur_time is less than prev_time
+    if self.prev_time > 1e-5:
+      dt = cur_time - self.prev_time
+
+      self.avg_dt.add_value(dt)
+      self.recent_avg_dt.add_value(dt)
+
+    self.prev_time = cur_time
+
+  @property
+  def valid(self) -> bool:
+    if self.avg_dt.count == 0:
+      return False
+
+    avg_freq = 1.0 / self.avg_dt.get_average()
+    if self.min_freq <= avg_freq <= self.max_freq:
+      return True
+
+    avg_freq_recent = 1.0 / self.recent_avg_dt.get_average()
+    return self.min_freq <= avg_freq_recent <= self.max_freq
+
+
+class SubMaster:
+  def __init__(self, services: List[str], poll: Optional[str] = None,
+               ignore_alive: Optional[List[str]] = None, ignore_avg_freq: Optional[List[str]] = None,
+               ignore_valid: Optional[List[str]] = None, addr: str = "127.0.0.1", frequency: Optional[float] = None):
+    self.frame = -1
+    self.services = services
+    self.seen = {s: False for s in services}
+    self.updated = {s: False for s in services}
+    self.recv_time = {s: 0. for s in services}
+    self.recv_frame = {s: 0 for s in services}
+    self.sock = {}
+    self.data = {}
+    self.logMonoTime = {s: 0 for s in services}
+
+    # zero-frequency / on-demand services are always alive and presumed valid; all others must pass checks
+    on_demand = {s: SERVICE_LIST[s].frequency <= 1e-5 for s in services}
+    self.static_freq_services = set(s for s in services if not on_demand[s])
+    self.alive = {s: on_demand[s] for s in services}
+    self.freq_ok = {s: on_demand[s] for s in services}
+    self.valid = {s: on_demand[s] for s in services}
+
+    self.freq_tracker: Dict[str, FrequencyTracker] = {}
+    self.poller = Poller()
+    polled_services = set([poll, ] if poll is not None else services)
+    self.non_polled_services = set(services) - polled_services
+
+    self.ignore_average_freq = [] if ignore_avg_freq is None else ignore_avg_freq
+    self.ignore_alive = [] if ignore_alive is None else ignore_alive
+    self.ignore_valid = [] if ignore_valid is None else ignore_valid
+
+    self.simulation = bool(int(os.getenv("SIMULATION", "0")))
+
+    # if freq and poll aren't specified, assume the max to be conservative
+    assert frequency is None or poll is None, "Do not specify 'frequency' - frequency of the polled service will be used."
+    self.update_freq = frequency or max([SERVICE_LIST[s].frequency for s in polled_services])
+
+    for s in services:
+      p = self.poller if s not in self.non_polled_services else None
+      self.sock[s] = sub_sock(s, poller=p, addr=addr, conflate=True)
+
+      try:
+        data = new_message(s)
+      except capnp.lib.capnp.KjException:
+        data = new_message(s, 0) # lists
+
+      self.data[s] = getattr(data.as_reader(), s)
+      self.freq_tracker[s] = FrequencyTracker(SERVICE_LIST[s].frequency, self.update_freq, s == poll)
+
+  def __getitem__(self, s: str) -> capnp.lib.capnp._DynamicStructReader:
+    return self.data[s]
+
+  def _check_avg_freq(self, s: str) -> bool:
+    return SERVICE_LIST[s].frequency > 0.99 and (s not in self.ignore_average_freq) and (s not in self.ignore_alive)
+
+  def update(self, timeout: int = 100) -> None:
+    msgs = []
+    for sock in self.poller.poll(timeout):
+      msgs.append(recv_one_or_none(sock))
+
+    # non-blocking receive for non-polled sockets
+    for s in self.non_polled_services:
+      msgs.append(recv_one_or_none(self.sock[s]))
+    self.update_msgs(time.monotonic(), msgs)
+
+  def update_msgs(self, cur_time: float, msgs: List[capnp.lib.capnp._DynamicStructReader]) -> None:
+    self.frame += 1
+    self.updated = dict.fromkeys(self.services, False)
+    for msg in msgs:
+      if msg is None:
+        continue
+
+      s = msg.which()
+      self.seen[s] = True
+      self.updated[s] = True
+
+      self.freq_tracker[s].record_recv_time(cur_time)
+      self.recv_time[s] = cur_time
+      self.recv_frame[s] = self.frame
+      self.data[s] = getattr(msg, s)
+      self.logMonoTime[s] = msg.logMonoTime
+      self.valid[s] = msg.valid
+
+    for s in self.static_freq_services:
+      # alive if delay is within 10x the expected frequency; checks relaxed in simulator
+      self.alive[s] = (cur_time - self.recv_time[s]) < (10. / SERVICE_LIST[s].frequency) or (self.seen[s] and self.simulation)
+      self.freq_ok[s] = self.freq_tracker[s].valid or self.simulation
+
+  def all_alive(self, service_list: Optional[List[str]] = None) -> bool:
+    return all(self.alive[s] for s in (service_list or self.services) if s not in self.ignore_alive)
+
+  def all_freq_ok(self, service_list: Optional[List[str]] = None) -> bool:
+    return all(self.freq_ok[s] for s in (service_list or self.services) if self._check_avg_freq(s))
+
+  def all_valid(self, service_list: Optional[List[str]] = None) -> bool:
+    return all(self.valid[s] for s in (service_list or self.services) if s not in self.ignore_valid)
+
+  def all_checks(self, service_list: Optional[List[str]] = None) -> bool:
+    return self.all_alive(service_list) and self.all_freq_ok(service_list) and self.all_valid(service_list)
+
+
+class PubMaster:
+  def __init__(self, services: List[str]):
+    self.sock = {}
+    for s in services:
+      self.sock[s] = pub_sock(s)
+
+  def send(self, s: str, dat: Union[bytes, capnp.lib.capnp._DynamicStructBuilder]) -> None:
+    if not isinstance(dat, bytes):
+      dat = dat.to_bytes()
+    self.sock[s].send(dat)
+
+  def wait_for_readers_to_update(self, s: str, timeout: int, dt: float = 0.05) -> bool:
+    for _ in range(int(timeout*(1./dt))):
+      if self.sock[s].all_readers_updated():
+        return True
+      time.sleep(dt)
+    return False
+
+  def all_readers_updated(self, s: str) -> bool:
+    return self.sock[s].all_readers_updated()  # type: ignore

iqpilot/cereal/messaging/bridge.cc

@@ -0,0 +1,72 @@
+#include <cassert>
+
+#include "cereal/messaging/msgq_to_zmq.h"
+#include "cereal/services.h"
+#include "common/util.h"
+
+ExitHandler do_exit;
+
+static std::vector<std::string> get_services(const 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 (zmq_to_msgq && !in_whitelist) {
+      continue;
+    }
+    service_list.push_back(name);
+  }
+  return service_list;
+}
+
+void msgq_to_zmq(const std::vector<std::string> &endpoints, const std::string &ip) {
+  MsgqToZmq bridge;
+  bridge.run(endpoints, ip);
+}
+
+void zmq_to_msgq(const std::vector<std::string> &endpoints, const std::string &ip) {
+  auto poller = std::make_unique<ZMQPoller>();
+  auto pub_context = std::make_unique<MSGQContext>();
+  auto sub_context = std::make_unique<ZMQContext>();
+  std::map<SubSocket *, PubSocket *> sub2pub;
+
+  for (auto endpoint : endpoints) {
+    auto pub_sock = new MSGQPubSocket();
+    auto sub_sock = new ZMQSubSocket();
+    size_t queue_size = services.at(endpoint).queue_size;
+    pub_sock->connect(pub_context.get(), endpoint, true, queue_size);
+    sub_sock->connect(sub_context.get(), endpoint, ip, false);
+
+    poller->registerSocket(sub_sock);
+    sub2pub[sub_sock] = pub_sock;
+  }
+
+  while (!do_exit) {
+    for (auto sub_sock : poller->poll(100)) {
+      std::unique_ptr<Message> msg(sub_sock->receive(true));
+      if (msg) {
+        sub2pub[sub_sock]->sendMessage(msg.get());
+      }
+    }
+  }
+
+  // Clean up allocated sockets
+  for (auto &[sub_sock, pub_sock] : sub2pub) {
+    delete sub_sock;
+    delete pub_sock;
+  }
+}
+
+int main(int argc, char **argv) {
+  bool is_zmq_to_msgq = argc > 2;
+  std::string ip = is_zmq_to_msgq ? argv[1] : "127.0.0.1";
+  std::string whitelist_str = is_zmq_to_msgq ? std::string(argv[2]) : "";
+  std::vector<std::string> endpoints = get_services(whitelist_str, is_zmq_to_msgq);
+
+  if (is_zmq_to_msgq) {
+    zmq_to_msgq(endpoints, ip);
+  } else {
+    msgq_to_zmq(endpoints, ip);
+  }
+  return 0;
+}

iqpilot/cereal/messaging/messaging.h

@@ -0,0 +1,102 @@
+#pragma once
+
+#include <cstddef>
+#include <map>
+#include <string>
+#include <vector>
+#include <utility>
+
+#include <capnp/serialize.h>
+
+#include "cereal/gen/cpp/log.capnp.h"
+#include "common/timing.h"
+#include "msgq/ipc.h"
+
+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>();
+    event.setLogMonoTime(nanos_since_boot());
+    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;
+};

iqpilot/cereal/messaging/msgq_to_zmq.cc

@@ -0,0 +1,146 @@
+#include "cereal/messaging/msgq_to_zmq.h"
+
+#include <cassert>
+
+#include "cereal/services.h"
+#include "common/util.h"
+
+extern ExitHandler do_exit;
+
+// Max messages to process per socket per poll
+constexpr int MAX_MESSAGES_PER_SOCKET = 50;
+
+static std::string recv_zmq_msg(void *sock) {
+  zmq_msg_t msg;
+  zmq_msg_init(&msg);
+  std::string ret;
+  if (zmq_msg_recv(&msg, sock, 0) > 0) {
+    ret.assign((char *)zmq_msg_data(&msg), zmq_msg_size(&msg));
+  }
+  zmq_msg_close(&msg);
+  return ret;
+}
+
+void MsgqToZmq::run(const std::vector<std::string> &endpoints, const std::string &ip) {
+  zmq_context = std::make_unique<ZMQContext>();
+  msgq_context = std::make_unique<MSGQContext>();
+
+  // Create ZMQPubSockets for each endpoint
+  for (const auto &endpoint : endpoints) {
+    auto &socket_pair = socket_pairs.emplace_back();
+    socket_pair.endpoint = endpoint;
+    socket_pair.pub_sock = std::make_unique<ZMQPubSocket>();
+    int ret = socket_pair.pub_sock->connect(zmq_context.get(), endpoint);
+    if (ret != 0) {
+      printf("Failed to create ZMQ publisher for [%s]: %s\n", endpoint.c_str(), zmq_strerror(zmq_errno()));
+      return;
+    }
+  }
+
+  // Start ZMQ monitoring thread to monitor socket events
+  std::thread thread(&MsgqToZmq::zmqMonitorThread, this);
+
+  // Main loop for processing messages
+  while (!do_exit) {
+    {
+      std::unique_lock lk(mutex);
+      cv.wait(lk, [this]() { return do_exit || !sub2pub.empty(); });
+      if (do_exit) break;
+
+      for (auto sub_sock : msgq_poller->poll(100)) {
+        // Process messages for each socket
+        ZMQPubSocket *pub_sock = sub2pub.at(sub_sock);
+        for (int i = 0; i < MAX_MESSAGES_PER_SOCKET; ++i) {
+          auto msg = std::unique_ptr<Message>(sub_sock->receive(true));
+          if (!msg) break;
+
+          while (pub_sock->sendMessage(msg.get()) == -1) {
+            if (errno != EINTR) break;
+          }
+        }
+      }
+    }
+    util::sleep_for(1);  // Give zmqMonitorThread a chance to acquire the mutex
+  }
+
+  thread.join();
+}
+
+void MsgqToZmq::zmqMonitorThread() {
+  std::vector<zmq_pollitem_t> pollitems;
+
+  // Set up ZMQ monitor for each pub socket
+  for (int i = 0; i < socket_pairs.size(); ++i) {
+    std::string addr = "inproc://op-bridge-monitor-" + std::to_string(i);
+    zmq_socket_monitor(socket_pairs[i].pub_sock->sock, addr.c_str(), ZMQ_EVENT_ACCEPTED | ZMQ_EVENT_DISCONNECTED);
+
+    void *monitor_socket = zmq_socket(zmq_context->getRawContext(), ZMQ_PAIR);
+    zmq_connect(monitor_socket, addr.c_str());
+    pollitems.emplace_back(zmq_pollitem_t{.socket = monitor_socket, .events = ZMQ_POLLIN});
+  }
+
+  while (!do_exit) {
+    int ret = zmq_poll(pollitems.data(), pollitems.size(), 1000);
+    if (ret < 0) {
+      if (errno == EINTR) {
+        // Due to frequent EINTR signals from msgq, introduce a brief delay (200 ms)
+        // to reduce CPU usage during retry attempts.
+        util::sleep_for(200);
+      }
+      continue;
+    }
+
+    for (int i = 0; i < pollitems.size(); ++i) {
+      if (pollitems[i].revents & ZMQ_POLLIN) {
+        // First frame in message contains event number and value
+        std::string frame = recv_zmq_msg(pollitems[i].socket);
+        if (frame.empty()) continue;
+
+        uint16_t event_type = *(uint16_t *)(frame.data());
+
+        // Second frame in message contains event address
+        frame = recv_zmq_msg(pollitems[i].socket);
+        if (frame.empty()) continue;
+
+        std::unique_lock lk(mutex);
+        auto &pair = socket_pairs[i];
+        if (event_type & ZMQ_EVENT_ACCEPTED) {
+          printf("socket [%s] connected\n", pair.endpoint.c_str());
+          if (++pair.connected_clients == 1) {
+            // Create new MSGQ subscriber socket and map to ZMQ publisher
+            pair.sub_sock = std::make_unique<MSGQSubSocket>();
+            size_t queue_size = services.at(pair.endpoint).queue_size;
+            pair.sub_sock->connect(msgq_context.get(), pair.endpoint, "127.0.0.1", false, true, queue_size);
+            sub2pub[pair.sub_sock.get()] = pair.pub_sock.get();
+            registerSockets();
+          }
+        } else if (event_type & ZMQ_EVENT_DISCONNECTED) {
+          printf("socket [%s] disconnected\n", pair.endpoint.c_str());
+          if (pair.connected_clients == 0 || --pair.connected_clients == 0) {
+            // Remove MSGQ subscriber socket from mapping and reset it
+            sub2pub.erase(pair.sub_sock.get());
+            pair.sub_sock.reset(nullptr);
+            registerSockets();
+          }
+        }
+        cv.notify_one();
+      }
+    }
+  }
+
+  // Clean up monitor sockets
+  for (int i = 0; i < pollitems.size(); ++i) {
+    zmq_socket_monitor(socket_pairs[i].pub_sock->sock, nullptr, 0);
+    zmq_close(pollitems[i].socket);
+  }
+  cv.notify_one();
+}
+
+void MsgqToZmq::registerSockets() {
+  msgq_poller = std::make_unique<MSGQPoller>();
+  for (const auto &socket_pair : socket_pairs) {
+    if (socket_pair.sub_sock) {
+      msgq_poller->registerSocket(socket_pair.sub_sock.get());
+    }
+  }
+}

iqpilot/cereal/messaging/msgq_to_zmq.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <condition_variable>
+#include <map>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#define private public
+#include "msgq/impl_msgq.h"
+#include "msgq/impl_zmq.h"
+
+class MsgqToZmq {
+public:
+  MsgqToZmq() {}
+  void run(const std::vector<std::string> &endpoints, const std::string &ip);
+
+protected:
+  void registerSockets();
+  void zmqMonitorThread();
+
+  struct SocketPair {
+    std::string endpoint;
+    std::unique_ptr<ZMQPubSocket> pub_sock;
+    std::unique_ptr<MSGQSubSocket> sub_sock;
+    int connected_clients = 0;
+  };
+
+  std::unique_ptr<MSGQContext> msgq_context;
+  std::unique_ptr<ZMQContext> zmq_context;
+  std::mutex mutex;
+  std::condition_variable cv;
+  std::unique_ptr<MSGQPoller> msgq_poller;
+  std::map<SubSocket *, ZMQPubSocket *> sub2pub;
+  std::vector<SocketPair> socket_pairs;
+};

iqpilot/cereal/messaging/socketmaster.cc

@@ -0,0 +1,211 @@
+#include <assert.h>
+#include <stdio.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 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;
+  float freq = 0.0f;
+  bool updated = false, alive = false, valid = false, 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) {
+    if (services.count(std::string(name)) == 0) {
+      fprintf(stderr, "SubMaster: unknown service '%s', skipping subscription\n", name);
+      continue;
+    }
+
+    service serv = services.at(std::string(name));
+    SubSocket *socket = SubSocket::create(message_context.context(), name, address ? address : "127.0.0.1", true, true, serv.queue_size);
+    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) {
+    if (services.count(name) == 0) {
+      fprintf(stderr, "PubMaster: unknown service '%s', skipping publisher\n", name);
+      continue;
+    }
+    service serv = services.at(std::string(name));
+    PubSocket *socket = PubSocket::create(message_context.context(), name, true, serv.queue_size);
+    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;
+}

iqpilot/cereal/messaging/tests/test_messaging.py

@@ -0,0 +1,186 @@
+import os
+import capnp
+import numbers
+import random
+import threading
+import time
+from parameterized import parameterized
+
+from iqpilot.cereal import log, car
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal.services import SERVICE_LIST
+
+events = [evt for evt in log.Event.schema.union_fields if evt in SERVICE_LIST.keys()]
+
+def random_sock():
+  return random.choice(events)
+
+def random_socks(num_socks=10):
+  return list({random_sock() for _ in range(num_socks)})
+
+def random_bytes(length=1000):
+  return bytes([random.randrange(0xFF) for _ in range(length)])
+
+def zmq_sleep(t=1):
+  if "ZMQ" in os.environ:
+    time.sleep(t)
+
+
+# TODO: this should take any capnp struct and returrn a msg with random populated data
+def random_carstate():
+  fields = ["vEgo", "aEgo", "brake", "steeringAngleDeg"]
+  msg = messaging.new_message("carState")
+  cs = msg.carState
+  for f in fields:
+    setattr(cs, f, random.random() * 10)
+  return msg
+
+# TODO: this should compare any capnp structs
+def assert_carstate(cs1, cs2):
+  for f in car.CarState.schema.non_union_fields:
+    # TODO: check all types
+    val1, val2 = getattr(cs1, f), getattr(cs2, f)
+    if isinstance(val1, numbers.Number):
+      assert val1 == val2, f"{f}: sent '{val1}' vs recvd '{val2}'"
+
+def delayed_send(delay, sock, dat):
+  def send_func():
+    sock.send(dat)
+  threading.Timer(delay, send_func).start()
+
+
+class TestMessaging:
+  def setUp(self):
+    # TODO: ZMQ tests are too slow; all sleeps will need to be
+    # replaced with logic to block on the necessary condition
+    assert "ZMQ" not in os.environ
+
+    # ZMQ pub socket takes too long to die
+    # sleep to prevent multiple publishers error between tests
+    zmq_sleep()
+
+  @parameterized.expand(events)
+  def test_new_message(self, evt):
+    try:
+      msg = messaging.new_message(evt)
+    except capnp.lib.capnp.KjException:
+      msg = messaging.new_message(evt, random.randrange(200))
+    assert (time.monotonic() - msg.logMonoTime) < 0.1
+    assert not msg.valid
+    assert evt == msg.which()
+
+  @parameterized.expand(events)
+  def test_pub_sock(self, evt):
+    messaging.pub_sock(evt)
+
+  @parameterized.expand(events)
+  def test_sub_sock(self, evt):
+    messaging.sub_sock(evt)
+
+  @parameterized.expand([
+    (messaging.drain_sock, capnp._DynamicStructReader),
+    (messaging.drain_sock_raw, bytes),
+  ])
+  def test_drain_sock(self, func, expected_type):
+    sock = "carState"
+    pub_sock = messaging.pub_sock(sock)
+    sub_sock = messaging.sub_sock(sock, timeout=1000)
+    zmq_sleep()
+
+    # no wait and no msgs in queue
+    msgs = func(sub_sock)
+    assert isinstance(msgs, list)
+    assert len(msgs) == 0
+
+    # no wait but msgs are queued up
+    num_msgs = random.randrange(3, 10)
+    for _ in range(num_msgs):
+      pub_sock.send(messaging.new_message(sock).to_bytes())
+    time.sleep(0.1)
+    msgs = func(sub_sock)
+    assert isinstance(msgs, list)
+    assert all(isinstance(msg, expected_type) for msg in msgs)
+    assert len(msgs) == num_msgs
+
+  def test_recv_sock(self):
+    sock = "carState"
+    pub_sock = messaging.pub_sock(sock)
+    sub_sock = messaging.sub_sock(sock, timeout=100)
+    zmq_sleep()
+
+    # no wait and no msg in queue, socket should timeout
+    recvd = messaging.recv_sock(sub_sock)
+    assert recvd is None
+
+    # no wait and one msg in queue
+    msg = random_carstate()
+    pub_sock.send(msg.to_bytes())
+    time.sleep(0.01)
+    recvd = messaging.recv_sock(sub_sock)
+    assert isinstance(recvd, capnp._DynamicStructReader)
+    # https://github.com/python/mypy/issues/13038
+    assert_carstate(msg.carState, recvd.carState)
+
+  def test_recv_one(self):
+    sock = "carState"
+    pub_sock = messaging.pub_sock(sock)
+    sub_sock = messaging.sub_sock(sock, timeout=1000)
+    zmq_sleep()
+
+    # no msg in queue, socket should timeout
+    recvd = messaging.recv_one(sub_sock)
+    assert recvd is None
+
+    # one msg in queue
+    msg = random_carstate()
+    pub_sock.send(msg.to_bytes())
+    recvd = messaging.recv_one(sub_sock)
+    assert isinstance(recvd, capnp._DynamicStructReader)
+    assert_carstate(msg.carState, recvd.carState)
+
+  def test_recv_one_or_none(self):
+    sock = "carState"
+    pub_sock = messaging.pub_sock(sock)
+    sub_sock = messaging.sub_sock(sock)
+    zmq_sleep()
+
+    # no msg in queue, socket shouldn't block
+    recvd = messaging.recv_one_or_none(sub_sock)
+    assert recvd is None
+
+    # one msg in queue
+    msg = random_carstate()
+    pub_sock.send(msg.to_bytes())
+    recvd = messaging.recv_one_or_none(sub_sock)
+    assert isinstance(recvd, capnp._DynamicStructReader)
+    assert_carstate(msg.carState, recvd.carState)
+
+  def test_recv_one_retry(self):
+    sock = "carState"
+    sock_timeout = 0.1
+    pub_sock = messaging.pub_sock(sock)
+    sub_sock = messaging.sub_sock(sock, timeout=round(sock_timeout*1000))
+    zmq_sleep()
+
+    # this test doesn't work with ZMQ since multiprocessing interrupts it
+    if "ZMQ" not in os.environ:
+      # wait 5 socket timeouts and make sure it's still retrying
+      result = []
+      thread = threading.Thread(target=lambda: result.append(messaging.recv_one_retry(sub_sock)))
+      thread.start()
+      time.sleep(sock_timeout*5)
+      assert thread.is_alive()
+      msg = random_carstate()
+      pub_sock.send(msg.to_bytes())
+      thread.join(timeout=1)
+      assert not thread.is_alive()
+      assert_carstate(msg.carState, result[0].carState)
+
+    # wait 5 socket timeouts before sending
+    msg = random_carstate()
+    start_time = time.monotonic()
+    delayed_send(sock_timeout*5, pub_sock, msg.to_bytes())
+    recvd = messaging.recv_one_retry(sub_sock)
+    assert (time.monotonic() - start_time) >= sock_timeout*5
+    assert isinstance(recvd, capnp._DynamicStructReader)
+    assert_carstate(msg.carState, recvd.carState)

iqpilot/cereal/messaging/tests/test_pub_sub_master.py

@@ -0,0 +1,161 @@
+import random
+import time
+from typing import Sized, cast
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal.messaging.tests.test_messaging import events, random_sock, random_socks, \
+                                                  random_bytes, random_carstate, assert_carstate, \
+                                                  zmq_sleep
+from iqpilot.cereal.services import SERVICE_LIST
+from iqpilot.common.timeout import Timeout
+
+
+class TestSubMaster:
+
+  def setup_method(self):
+    # ZMQ pub socket takes too long to die
+    # sleep to prevent multiple publishers error between tests
+    zmq_sleep(3)
+
+  def test_init(self):
+    sm = messaging.SubMaster(events)
+    for p in [sm.updated, sm.recv_time, sm.recv_frame, sm.alive,
+              sm.sock, sm.data, sm.logMonoTime, sm.valid]:
+      assert len(cast(Sized, p)) == len(events)
+
+  def test_init_state(self):
+    socks = random_socks()
+    sm = messaging.SubMaster(socks)
+    assert sm.frame == -1
+    assert not any(sm.updated.values())
+    assert not any(sm.seen.values())
+    on_demand = {s: SERVICE_LIST[s].frequency <= 1e-5 for s in sm.services}
+    assert all(sm.alive[s] == sm.valid[s] == sm.freq_ok[s] == on_demand[s] for s in sm.services)
+    assert all(t == 0. for t in sm.recv_time.values())
+    assert all(f == 0 for f in sm.recv_frame.values())
+    assert all(t == 0 for t in sm.logMonoTime.values())
+
+    for p in [sm.updated, sm.recv_time, sm.recv_frame, sm.alive,
+              sm.sock, sm.data, sm.logMonoTime, sm.valid]:
+      assert len(cast(Sized, p)) == len(socks)
+
+  def test_getitem(self):
+    sock = "carState"
+    pub_sock = messaging.pub_sock(sock)
+    sm = messaging.SubMaster([sock,])
+    zmq_sleep()
+
+    msg = random_carstate()
+    pub_sock.send(msg.to_bytes())
+    sm.update(1000)
+    assert_carstate(msg.carState, sm[sock])
+
+  # TODO: break this test up to individually test SubMaster.update and SubMaster.update_msgs
+  def test_update(self):
+    sock = "carState"
+    pub_sock = messaging.pub_sock(sock)
+    sm = messaging.SubMaster([sock,])
+    zmq_sleep()
+
+    for i in range(10):
+      msg = messaging.new_message(sock)
+      pub_sock.send(msg.to_bytes())
+      sm.update(1000)
+      assert sm.frame == i
+      assert all(sm.updated.values())
+
+  def test_update_timeout(self):
+    sock = random_sock()
+    sm = messaging.SubMaster([sock,])
+    timeout = 100
+    start_time = time.monotonic()
+    with Timeout(2):
+      sm.update(timeout)
+    t = time.monotonic() - start_time
+    assert t >= timeout/1000.
+    assert not any(sm.updated.values())
+
+  def test_avg_frequency_checks(self):
+    for poll in (True, False):
+      sm = messaging.SubMaster(["modelV2", "carParams", "carState", "cameraOdometry", "extrinsicsCalibration"],
+                               poll=("modelV2" if poll else None),
+                               frequency=(20. if not poll else None))
+
+      checks = {
+        "carState": (20, 20),
+        "modelV2": (20, 20 if poll else 10),
+        "cameraOdometry": (20, 10),
+        "extrinsicsCalibration": (4, 4),
+        "carParams": (None, None),
+        "userBookmark": (None, None),
+      }
+
+      for service, (max_freq, min_freq) in checks.items():
+        if max_freq is not None:
+          assert sm._check_avg_freq(service)
+          assert sm.freq_tracker[service].max_freq == max_freq*1.2
+          assert sm.freq_tracker[service].min_freq == min_freq*0.8
+        else:
+          assert not sm._check_avg_freq(service)
+
+  def test_alive(self):
+    pass
+
+  def test_ignore_alive(self):
+    pass
+
+  def test_valid(self):
+    pass
+
+  # SubMaster should always conflate
+  def test_conflate(self):
+    sock = "carState"
+    pub_sock = messaging.pub_sock(sock)
+    sm = messaging.SubMaster([sock,])
+
+    n = 10
+    for i in range(n+1):
+      msg = messaging.new_message(sock)
+      msg.carState.vEgo = i
+      pub_sock.send(msg.to_bytes())
+      time.sleep(0.01)
+    sm.update(1000)
+    assert sm[sock].vEgo == n
+
+
+class TestPubMaster:
+
+  def setup_method(self):
+    # ZMQ pub socket takes too long to die
+    # sleep to prevent multiple publishers error between tests
+    zmq_sleep(3)
+
+  def test_init(self):
+    messaging.PubMaster(events)
+
+  def test_send(self):
+    socks = random_socks()
+    pm = messaging.PubMaster(socks)
+    sub_socks = {s: messaging.sub_sock(s, conflate=True, timeout=1000) for s in socks}
+    zmq_sleep()
+
+    # PubMaster accepts either a capnp msg builder or bytes
+    for capnp in [True, False]:
+      for i in range(100):
+        sock = socks[i % len(socks)]
+
+        if capnp:
+          try:
+            msg = messaging.new_message(sock)
+          except Exception:
+            msg = messaging.new_message(sock, random.randrange(50))
+        else:
+          msg = random_bytes()
+
+        pm.send(sock, msg)
+        recvd = sub_socks[sock].receive()
+
+        if capnp:
+          msg.clear_write_flag()
+          msg = msg.to_bytes()
+        assert msg == recvd, i

iqpilot/cereal/messaging/tests/test_services.py

@@ -0,0 +1,27 @@
+import os
+import tempfile
+from typing import Dict
+from parameterized import parameterized
+
+from iqpilot.cereal import log
+import iqpilot.cereal.services as services
+from iqpilot.cereal.services import SERVICE_LIST
+
+
+class TestServices:
+
+  @parameterized.expand(SERVICE_LIST.keys())
+  def test_services(self, s):
+    service = SERVICE_LIST[s]
+    assert service.frequency <= 104
+    assert service.decimation != 0
+
+  def test_generated_header(self):
+    with tempfile.NamedTemporaryFile(suffix=".h") as f:
+      ret = os.system(f"python3 {services.__file__} > {f.name} && clang++ {f.name} -std=c++11")
+      assert ret == 0, "generated services header is not valid C"
+
+  def test_all_services_exist_in_log_union(self):
+    event_fields = set(log.Event.schema.union_fields)
+    missing = sorted(s for s in SERVICE_LIST if s not in event_fields)
+    assert not missing, f"services missing from log.capnp Event union: {missing}"

iqpilot/cereal/messaging/tests/validate_sp_cereal_upstream.py

@@ -0,0 +1,222 @@
+#!/usr/bin/env python3
+import argparse
+import sys
+from typing import Any, List, Tuple
+
+DEBUG = False
+
+
+def print_debug(string: str) -> None:
+  if DEBUG:
+    print(string)
+
+
+def create_schema_instance(struct: Any, prop: Tuple[str, Any]) -> Any:
+  """
+  Create a new instance of a schema type, handling different field types.
+
+  Args:
+      struct: The Cap'n Proto schema structure
+      prop: A tuple containing the field name and field metadata
+
+  Returns:
+      A new initialized schema instance
+  """
+  struct_instance = struct.new_message()
+  field_name, field_metadata = prop
+
+  try:
+    field_type = field_metadata.proto.slot.type.which()
+
+    # Initialize different types of fields
+    if field_type in ('list', 'text', 'data'):
+      struct_instance.init(field_name, 1)
+      print_debug(f"Initialized list/text/data field: {field_name}")
+    elif field_type in ('struct', 'object'):
+      struct_instance.init(field_name)
+      print_debug(f"Initialized struct/object field: {field_name}")
+
+    return struct_instance
+
+  except Exception as e:
+    print(f"Error creating instance for {field_name}: {e}")
+    return None
+
+
+def get_schema_fields(schema_struct: Any) -> List[Tuple[str, Any]]:
+  """
+  Retrieve all fields from a given schema structure.
+
+  Args:
+      schema_struct: The Cap'n Proto schema structure
+
+  Returns:
+      A list of field names and their metadata
+  """
+  try:
+    # Get all fields from the schema
+    schema_fields = list(schema_struct.schema.fields.items())
+
+    print_debug("Discovered schema fields:")
+    for field_name, field_metadata in schema_fields:
+      print_debug(f"- {field_name}")
+
+    return schema_fields
+
+  except Exception as e:
+    print(f"Error retrieving schema fields: {e}")
+    return []
+
+
+def generate_schema_instances(schema_struct: Any) -> List[Any]:
+  """
+  Generate instances for all fields in a given schema.
+
+  Args:
+      schema_struct: The Cap'n Proto schema structure
+
+  Returns:
+      A list of schema instances
+  """
+  schema_fields = get_schema_fields(schema_struct)
+  instances = []
+
+  for field_prop in schema_fields:
+    try:
+      instance = create_schema_instance(schema_struct, field_prop)
+      if instance is not None:
+        instances.append(instance)
+    except Exception as e:
+      print(f"Skipping field due to error: {e}")
+
+  print(f"Generated {len(instances)} schema instances")
+  return instances
+
+
+def persist_instances(instances: List[Any], filename: str) -> None:
+  """
+  Write schema instances to a binary file.
+
+  Args:
+      instances: List of schema instances
+      filename: Output file path
+  """
+  try:
+    with open(filename, 'wb') as f:
+      for instance in instances:
+        f.write(instance.to_bytes())
+
+    print(f"Successfully wrote {len(instances)} instances to {filename}")
+
+  except Exception as e:
+    print(f"Error persisting instances: {e}")
+    sys.exit(1)
+
+
+def read_instances(filename: str, schema_type: Any) -> List[Any]:
+  """
+  Read schema instances from a binary file.
+
+  Args:
+      filename: Input file path
+      schema_type: The schema type to use for reading
+
+  Returns:
+      A list of read schema instances
+  """
+  try:
+    with open(filename, 'rb') as f:
+      data = f.read()
+
+    instances = list(schema_type.read_multiple_bytes(data))
+
+    print(f"Read {len(instances)} instances from {filename}")
+    return instances
+
+  except Exception as e:
+    print(f"Error reading instances: {e}")
+    sys.exit(1)
+
+
+def compare_schemas(original_instances: List[Any], read_instances: List[Any]) -> bool:
+  """
+  Compare original and read-back instances to detect potential breaking changes.
+
+  Args:
+      original_instances: List of originally generated instances
+      read_instances: List of instances read back from file
+
+  Returns:
+      Boolean indicating whether schemas appear compatible
+  """
+  if len(original_instances) != len(read_instances):
+    print("❌ Schema Compatibility Warning: Instance count mismatch")
+    return False
+
+  compatible = True
+  for struct in read_instances:
+    try:
+      getattr(struct, struct.which())  # Attempting to access the field to validate readability
+    except Exception as e:
+      print(f"❌ Structural change detected: {struct.which()} is not readable.\nFull error: {e}")
+      compatible = False
+
+  return compatible
+
+
+def main():
+  """
+  CLI entry point for schema compatibility testing.
+  """
+  # Setup argument parser
+  parser = argparse.ArgumentParser(
+    description='Cap\'n Proto Schema Compatibility Testing Tool',
+    epilog='Test schema compatibility by generating and reading back instances.'
+  )
+
+  # Add mutually exclusive group for generation or reading mode
+  mode_group = parser.add_mutually_exclusive_group(required=True)
+  mode_group.add_argument('-g', '--generate', action='store_true',
+                          help='Generate schema instances')
+  mode_group.add_argument('-r', '--read', action='store_true',
+                          help='Read and validate schema instances')
+
+  # Common arguments
+  parser.add_argument('-f', '--file',
+                      default='schema_instances.bin',
+                      help='Output/input binary file (default: schema_instances.bin)')
+
+  # Parse arguments
+  args = parser.parse_args()
+
+  # Import the schema dynamically 
+  try:
+    from iqpilot.cereal import log
+    schema_type = log.Event
+  except ImportError:
+    print("Error: Unable to import schema. Ensure 'cereal' is installed.")
+    sys.exit(1)
+
+  # Execute based on mode
+  if args.generate:
+    print("🔧 Generating Schema Instances")
+    instances = generate_schema_instances(schema_type)
+    persist_instances(instances, args.file)
+    print("✅ Instance generation complete")
+
+  elif args.read:
+    print("🔍 Reading and Validating Schema Instances")
+    generated_instances = generate_schema_instances(schema_type)
+    read_back_instances = read_instances(args.file, schema_type)
+
+    # Compare schemas
+    if compare_schemas(generated_instances, read_back_instances):
+      print("✅ Schema Compatibility: No breaking changes detected")
+      sys.exit(0)
+    else:
+      print("❌ Potential Schema Breaking Changes Detected")
+      sys.exit(1)
+
+
+if __name__ == "__main__":
+  main()