#2 Complete the basic framework for the Python version.

Author: KingRichord

python/infinite_sense_core/__init__.py

@@ -0,0 +1,96 @@
+import tkinter as tk
+
+SENSOR_SPACING = 60  # 每个传感器之间的垂直间隔
+TICK_INTERVAL = 100  # 时间刻度线间隔（ms）
+
+# 示例传感器数据：{sensor_name: [timestamp1, timestamp2, ...]}
+sensor_data = {
+    "IMU": [100, 300, 500, 800],
+    "Cam": [150, 350, 700],
+    "Lid": [200, 600, 900]
+}
+
+
+class TimelineApp:
+    def __init__(self, root):
+        self.root = root
+        self.canvas = tk.Canvas(root, bg="white")
+        self.canvas.pack(fill=tk.BOTH, expand=True)
+
+        self.time_scale = 1.0  # px / ms，初始比例
+        self.time_offset = 0  # ms，时间轴左端对应时间（平移偏移）
+
+        self.dragging = False
+        self.last_x = 0
+
+        self.canvas.bind("<ButtonPress-1>", self.on_drag_start)
+        self.canvas.bind("<B1-Motion>", self.on_drag_motion)
+        self.canvas.bind("<MouseWheel>", self.on_mousewheel)  # Windows/Mac
+        self.canvas.bind("<Button-4>", self.on_mousewheel)  # Linux 鼠标滚轮向上
+        self.canvas.bind("<Button-5>", self.on_mousewheel)  # Linux 鼠标滚轮向下
+
+        # 绑定窗口尺寸变化事件
+        self.canvas.bind("<Configure>", lambda e: self.draw())
+
+        self.draw()
+
+    def draw(self):
+        self.canvas.delete("all")
+        width = self.canvas.winfo_width()
+        height = self.canvas.winfo_height()
+
+        sensors = list(sensor_data.keys())
+
+        # 画每个传感器的时间轴
+        for idx, sensor in enumerate(sensors):
+            y = 50 + idx * SENSOR_SPACING
+            self.canvas.create_text(30, y, text=sensor, anchor="e")
+            self.canvas.create_line(40, y, width - 20, y, fill="gray", dash=(2, 2))
+
+            for t in sensor_data[sensor]:
+                # 时间根据偏移和缩放计算X坐标
+                x = 40 + (t - self.time_offset) * self.time_scale
+                if 40 <= x <= width - 20:
+                    self.canvas.create_line(x, y - 10, x, y + 10, fill="blue")
+                    self.canvas.create_text(x, y + 15, text=str(t), anchor="n", font=("Arial", 8))
+
+        # 画时间刻度线
+        max_time_display = self.time_offset + (width - 60) / self.time_scale
+        t = (self.time_offset // TICK_INTERVAL) * TICK_INTERVAL
+        while t <= max_time_display:
+            x = 40 + (t - self.time_offset) * self.time_scale
+            self.canvas.create_line(x, 20, x, height - 20, fill="lightgray", dash=(1, 2))
+            self.canvas.create_text(x, 20, text=str(int(t)), anchor="s", font=("Arial", 8))
+            t += TICK_INTERVAL
+
+    def on_drag_start(self, event):
+        self.dragging = True
+        self.last_x = event.x
+
+    def on_drag_motion(self, event):
+        if self.dragging:
+            dx = event.x - self.last_x
+            self.last_x = event.x
+            self.time_offset -= dx / self.time_scale
+            if self.time_offset < 0:
+                self.time_offset = 0
+            self.draw()
+
+    def on_mousewheel(self, event):
+        if event.num == 4 or event.delta > 0:
+            factor = 1.1
+        else:
+            factor = 0.9
+
+        width = self.canvas.winfo_width()
+        mouse_time = self.time_offset + (event.x - 40) / self.time_scale
+
+        new_scale = self.time_scale * factor
+        new_scale = max(0.1, min(new_scale, 10))
+
+        self.time_offset = mouse_time - (event.x - 40) / new_scale
+        if self.time_offset < 0:
+            self.time_offset = 0
+
+        self.time_scale = new_scale
+        self.draw()

python/infinite_sense_core/app.py

@@ -0,0 +1,42 @@
+import json
+from typing import Dict
+
+from .message import Messenger
+
+def process_trigger_data(data: Dict):
+    if data.get("f") != "t":
+        return
+    time_stamp = data["t"]
+    status = data["s"]
+    # print(f"[Trigger] time={time_stamp}, status={status}")
+
+
+def process_imu_data(data: Dict):
+    if data.get("f") != "imu":
+        return
+    imu = {
+        "time_stamp_us": data["t"],
+        "a": data["d"][:3],
+        "g": data["d"][3:6],
+        "temperature": data["d"][6],
+        "q": data["q"][:4]
+    }
+    Messenger.get_instance().pub("imu_1", json.dumps(imu))
+
+
+def process_gps_data(data: Dict):
+    if data.get("f") != "GNGGA":
+        return
+    gps = {
+        "data": data["d"],
+        "trigger_time_us": data["pps"],
+        "time_stamp_us": data["t"]
+    }
+    Messenger.pub("gps", json.dumps(gps))
+
+def process_log_data(data: Dict):
+    if data.get("f") != "log":
+        return
+    level = data.get("l", "INFO")
+    msg = data.get("msg", "")
+    print(f"[LOG-{level}] {msg}")

python/infinite_sense_core/data.py

@@ -0,0 +1,68 @@
+import threading
+import time
+import logging
+
+class Synchronizer:
+    def __init__(self):
+        self.net_manager = None
+        self.serial_manager = None
+        self.sensor_manager = None
+        self.net_ip = None
+        self.net_port = None
+        self.serial_dev = None
+        self.serial_baud_rate = None
+        self._lock = threading.Lock()
+
+        logging.info("\n"
+                     "  ▗▄▄▄▖▗▖  ▗▖▗▄▄▄▖▗▄▄▄▖▗▖  ▗▖▗▄▄▄▖▗▄▄▄▖▗▄▄▄▖ ▗▄▄▖▗▄▄▄▖▗▖  ▗▖ ▗▄▄▖▗▄▄▄▖\n"
+                     "    █  ▐▛▚▖▐▌▐▌     █  ▐▛▚▖▐▌  █    █  ▐▌   ▐▌   ▐▌   ▐▛▚▖▐▌▐▌   ▐▌   \n"
+                     "    █  ▐▌ ▝▜▌▐▛▀▀▘  █  ▐▌ ▝▜▌  █    █  ▐▛▀▀▘ ▝▀▚▖▐▛▀▀▘▐▌ ▝▜▌ ▝▀▚▖▐▛▀▀▘\n"
+                     "  ▗▄█▄▖▐▌  ▐▌▐▌   ▗▄█▄▖▐▌  ▐▌▗▄█▄▖  █  ▐▙▄▄▖▗▄▄▞▘▐▙▄▄▖▐▌  ▐▌▗▄▄▞▘▐▙▄▄▖")
+
+    def set_log_path(self, path):
+        logging.basicConfig(filename=path, level=logging.INFO,
+                            format='[%(asctime)s] %(levelname)s: %(message)s')
+
+    def set_net_link(self, net_ip, net_port):
+        self.net_ip = net_ip
+        self.net_port = net_port
+        from infinite_sense_core.net import NetManager
+        self.net_manager = NetManager(net_ip, net_port)
+
+    def set_usb_link(self, serial_dev, serial_baud_rate):
+        self.serial_dev = serial_dev
+        self.serial_baud_rate = serial_baud_rate
+        from usb import UsbManager
+        self.serial_manager = UsbManager(serial_dev, serial_baud_rate)
+        self.net_manager = None
+
+    def use_sensor(self, sensor):
+        self.sensor_manager = sensor
+
+    def start(self):
+        with self._lock:
+            try:
+                if self.net_manager:
+                    self.net_manager.start()
+                if self.serial_manager:
+                    self.serial_manager.start()
+                if self.sensor_manager:
+                    time.sleep(2)
+                    self.sensor_manager.initialization()
+                    self.sensor_manager.start()
+                logging.info("Synchronizer Started.")
+            except Exception as e:
+                logging.error(f"Synchronizer start exception: {e}")
+
+    def stop(self):
+        with self._lock:
+            try:
+                if self.net_manager:
+                    self.net_manager.stop()
+                if self.serial_manager:
+                    self.serial_manager.stop()
+                if self.sensor_manager:
+                    self.sensor_manager.stop()
+                logging.info("Synchronizer Stopped.")
+            except Exception as e:
+                logging.error(f"Synchronizer stop exception: {e}")

python/infinite_sense_core/infinit_sense.py

@@ -0,0 +1,91 @@
+import zmq
+import threading
+
+class Messenger:
+    _instance = None
+    def __init__(self, endpoint="tcp://127.0.0.1:4565"):
+        self.endpoint = endpoint
+        self.context = zmq.Context()
+        self.publisher = self.context.socket(zmq.PUB)
+        self.sub_threads = []
+        self.lock = threading.Lock()
+
+        try:
+            self.publisher.bind(self.endpoint)
+            print(f"[INFO] Link Net: {self.endpoint}")
+        except zmq.ZMQError as e:
+            print(f"[ERROR] Net initialization error: {e}")
+            self.cleanup()
+        Messenger._instance = self
+
+    @classmethod
+    def get_instance(cls):
+        if cls._instance is None:
+            cls._instance = Messenger()
+        return cls._instance
+
+    def cleanup(self):
+        try:
+            self.publisher.close()
+            self.context.term()
+            for thread in self.sub_threads:
+                if thread.is_alive():
+                    thread.join()
+            print("[INFO] Messenger clean up successful")
+        except Exception:
+            print("[ERROR] Messenger clean up error")
+
+    def pub(self, topic: str, metadata: str):
+        try:
+            with self.lock:
+                self.publisher.send_string(topic, zmq.SNDMORE)
+                self.publisher.send_string(metadata, zmq.DONTWAIT)
+        except zmq.ZMQError as e:
+            print(f"[ERROR] Publish error: {e}")
+
+    def pub_struct(self, topic: str, data: bytes):
+        try:
+            with self.lock:
+                self.publisher.send_string(+topic, zmq.SNDMORE)
+                self.publisher.send(data, zmq.DONTWAIT)
+        except zmq.ZMQError as e:
+            print(f"[ERROR] Publish struct error: {e}")
+
+    def sub(self, topic: str, callback):
+        def run():
+            try:
+                subscriber = self.context.socket(zmq.SUB)
+                subscriber.connect(self.endpoint)
+                subscriber.setsockopt_string(zmq.SUBSCRIBE, topic)
+                while True:
+                    topic_msg = subscriber.recv_string()
+                    data_msg = subscriber.recv_string()
+                    if topic_msg != topic:
+                        continue
+                    callback(data_msg)
+            except zmq.ZMQError as e:
+                print(f"[ERROR] Exception in Sub thread for topic [{topic}]: {e}")
+
+        thread = threading.Thread(target=run, daemon=True)
+        thread.start()
+        self.sub_threads.append(thread)
+
+    def sub_struct(self, topic: str, callback):
+        def run():
+            try:
+                context = zmq.Context()
+                subscriber = context.socket(zmq.SUB)
+                subscriber.connect(self.endpoint)
+                subscriber.setsockopt_string(zmq.SUBSCRIBE, topic)
+                while True:
+                    topic_msg = subscriber.recv_string()
+                    data_msg = subscriber.recv()
+                    if topic_msg != topic:
+                        continue
+                    callback(data_msg)
+            except zmq.ZMQError as e:
+                print(f"[ERROR] Exception in SubStruct for topic [{topic}]: {e}")
+
+        thread = threading.Thread(target=run, daemon=True)
+        thread.start()
+        self.sub_threads.append(thread)

python/infinite_sense_core/message.py

@@ -0,0 +1,84 @@
+import json
+import socket
+import struct
+import threading
+import time
+
+from infinite_sense_core.data import process_trigger_data, process_imu_data, process_gps_data, process_log_data
+from .times import precise_sleep
+from .ptp import Ptp
+
+class NetManager:
+    def __init__(self, target_ip: str, port: int):
+        self.target_ip = target_ip
+        self.port = port
+        self.started = False
+
+        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self.sock.setblocking(False)
+
+        curr_time = int(time.time() * 1_000_000)
+        self.sock.sendto(struct.pack("<Q", curr_time), (self.target_ip, self.port))
+
+        self.ptp = Ptp()
+        self.ptp.set_net_ptr(self.sock, self.target_ip, self.port)
+
+        self.rx_thread = None
+        self.tx_thread = None
+
+    def start(self):
+        if self.started:
+            return
+        self.started = True
+        self.rx_thread = threading.Thread(target=self.receive, daemon=True)
+        self.tx_thread = threading.Thread(target=self.timestamp_synchronization, daemon=True)
+        self.rx_thread.start()
+        self.tx_thread.start()
+        print("[NetManager] Started")
+
+    def stop(self):
+        if not self.started:
+            return
+        self.started = False
+        print("[NetManager] Stopped")
+
+    def receive(self):
+        while self.started:
+            try:
+                data, addr = self.sock.recvfrom(65536)
+            except BlockingIOError:
+                precise_sleep(0.001)
+                continue
+
+            try:
+                recv_data = data.decode("utf-8")
+                if not recv_data:
+                    continue
+                json_data = json.loads(recv_data)
+            except Exception as e:
+                print(f"[WARN] JSON parse error: {e}")
+                continue
+            # print(json_data)
+            self.ptp.receive_ptp_data(json_data)
+            process_trigger_data(json_data)
+            process_imu_data(json_data)
+            process_gps_data(json_data)
+            process_log_data(json_data)
+
+    def timestamp_synchronization(self):
+        while self.started:
+            try:
+                self.ptp.send_ptp_data()
+                precise_sleep(0.01)
+            except Exception as e:
+                print(f"[ERROR] Timestamp sync error: {e}")
+
+
+if __name__ == "__main__":
+    nm = NetManager("192.168.1.188", 8888)
+    nm.start()
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        nm.stop()