IQ.Pilot Prebuilt Release @ 27f668a

artifacts/package_runtime/msgq/.gitignore

@@ -0,0 +1 @@
+ipc_pyx.cpp

artifacts/package_runtime/msgq/__init__.py

@@ -0,0 +1,69 @@
+import os
+
+INCLUDE_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
+LIB_PATH = os.path.join(os.path.dirname(__file__), "libmsgq.a")
+VISIONIPC_LIB_PATH = os.path.join(os.path.dirname(__file__), "visionipc", "libvisionipc.a")
+PYTHON_LIB_PATH = os.path.join(os.path.dirname(__file__), "ipc_pyx.so")
+
+from msgq.ipc_pyx import Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \
+                                set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event, \
+                                context_is_zmq
+from msgq.ipc_pyx import MultiplePublishersError, IpcError
+
+from typing import Optional, List, Union
+
+assert MultiplePublishersError
+assert IpcError
+assert toggle_fake_events
+assert set_fake_prefix
+assert get_fake_prefix
+assert delete_fake_prefix
+assert wait_for_one_event
+assert context_is_zmq
+
+NO_TRAVERSAL_LIMIT = 2**64-1
+
+context = Context()
+
+
+def fake_event_handle(endpoint: str, identifier: Optional[Union[str, bytes]] = None, override: bool = True, enable: bool = False) -> SocketEventHandle:
+  ident = identifier if identifier is not None else get_fake_prefix()
+  handle = SocketEventHandle(endpoint, ident, override)
+  if override:
+    handle.enabled = enable
+
+  return handle
+
+def pub_sock(endpoint: str, segment_size: int = 0) -> PubSocket:
+  sock = PubSocket()
+  sock.connect(context, endpoint, segment_size)
+  return sock
+
+
+def sub_sock(endpoint: str, poller: Optional[Poller] = None, addr: str = "127.0.0.1",
+             conflate: bool = False, timeout: Optional[int] = None, segment_size: int = 0) -> SubSocket:
+  sock = SubSocket()
+  sock.connect(context, endpoint, addr.encode('utf8'), conflate, segment_size)
+
+  if timeout is not None:
+    sock.setTimeout(timeout)
+
+  if poller is not None:
+    poller.registerSocket(sock)
+  return sock
+
+def drain_sock_raw(sock: SubSocket, wait_for_one: bool = False) -> List[bytes]:
+  """Receive all message currently available on the queue"""
+  ret: List[bytes] = []
+  while 1:
+    if wait_for_one and len(ret) == 0:
+      dat = sock.receive()
+    else:
+      dat = sock.receive(non_blocking=True)
+
+    if dat is None:
+      break
+
+    ret.append(dat)
+
+  return ret

artifacts/package_runtime/msgq/conftest.py

@@ -0,0 +1,14 @@
+import os
+import pytest
+import msgq
+
+@pytest.fixture(params=[False, True], ids=["msgq", "zmq"], autouse=True)
+def zmq_mode(request):
+  if request.param:
+    os.environ["ZMQ"] = "1"
+  else:
+    os.environ.pop("ZMQ", None)
+  msgq.context = msgq.Context()
+  assert msgq.context_is_zmq() == request.param
+  yield request.param
+  os.environ.pop("ZMQ", None)

artifacts/package_runtime/msgq/tests/test_fake.py

@@ -0,0 +1,193 @@
+import pytest
+import os
+import multiprocessing
+import platform
+import msgq
+from parameterized import parameterized_class
+from typing import Optional
+
+WAIT_TIMEOUT = 5
+
+
+@pytest.mark.skipif(condition=platform.system() == "Darwin", reason="Events not supported on macOS")
+class TestEvents:
+
+  def test_mutation(self):
+    handle = msgq.fake_event_handle("carState")
+    event = handle.recv_called_event
+
+    assert not event.peek()
+    event.set()
+    assert event.peek()
+    event.clear()
+    assert not event.peek()
+
+    del event
+
+  def test_wait(self):
+    handle = msgq.fake_event_handle("carState")
+    event = handle.recv_called_event
+
+    event.set()
+    try:
+      event.wait(WAIT_TIMEOUT)
+      assert event.peek()
+    except RuntimeError:
+      pytest.fail("event.wait() timed out")
+
+  def test_wait_multiprocess(self):
+    handle = msgq.fake_event_handle("carState")
+    event = handle.recv_called_event
+
+    def set_event_run():
+      event.set()
+
+    try:
+      p = multiprocessing.Process(target=set_event_run)
+      p.start()
+      event.wait(WAIT_TIMEOUT)
+      assert event.peek()
+    except RuntimeError:
+      pytest.fail("event.wait() timed out")
+
+    p.kill()
+
+  def test_wait_zero_timeout(self):
+    handle = msgq.fake_event_handle("carState")
+    event = handle.recv_called_event
+
+    try:
+      event.wait(0)
+      pytest.fail("event.wait() did not time out")
+    except RuntimeError:
+      assert not event.peek()
+
+
+@pytest.mark.skipif(condition=platform.system() == "Darwin", reason="FakeSockets not supported on macOS")
+@parameterized_class([{"prefix": None}, {"prefix": "test"}])
+class TestFakeSockets:
+  prefix: Optional[str] = None
+
+  @pytest.fixture(autouse=True)
+  def zmq_mode(self):
+    os.environ.pop("ZMQ", None)
+    msgq.context = msgq.Context()
+    yield
+
+  def setup_method(self):
+    msgq.toggle_fake_events(True)
+    if self.prefix is not None:
+      msgq.set_fake_prefix(self.prefix)
+    else:
+      msgq.delete_fake_prefix()
+
+  def teardown_method(self):
+    msgq.toggle_fake_events(False)
+    msgq.delete_fake_prefix()
+
+  def test_event_handle_init(self):
+    handle = msgq.fake_event_handle("controlsState", override=True)
+
+    assert not handle.enabled
+    assert handle.recv_called_event.fd >= 0
+    assert handle.recv_ready_event.fd >= 0
+
+  def test_non_managed_socket_state(self):
+    # non managed socket should have zero state
+    _ = msgq.pub_sock("ubloxGnss")
+
+    handle = msgq.fake_event_handle("ubloxGnss", override=False)
+
+    assert not handle.enabled
+    assert handle.recv_called_event.fd == 0
+    assert handle.recv_ready_event.fd == 0
+
+  def test_managed_socket_state(self):
+    # managed socket should not change anything about the state
+    handle = msgq.fake_event_handle("ubloxGnss")
+    handle.enabled = True
+
+    expected_enabled = handle.enabled
+    expected_recv_called_fd = handle.recv_called_event.fd
+    expected_recv_ready_fd = handle.recv_ready_event.fd
+
+    _ = msgq.pub_sock("ubloxGnss")
+
+    assert handle.enabled == expected_enabled
+    assert handle.recv_called_event.fd == expected_recv_called_fd
+    assert handle.recv_ready_event.fd == expected_recv_ready_fd
+
+  def test_sockets_enable_disable(self):
+    carState_handle = msgq.fake_event_handle("ubloxGnss", enable=True)
+    recv_called = carState_handle.recv_called_event
+    recv_ready = carState_handle.recv_ready_event
+
+    pub_sock = msgq.pub_sock("ubloxGnss")
+    sub_sock = msgq.sub_sock("ubloxGnss")
+
+    try:
+      carState_handle.enabled = True
+      recv_ready.set()
+      pub_sock.send(b"test")
+      _ = sub_sock.receive()
+      assert recv_called.peek()
+      recv_called.clear()
+
+      carState_handle.enabled = False
+      recv_ready.set()
+      pub_sock.send(b"test")
+      _ = sub_sock.receive()
+      assert not recv_called.peek()
+    except RuntimeError:
+      pytest.fail("event.wait() timed out")
+
+  def test_synced_pub_sub(self):
+    def daemon_repub_process_run():
+      pub_sock = msgq.pub_sock("ubloxGnss")
+      sub_sock = msgq.sub_sock("carState")
+
+      frame = -1
+      while True:
+        frame += 1
+        msg = sub_sock.receive(non_blocking=True)
+        if msg is None:
+          print("none received")
+          continue
+
+        bts = frame.to_bytes(8, 'little')
+        pub_sock.send(bts)
+
+    carState_handle = msgq.fake_event_handle("carState", enable=True)
+    recv_called = carState_handle.recv_called_event
+    recv_ready = carState_handle.recv_ready_event
+
+    p = multiprocessing.Process(target=daemon_repub_process_run)
+    p.start()
+
+    pub_sock = msgq.pub_sock("carState")
+    sub_sock = msgq.sub_sock("ubloxGnss")
+
+    try:
+      for i in range(10):
+        recv_called.wait(WAIT_TIMEOUT)
+        recv_called.clear()
+
+        if i == 0:
+          sub_sock.receive(non_blocking=True)
+
+        bts = i.to_bytes(8, 'little')
+        pub_sock.send(bts)
+
+        recv_ready.set()
+        recv_called.wait(WAIT_TIMEOUT)
+
+        msg = sub_sock.receive(non_blocking=True)
+        assert msg is not None
+        assert len(msg) == 8
+
+        frame = int.from_bytes(msg, 'little')
+        assert frame == i
+    except RuntimeError:
+      pytest.fail("event.wait() timed out")
+    finally:
+      p.kill()

artifacts/package_runtime/msgq/tests/test_messaging.py

@@ -0,0 +1,68 @@
+import os
+import random
+import time
+import string
+import msgq
+
+def random_sock():
+  return ''.join(random.choices(string.ascii_uppercase + string.digits, k=10))
+
+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)
+
+class TestPubSubSockets:
+
+  def setup_method(self):
+    # ZMQ pub socket takes too long to die
+    # sleep to prevent multiple publishers error between tests
+    zmq_sleep()
+
+  def test_pub_sub(self):
+    sock = random_sock()
+    pub_sock = msgq.pub_sock(sock)
+    sub_sock = msgq.sub_sock(sock, conflate=False, timeout=None)
+    zmq_sleep(3)
+
+    for _ in range(1000):
+      msg = random_bytes()
+      pub_sock.send(msg)
+      recvd = sub_sock.receive()
+      assert msg == recvd
+
+  def test_conflate(self):
+    sock = random_sock()
+    pub_sock = msgq.pub_sock(sock)
+    for conflate in [True, False]:
+      num_msgs = random.randint(3, 10)
+      sub_sock = msgq.sub_sock(sock, conflate=conflate, timeout=None)
+      zmq_sleep()
+
+      sent_msgs = []
+      for __ in range(num_msgs):
+        msg = random_bytes()
+        pub_sock.send(msg)
+        sent_msgs.append(msg)
+      time.sleep(0.1)
+      recvd_msgs = msgq.drain_sock_raw(sub_sock)
+      if conflate:
+        assert len(recvd_msgs) == 1
+        assert recvd_msgs[0] == sent_msgs[-1]
+      else:
+        assert len(recvd_msgs) == len(sent_msgs)
+        for rec_msg, sent_msg in zip(recvd_msgs, sent_msgs):
+          assert rec_msg == sent_msg
+
+  def test_receive_timeout(self):
+    sock = random_sock()
+    timeout = random.randrange(200)
+    sub_sock = msgq.sub_sock(sock, timeout=timeout)
+    zmq_sleep()
+
+    start_time = time.monotonic()
+    recvd = sub_sock.receive()
+    assert (time.monotonic() - start_time) < (timeout + 0.1)
+    assert recvd is None

artifacts/package_runtime/msgq/tests/test_poller.py

@@ -0,0 +1,138 @@
+import pytest
+import time
+import msgq
+import concurrent.futures
+
+SERVICE_NAME = 'myService'
+
+def poller():
+  context = msgq.Context()
+
+  p = msgq.Poller()
+
+  sub = msgq.SubSocket()
+  sub.connect(context, SERVICE_NAME)
+  p.registerSocket(sub)
+
+  socks = p.poll(10000)
+  r = [s.receive(non_blocking=True) for s in socks]
+
+  return r
+
+
+class TestPoller:
+  def test_poll_once(self):
+    context = msgq.Context()
+
+    pub = msgq.PubSocket()
+    pub.connect(context, SERVICE_NAME)
+
+    with concurrent.futures.ThreadPoolExecutor() as e:
+      poll = e.submit(poller)
+
+      time.sleep(0.1)  # Slow joiner syndrome
+
+      # Send message
+      pub.send(b"a")
+
+      # Wait for poll result
+      result = poll.result()
+
+    del pub
+    context.term()
+
+    assert result == [b"a"]
+
+  def test_poll_and_create_many_subscribers(self):
+    context = msgq.Context()
+
+    pub = msgq.PubSocket()
+    pub.connect(context, SERVICE_NAME)
+
+    with concurrent.futures.ThreadPoolExecutor() as e:
+      poll = e.submit(poller)
+
+      time.sleep(0.1)  # Slow joiner syndrome
+      c = msgq.Context()
+      for _ in range(10):
+        msgq.SubSocket().connect(c, SERVICE_NAME)
+
+      time.sleep(0.1)
+
+      # Send message
+      pub.send(b"a")
+
+      # Wait for poll result
+      result = poll.result()
+
+    del pub
+    context.term()
+
+    assert result == [b"a"]
+
+  def test_multiple_publishers_exception(self):
+    context = msgq.Context()
+
+    with pytest.raises(msgq.MultiplePublishersError):
+      pub1 = msgq.PubSocket()
+      pub1.connect(context, SERVICE_NAME)
+
+      pub2 = msgq.PubSocket()
+      pub2.connect(context, SERVICE_NAME)
+
+      pub1.send(b"a")
+
+    del pub1
+    del pub2
+    context.term()
+
+  def test_multiple_messages(self):
+    context = msgq.Context()
+
+    pub = msgq.PubSocket()
+    pub.connect(context, SERVICE_NAME)
+
+    sub = msgq.SubSocket()
+    sub.connect(context, SERVICE_NAME)
+
+    time.sleep(0.1)  # Slow joiner
+
+    for i in range(1, 100):
+      pub.send(b'a'*i)
+
+    msg_seen = False
+    i = 1
+    while True:
+      r = sub.receive(non_blocking=True)
+
+      if r is not None:
+        assert b'a'*i == r
+
+        msg_seen = True
+        i += 1
+
+      if r is None and msg_seen:  # ZMQ sometimes receives nothing on the first receive
+        break
+
+    del pub
+    del sub
+    context.term()
+
+  def test_conflate(self):
+    context = msgq.Context()
+
+    pub = msgq.PubSocket()
+    pub.connect(context, SERVICE_NAME)
+
+    sub = msgq.SubSocket()
+    sub.connect(context, SERVICE_NAME, conflate=True)
+
+    time.sleep(0.1)  # Slow joiner
+    pub.send(b'a')
+    pub.send(b'b')
+
+    assert b'b' == sub.receive()
+
+    del pub
+    del sub
+    context.term()

artifacts/package_runtime/msgq/visionipc/.gitignore

@@ -0,0 +1,2 @@
+visionipc_pyx.cpp
+*.so

artifacts/package_runtime/msgq/visionipc/__init__.py

@@ -0,0 +1,5 @@
+from msgq.visionipc.visionipc_pyx import VisionBuf, VisionIpcClient, VisionIpcServer, get_endpoint_name
+assert VisionBuf
+assert VisionIpcClient
+assert VisionIpcServer
+assert get_endpoint_name

artifacts/package_runtime/msgq/visionipc/tests/test_visionipc.py

@@ -0,0 +1,115 @@
+import os
+import time
+import random
+from enum import IntEnum
+from typing import Optional
+import numpy as np
+from msgq.visionipc import VisionIpcServer, VisionIpcClient
+
+
+class VisionStreamType(IntEnum):
+  VISION_STREAM_ROAD = 0
+  VISION_STREAM_DRIVER = 1
+  VISION_STREAM_WIDE_ROAD = 2
+  VISION_STREAM_MAP = 3
+
+def zmq_sleep(t=1):
+  if "ZMQ" in os.environ:
+    time.sleep(t)
+
+
+class TestVisionIpc:
+  server: VisionIpcServer
+  client: Optional[VisionIpcClient]
+
+  def setup_vipc(self, name, *stream_types, num_buffers=1, width=100, height=100, conflate=False):
+    self.server = VisionIpcServer(name)
+    for stream_type in stream_types:
+      self.server.create_buffers(stream_type, num_buffers, width, height)
+    self.server.start_listener()
+
+    if len(stream_types):
+      self.client = VisionIpcClient(name, stream_types[0], conflate)
+      assert self.client.connect(True)
+    else:
+      self.client = None
+
+    zmq_sleep()
+    return self.server, self.client
+
+  def test_connect(self):
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD)
+    assert self.client is not None
+    assert self.client.is_connected
+    del self.client
+    del self.server
+
+  def test_available_streams(self):
+    for k in range(4):
+      stream_types = set(random.choices([x.value for x in VisionStreamType], k=k))
+      self.setup_vipc("camerad", *stream_types)
+      available_streams = VisionIpcClient.available_streams("camerad", True)
+      assert available_streams == stream_types
+      del self.client
+      del self.server
+
+  def test_buffers(self):
+    width, height, num_buffers = 100, 200, 5
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD, num_buffers=num_buffers, width=width, height=height)
+    assert self.client is not None
+    assert self.client.width == width
+    assert self.client.height == height
+    assert self.client.buffer_len is not None and self.client.buffer_len > 0
+    assert self.client.num_buffers == num_buffers
+    del self.client
+    del self.server
+
+  def test_send_single_buffer(self):
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD)
+    assert self.client is not None
+    assert self.client.buffer_len is not None
+    buf = np.zeros(self.client.buffer_len, dtype=np.uint8)
+    buf.view('<i4')[0] = 1234
+    self.server.send(VisionStreamType.VISION_STREAM_ROAD, buf, frame_id=1337)
+
+    recv_buf = self.client.recv()
+    assert recv_buf is not None
+    assert recv_buf.data.view('<i4')[0] == 1234
+    assert self.client.frame_id == 1337
+    del self.client
+    del self.server
+
+  def test_no_conflate(self):
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD)
+    assert self.client is not None
+    assert self.client.buffer_len is not None
+    buf = np.zeros(self.client.buffer_len, dtype=np.uint8)
+    self.server.send(VisionStreamType.VISION_STREAM_ROAD, buf, frame_id=1)
+    self.server.send(VisionStreamType.VISION_STREAM_ROAD, buf, frame_id=2)
+
+    recv_buf = self.client.recv()
+    assert recv_buf is not None
+    assert self.client.frame_id == 1
+
+    recv_buf = self.client.recv()
+    assert recv_buf is not None
+    assert self.client.frame_id == 2
+    del self.client
+    del self.server
+
+  def test_conflate(self):
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD, conflate=True)
+    assert self.client is not None
+    assert self.client.buffer_len is not None
+    buf = np.zeros(self.client.buffer_len, dtype=np.uint8)
+    self.server.send(VisionStreamType.VISION_STREAM_ROAD, buf, frame_id=1)
+    self.server.send(VisionStreamType.VISION_STREAM_ROAD, buf, frame_id=2)
+
+    recv_buf = self.client.recv()
+    assert recv_buf is not None
+    assert self.client.frame_id == 2
+
+    recv_buf = self.client.recv()
+    assert recv_buf is None
+    del self.client
+    del self.server