Modified callback type.

.gitignore

@@ -1,3 +1,43 @@
-*.so
-*.dll
+# Prerequisites
+*.d
+
+# Object files
 *.o
+*.ko
+*.obj
+*.elf
+
+# Linker output
+*.ilk
+*.map
+*.exp
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Libraries
+*.lib
+*.a
+*.la
+*.lo
+
+# Shared objects (inc. Windows DLLs)
+*.dll
+*.so
+*.so.*
+*.dylib
+
+# Executables
+*.exe
+*.out
+*.app
+*.i*86
+*.x86_64
+*.hex
+
+# Debug files
+*.dSYM/
+*.su
+*.idb
+*.pdb

.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "rb_lidar.h": "c"
+    }
+}

data_logger.py

@@ -0,0 +1,33 @@
+import numpy as np
+import logging
+from rblidar import RBLidar  # Assuming rblidar.py is in the same directory
+
+class DataLogger:
+    def __init__(self):
+        self.data_frames = []  # List to store the frames of data
+        logging.basicConfig(level=logging.INFO, format='%(asctime)s %(levelname)s: %(message)s')
+
+    def frame_callback(self, point_data_array):
+        # Append the received frame (NumPy array) to the list
+        self.data_frames.append(point_data_array)
+        logging.info(f"Logged frame with {len(point_data_array)} points.")
+
+    def save_data(self, filename='logged_data.npy'):
+        # Concatenate all frames into a single NumPy array and save it
+        if self.data_frames:
+            all_data = np.concatenate(self.data_frames)
+            np.save(filename, all_data)
+            logging.info(f"Data saved to {filename}.")
+        else:
+            logging.info("No data to save.")
+
+if __name__ == "__main__":
+    data_logger = DataLogger()
+    lidar = RBLidar("192.168.8.1", 2368, frame_callback=data_logger.frame_callback)
+
+    try:
+        while True:
+            pass  # Keep the program running to receive data
+    except KeyboardInterrupt:
+        logging.info("Stopping and saving data...")
+        data_logger.save_data()  # Save data on exit

main.py

@@ -1,108 +0,0 @@
-import socket
-import threading
-import ctypes
-import queue
-import logging
-
-# 配置日志
-logging.basicConfig(level=logging.DEBUG, format='%(asctime)s - %(levelname)s - %(message)s')
-
-# 定义深度数据结构
-class Depth_t(ctypes.Structure):
-    _fields_ = [
-        ("distance0", ctypes.c_uint16),  # 深度数据
-        ("rssi0", ctypes.c_uint8),       # 回波强度
-        ("distance1", ctypes.c_uint16),   # 深度数据
-        ("rssi1", ctypes.c_uint8)         # 回波强度
-    ]
-
-# 定义小包结构体
-class LittlePacket(ctypes.Structure):
-    _fields_ = [
-        ("azimuth", ctypes.c_uint16),    # 测距数据的角度值
-        ("depth", Depth_t * 16),          # 深度数据点位包
-        ("timestamp", ctypes.c_uint32),   # 数据头
-    ]
-
-# 创建两个共享的队列作为双缓冲区
-buffer1 = queue.Queue(maxsize=1440)
-buffer2 = queue.Queue(maxsize=1440)
-
-# 创建一个事件对象
-zeroFoundEvent = threading.Event()
-currentBuffer = 1  # 指示当前使用的缓冲区
-
-def parse_msop_packet(data):
-    global currentBuffer
-
-    # 提取数据块
-    for i in range(12):  # 假设有12个数据块
-        start = i * 100  # 每个数据块100字节
-        end = start + 100
-        data_block = data[start:end]
-
-        # 检查标志位
-        if data_block[0:2] == b'\xff\xee':
-            little_packet = LittlePacket()
-            little_packet.azimuth = int.from_bytes(data_block[2:4], byteorder='little')
-            little_packet.timestamp = int.from_bytes(data_block[1201:1205], byteorder='little')
-           
-            # 提取16个测距数据
-            for j in range(16):
-                depth_data = Depth_t()
-                depth_data.distance0 = int.from_bytes(data_block[4 + j * 6: 4 + j * 6 + 2], byteorder='little')
-                depth_data.rssi0 = int.from_bytes(data_block[4 + j * 6 + 2: 4 + j * 6 + 3], byteorder='little')
-                depth_data.distance1 = int.from_bytes(data_block[4 + j * 6 + 3: 4 + j * 6 + 5], byteorder='little')
-                depth_data.rssi1 = int.from_bytes(data_block[4 + j * 6 + 5: 4 + j * 6 + 6], byteorder='little')
-                little_packet.depth[j] = depth_data
-
-            # 如果发现 0 角度数据，设置事件并切换缓冲区
-            if little_packet.azimuth == 0:
-                currentBuffer = 2 if currentBuffer == 1 else 1  # 切换到另一个缓冲区
-                logging.debug(f"切换到缓冲区 {currentBuffer}, 缓冲区1大小 {buffer1.qsize()}, 缓冲区2大小 {buffer2.qsize()}")
-                zeroFoundEvent.set()
-
-            # 将当前包加入到当前缓冲区
-            if currentBuffer == 1:
-                buffer1.put(little_packet)
-            else:
-                buffer2.put(little_packet)
-
-def read_buffer():
-    while True:
-        if zeroFoundEvent.is_set():  # 如果发现 0 角度数据
-            # 处理当前缓冲区的数据
-            if currentBuffer == 2:
-                while not buffer1.empty():
-                    little_packet = buffer1.get()
-                    logging.debug(f"Consumer from buffer1: azimuth: {little_packet.azimuth}")
-            else:
-                while not buffer2.empty():
-                    little_packet = buffer2.get()
-                    logging.debug(f"Consumer from buffer2: azimuth: {little_packet.azimuth}")
-
-            zeroFoundEvent.clear()  # 清除事件
-
-def receive_udp_data():
-    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-    sock.bind(('192.168.8.1', 2368))
-
-    while True:
-        try:
-            data, addr = sock.recvfrom(1206)  # 读取UDP数据包
-            parse_msop_packet(data)
-        except Exception as e:
-            logging.error(f"Error receiving data: {e}")
-
-receive_thread = threading.Thread(target=receive_udp_data)
-read_thread = threading.Thread(target=read_buffer)
-
-receive_thread.start()
-read_thread.start()
-
-# 主线程可以继续执行其他任务
-try:
-    while True:
-        pass
-except KeyboardInterrupt:
-    print("Exiting...")

rblidar.py

@@ -1,36 +1,10 @@
-# rblidar.py
 import ctypes
 import logging
 import numpy as np
 import matplotlib.pyplot as plt
 
-
-def plot_point_cloud(point_data_array, filename='point_cloud.png'):
-    # 提取点数据的坐标
-    x = point_data_array['dist'] * np.cos(np.deg2rad(point_data_array['azimuth']/100))/1000
-    y = point_data_array['dist'] * np.sin(np.deg2rad(point_data_array['azimuth']/100))/1000
-
-    # logging.info(point_data_array['dist'][1000:1200])
-    logging.info(y[1000:1200])
-
-    # 绘制点云
-    fig, ax = plt.subplots(figsize=(8, 8))
-    scatter = ax.scatter(x, y, s=2, c=point_data_array['rssi'], cmap='viridis')
-    ax.set_xlim([-2, 2])
-    ax.set_ylim([-2, 2])
-    ax.set_aspect('equal')
-    ax.set_title('Point Cloud')
-    ax.set_xlabel('X (m)')
-    ax.set_ylabel('Y (m)')
-    cbar = fig.colorbar(scatter, ax=ax)
-    cbar.set_label('RSSI')
-
-    # 保存为 PNG 文件
-    plt.savefig(filename, dpi=300)  # dpi 可以根据需要调整
-    plt.close(fig)  # 关闭图形，释放内存
-
 class point_data_t(ctypes.Structure):
-    _pack_ = 1  # 设置对齐方式为 1 字节
+    _pack_ = 1  # Set alignment to 1 byte
     _fields_ = [
         ("azimuth", ctypes.c_uint16),
         ("dist", ctypes.c_uint16),
@@ -38,60 +12,48 @@ class point_data_t(ctypes.Structure):
         ("timestamp", ctypes.c_uint32)
     ]
 
-
-# 创建与结构体相对应的 numpy dtype
-point_data_dtype = np.dtype([
+# Create a corresponding numpy dtype for the structure
+point_dtype = np.dtype([
     ('azimuth', np.uint16),
     ('dist', np.uint16),
     ('rssi', np.uint16),
     ('timestamp', np.uint32)
 ])
 
-
 rblidar_lib = ctypes.CDLL('./rb_lidar.so')  
 
-
 CALLBACK_TYPE = ctypes.CFUNCTYPE(None, ctypes.c_void_p, ctypes.c_int)
 
-def my_callback(data, length):
-    logging.info("Received length: %d", length)
-    byte_data = ctypes.string_at(data, length)
-
-
-    point_data_array = np.frombuffer(byte_data, dtype=point_data_dtype)
-
-    for point in point_data_array:
-        logging.info(f"Azimuth: {point['azimuth']}, Distance: {point['dist']}, RSSI: {point['rssi']}, Timestamp: {point['timestamp']}")
-
-    # 绘制点云
-    # plot_point_cloud(point_data_array)
-
-
 class RBLidar:
     def __init__(self, ip: str, port: int, frame_callback=None):
         logging.basicConfig(level=logging.INFO, format='%(asctime)s %(levelname)s: %(message)s')
 
-        # self.callback = CALLBACK_TYPE(my_callback)
-
         self.frame_callback = frame_callback
         self.callback = CALLBACK_TYPE(self._callback_wrapper)
-  
+
+        # Create the lidar instance
         self.lidar = rblidar_lib.rblidar_create(ip.encode('utf-8'), port, self.callback)
 
-    def _callback_wrapper(self,data,length):
-        my_callback(data, length)
+    def _callback_wrapper(self, data, length):
+        # Convert the received data into a byte buffer
+        byte_data = ctypes.string_at(data, length)
+
+        # Create a NumPy array from the byte data
+        point_data_array = np.frombuffer(byte_data, dtype=point_dtype)
 
-        if self.frame_callback:
-            self.frame_callback(data,length)
+        # If a NumPy callback is provided, call it
+        if self.frame_callback is not None:
+            self.frame_callback(point_data_array)
 
     def __del__(self):
         rblidar_lib.rblidar_destroy(self.lidar)
 
-
 if __name__ == "__main__":
-    def my_frame_callback(data, length):
-        logging.info("Frame callback processing data")
-    lidar = RBLidar("192.168.8.1", 2368,frame_callback=my_frame_callback) 
+    def my_frame_callback(point_data_array):
+        for point in point_data_array:
+            logging.info(f"Azimuth: {point['azimuth']}, Distance: {point['dist']}, RSSI: {point['rssi']}, Timestamp: {point['timestamp']}")
+
+    lidar = RBLidar("192.168.8.1", 2368, frame_callback=my_frame_callback) 
     try:
         while True:
             pass