TractorVision初期移植

lib/camera/ArenaCamera.py

@@ -0,0 +1,294 @@
+import ctypes
+import time
+import numpy as np
+import cv2
+
+# 定义错误代码
+ARENA_ACQ_SUCCESS = 0
+
+# 打印错误信息
+def print_error(operation, error):
+    print(f"{operation} failed with error code: {error}")
+
+# 重连机制函数 
+def retry_operation(operation_func, handle, *args, max_retries=1, delay=1):
+    """
+    重试机制函数
+    :param operation_func: 要执行的操作函数
+    :param handle: 相机句柄
+    :param args: 操作函数的参数
+    :param max_retries: 最大重试次数
+    :param delay: 重试间隔时间（秒）
+    :return: 操作结果
+    """
+    for attempt in range(max_retries + 1):
+        err = operation_func(handle, *args)
+        if err == ARENA_ACQ_SUCCESS:
+            return True
+        else:
+            print(f"Attempt {attempt + 1}/{max_retries} failed. Retrying in {delay} seconds...")
+            time.sleep(delay)
+    print_error(operation_func.__name__, err)
+    return False
+
+class ArenaCamera:
+    # 静态变量存储共享库
+    libArenaAcq = ctypes.CDLL('libArenaAcq.so')
+    sys_handle = ctypes.c_void_p()
+    functions_initialized = False  # Class-level flag
+
+    def __init__(self, sn):
+        self.sn = sn
+        self.handle = ctypes.c_void_p()
+        self.count = 0
+        
+        # Initialize function argument types and return types if not already done
+        if not ArenaCamera.functions_initialized:
+            self._initialize_functions()
+            ArenaCamera.functions_initialized = True
+
+    @classmethod
+    def _initialize_functions(cls):
+        # 定义 ArenaAcq_Initialize
+        cls.libArenaAcq.ArenaAcq_Initialize.argtypes = [ctypes.POINTER(ctypes.c_void_p)]
+        cls.libArenaAcq.ArenaAcq_Initialize.restype = ctypes.c_int
+
+        # 定义 ArenaAcq_Connect
+        cls.libArenaAcq.ArenaAcq_Connect.argtypes = [ctypes.c_void_p, ctypes.POINTER(ctypes.c_void_p),ctypes.c_char_p]
+        cls.libArenaAcq.ArenaAcq_Connect.restype = ctypes.c_int
+
+        # 定义 ArenaAcq_SetProperty
+        cls.libArenaAcq.ArenaAcq_SetProperty.argtypes = [
+            ctypes.c_void_p, ctypes.c_char_p, ctypes.c_char_p
+        ]
+        cls.libArenaAcq.ArenaAcq_SetProperty.restype = ctypes.c_int
+
+        # 定义 ArenaAcq_GetProperty
+        cls.libArenaAcq.ArenaAcq_GetProperty.argtypes = [
+            ctypes.c_void_p, ctypes.c_char_p, ctypes.c_char_p, ctypes.POINTER(ctypes.c_size_t)
+        ]
+        cls.libArenaAcq.ArenaAcq_GetProperty.restype = ctypes.c_int
+
+        # 定义 ArenaAcq_StartAcquisitionStream
+        cls.libArenaAcq.ArenaAcq_StartAcquisitionStream.argtypes = [ctypes.c_void_p]
+        cls.libArenaAcq.ArenaAcq_StartAcquisitionStream.restype = ctypes.c_int
+
+        # 定义 ArenaAcq_GetDepthImage
+        cls.libArenaAcq.ArenaAcq_GetDepthImage.argtypes = [
+            ctypes.c_void_p,
+            ctypes.POINTER(ctypes.POINTER(ctypes.c_uint16)),
+            ctypes.POINTER(ctypes.POINTER(ctypes.c_uint16)),
+            ctypes.POINTER(ctypes.POINTER(ctypes.c_uint16)),
+            ctypes.POINTER(ctypes.c_size_t),
+            ctypes.POINTER(ctypes.c_size_t),
+            ctypes.POINTER(ctypes.c_size_t),
+            ctypes.POINTER(ctypes.c_size_t)
+        ]
+        cls.libArenaAcq.ArenaAcq_GetDepthImage.restype = ctypes.c_int
+
+        # 定义 ArenaAcq_GetImage
+        cls.libArenaAcq.ArenaAcq_GetImage.argtypes = [
+            ctypes.c_void_p,
+            ctypes.POINTER(ctypes.POINTER(ctypes.c_uint16)),
+            ctypes.POINTER(ctypes.c_size_t),
+            ctypes.POINTER(ctypes.c_size_t),
+            ctypes.POINTER(ctypes.c_size_t),
+            ctypes.POINTER(ctypes.c_size_t)
+        ]
+        cls.libArenaAcq.ArenaAcq_GetImage.restype = ctypes.c_int
+
+        # 定义 ArenaAcq_ReleaseImage
+        cls.libArenaAcq.ArenaAcq_ReleaseImage.argtypes = [ctypes.POINTER(ctypes.c_uint8)]
+        cls.libArenaAcq.ArenaAcq_ReleaseImage.restype = ctypes.c_int
+
+        cls.libArenaAcq.ArenaAcq_ReleaseHandle.argtypes = [ctypes.POINTER(ctypes.c_void_p)]
+        cls.libArenaAcq.ArenaAcq_ReleaseHandle.restype = ctypes.c_int
+
+        cls.libArenaAcq.ArenaAcq_ReleaseDevice.argtypes = [ctypes.POINTER(ctypes.c_void_p)]
+        cls.libArenaAcq.ArenaAcq_ReleaseDevice.restype = ctypes.c_int
+
+        # 定义 ArenaAcq_StopAcquisitionStream
+        cls.libArenaAcq.ArenaAcq_StopAcquisitionStream.argtypes = [ctypes.c_void_p]
+        cls.libArenaAcq.ArenaAcq_StopAcquisitionStream.restype = ctypes.c_int
+
+        # 定义 ArenaAcq_Disconnect
+        cls.libArenaAcq.ArenaAcq_Disconnect.argtypes = [ctypes.c_void_p,ctypes.c_void_p]
+        cls.libArenaAcq.ArenaAcq_Disconnect.restype = ctypes.c_int
+
+        # 定义 ArenaAcq_Cleanup
+        cls.libArenaAcq.ArenaAcq_Cleanup.argtypes = [ctypes.c_void_p]
+        cls.libArenaAcq.ArenaAcq_Cleanup.restype = ctypes.c_int
+
+        cls.libArenaAcq.ArenaAcq_SaveDepthImageAsPGM.argtypes = [ctypes.POINTER(ctypes.c_uint16),ctypes.c_size_t,ctypes.c_size_t,ctypes.c_char_p]
+        cls.libArenaAcq.ArenaAcq_SaveDepthImageAsPGM.restype = ctypes.c_int
+
+
+        # 初始化库
+        if not retry_operation(cls.libArenaAcq.ArenaAcq_Initialize, ctypes.byref(cls.sys_handle)):
+            print(f"Initialization failed")
+            cls.libArenaAcq.ArenaAcq_Cleanup(cls.sys_handle)
+            cls.libArenaAcq.ArenaAcq_ReleaseHandle(cls.sys_handle)
+            return
+        print(f"Library initialized successfully")
+
+    @classmethod
+    def shutdown(cls):
+        cls.libArenaAcq.ArenaAcq_Cleanup(cls.sys_handle)
+        print(f"Library shutdown successfully")
+        
+        
+    def create(self):
+
+        # 连接到相机
+        if not retry_operation(self.libArenaAcq.ArenaAcq_Connect,self.sys_handle, self.handle, self.sn.encode('utf-8')):
+            print(f"Connection failed for camera {self.sn}")
+            self.libArenaAcq.ArenaAcq_Cleanup(self.handle)
+            return
+        print(f"Connected to camera {self.sn} successfully")
+
+        # 开始采集流
+        if not retry_operation(self.libArenaAcq.ArenaAcq_StartAcquisitionStream, self.handle):
+            print(f"Start acquisition failed for camera {self.sn}")
+            self.destroy()
+            return
+        print(f"Acquisition started successfully for camera {self.sn}") 
+            
+    def destroy(self):
+        # 停止采集流
+        if not retry_operation(self.libArenaAcq.ArenaAcq_StopAcquisitionStream, self.handle):
+            print(f"Stop acquisition failed for camera {self.sn}")
+            self.libArenaAcq.ArenaAcq_Disconnect(self.sys_handle,self.handle)
+            self.libArenaAcq.ArenaAcq_Cleanup(self.handle)
+            return
+        print(f"Acquisition stopped successfully for camera {self.sn}")
+
+        # 断开与相机的连接
+        if not retry_operation(self.libArenaAcq.ArenaAcq_Disconnect,self.sys_handle,self.handle):
+            print(f"Disconnection failed for camera {self.sn}")
+            self.libArenaAcq.ArenaAcq_Cleanup(self.handle)
+            return
+        print(f"Disconnected from camera {self.sn} successfully")
+
+        # 清理资源
+        if not retry_operation(self.libArenaAcq.ArenaAcq_Cleanup, self.handle):
+            print(f"Cleanup failed for camera {self.sn}")
+            return
+
+    def read(self):
+        image_data = ctypes.POINTER(ctypes.c_uint16)()
+        
+        image_size = ctypes.c_size_t()
+        width = ctypes.c_size_t()
+        height = ctypes.c_size_t()
+        bits_per_pixel = ctypes.c_size_t()
+
+        if not retry_operation(self.libArenaAcq.ArenaAcq_GetImage, self.handle, ctypes.byref(image_data), 
+                               ctypes.byref(image_size), ctypes.byref(width), ctypes.byref(height), 
+                               ctypes.byref(bits_per_pixel)):
+            print(f"Image acquisition failed for camera {self.sn}")
+            return None
+
+        if image_size.value<=0:
+            print(f"Image size is 0 for camera {self.sn}")
+            return None
+
+        # 将图像数据转换为OpenCV格式
+        image_array = ctypes.cast(image_data, ctypes.POINTER(ctypes.c_uint16 * (width.value * height.value))).contents
+
+        
+        image_np = np.array(image_array).reshape(height.value, width.value)
+
+        image_np = np.copy(image_np)
+        
+        self.count=self.count+1
+
+        # 释放图像资源
+        self.libArenaAcq.ArenaAcq_ReleaseImage(ctypes.cast(image_data, ctypes.POINTER(ctypes.c_uint8)))
+
+        return image_np
+
+    def capture(self):
+        image_data = ctypes.POINTER(ctypes.c_uint16)()
+        x_image_data = ctypes.POINTER(ctypes.c_uint16)()
+        y_image_data = ctypes.POINTER(ctypes.c_uint16)()
+        
+        image_size = ctypes.c_size_t()
+        width = ctypes.c_size_t()
+        height = ctypes.c_size_t()
+        bits_per_pixel = ctypes.c_size_t()
+
+        if not retry_operation(self.libArenaAcq.ArenaAcq_GetDepthImage, self.handle, ctypes.byref(image_data), 
+                               ctypes.byref(x_image_data), ctypes.byref(y_image_data),
+                               ctypes.byref(image_size), ctypes.byref(width), ctypes.byref(height), 
+                               ctypes.byref(bits_per_pixel)):
+            print(f"Image acquisition failed for camera {self.sn}")
+            return None, None, None
+
+        if image_size.value<=0:
+            return None,None,None
+
+        # 将图像数据转换为OpenCV格式
+        depth_image_array = ctypes.cast(image_data, ctypes.POINTER(ctypes.c_uint16 * (width.value * height.value))).contents
+        x_image_array = ctypes.cast(x_image_data, ctypes.POINTER(ctypes.c_uint16 * (width.value * height.value))).contents  
+        y_image_array = ctypes.cast(y_image_data, ctypes.POINTER(ctypes.c_uint16 * (width.value * height.value))).contents
+
+        
+        depth_image_np = np.array(depth_image_array).reshape(height.value, width.value)
+        x_image_np = np.array(x_image_array).reshape(height.value, width.value)
+        y_image_np = np.array(y_image_array).reshape(height.value, width.value)
+
+        depth_image_np = np.copy(depth_image_np)
+        x_image_np = np.copy(x_image_np)
+        y_image_np = np.copy(y_image_np)
+        # print("copied.")
+        
+        # self.libArenaAcq.ArenaAcq_SaveDepthImageAsPGM(image_data,width,height,f'dbg_{self.sn}_{self.count}'.encode('utf-8'))
+        self.count=self.count+1
+
+        # 释放图像资源
+        self.libArenaAcq.ArenaAcq_ReleaseImage(ctypes.cast(image_data, ctypes.POINTER(ctypes.c_uint8)))
+        self.libArenaAcq.ArenaAcq_ReleaseImage(ctypes.cast(x_image_data, ctypes.POINTER(ctypes.c_uint8)))
+        self.libArenaAcq.ArenaAcq_ReleaseImage(ctypes.cast(y_image_data, ctypes.POINTER(ctypes.c_uint8)))
+
+        return x_image_np, y_image_np, depth_image_np
+    
+if __name__ == '__main__':
+    cam2 = ArenaCamera("233801341")  # Hypothetical second camera serial number
+    cam2.create()
+    cam1 = ArenaCamera("233800056")
+    cam1.create()
+
+    # cv2.namedWindow("Z1",cv2.WINDOW_AUTOSIZE)
+    # cv2.namedWindow("Z2",cv2.WINDOW_AUTOSIZE)
+
+    i=0
+    while i<20:
+        raw1 = cam1.read()
+        X2, Y2, Z2 = cam2.capture()
+        # X1, Y1, Z1 = cam1.capture()
+        
+
+        # Display images for the first camera
+        # cv2.imshow("X1", X1)
+        # cv2.imshow("Y1", Y1)
+        # cv2.imshow("Z1", Z1)
+        
+        # Display images for the second camera
+        # cv2.imshow("X2", X2)
+        # cv2.imshow("Y2", Y2)
+        # cv2.imshow("Z2", Z2)
+        
+        # if cv2.waitKey(0) & 0xFF == ord('q'):
+        #     break
+        # cv2.imwrite("z1.png",Z1)
+        cv2.imwrite("z2.png",Z2)
+        # time.sleep(3)
+        i=i+1
+        cv2.imwrite("raw1.png",raw1)
+        print(f'read {i}')
+        
+
+    cv2.destroyAllWindows()
+    cam1.destroy()
+    cam2.destroy()
+    ArenaCamera.shutdown()

lib/camera/MindVisionCamera.py

@@ -0,0 +1,153 @@
+import sys
+import platform
+from PySide6.QtWidgets import QApplication, QMainWindow, QLabel
+from PySide6.QtCore import Qt
+from PySide6.QtGui import QImage, QPixmap
+from PySide6.QtCore import QTimer
+import numpy as np
+import ctypes
+from lib.camera.mvsdk import *
+from lib.camera import mvsdk
+
+
+class MindVisionCamera:
+    def __init__(self, sn):
+        self.sn = sn  # 相机序列号
+        self.hCamera = None  # 相机句柄
+        self.FrameBufferSize = None  # 帧缓冲区大小
+        self.pFrameBuffer = None  # 帧缓冲区指针
+        self.monoCamera = None  # 是否为单色相机标志
+        self.cap = None  # 相机能力参数
+
+        # Initialize camera
+        self._initialize_camera()
+
+    def _initialize_camera(self):
+        # Enumerate cameras
+        DevList = CameraEnumerateDevice()
+        nDev = len(DevList)
+        if nDev < 1:
+            print("No camera was found!")
+            return
+
+        for i, DevInfo in enumerate(DevList):
+            # 检索指定sn相机
+            if DevInfo.GetSn() == self.sn:
+                print(f"Found camera with SN: {self.sn}")
+                break
+        else:
+            print(f"Camera with SN: {self.sn} not found!")
+            return
+
+        # Open camera
+        try:
+            self.hCamera = CameraInit(DevInfo, -1, -1)
+        except CameraException as e:
+            print("CameraInit Failed({}): {}".format(e.error_code, e.message))
+            return
+        # 获取相机参数
+        self.cap = CameraGetCapability(self.hCamera)
+        # 判断是否为单色相机
+        self.monoCamera = self.cap.sIspCapacity.bMonoSensor != 0
+
+        if self.monoCamera:
+            CameraSetIspOutFormat(self.hCamera, CAMERA_MEDIA_TYPE_MONO8)
+
+        CameraSetTriggerMode(self.hCamera, 0)  # 设置触发模式为连续采集
+        CameraSetAeState(self.hCamera, 1)  # 关闭自动曝光
+        # CameraSetExposureTime(self.hCamera, 30 * 1000)  # 设置曝光时间为30ms
+        CameraPlay(self.hCamera)  # 开始采集
+
+        # 分配帧缓冲区
+        self.FrameBufferSize = (
+            self.cap.sResolutionRange.iWidthMax
+            * self.cap.sResolutionRange.iHeightMax
+            * (1 if self.monoCamera else 3)
+        )
+        self.pFrameBuffer = CameraAlignMalloc(self.FrameBufferSize, 16)
+
+    def capture(self):
+        try:
+            # 从相机获取图像数据，2000ms超时，返回原始数据指针和帧头信息
+            pRawData, FrameHead = CameraGetImageBuffer(self.hCamera, 2000)
+            CameraImageProcess(self.hCamera, pRawData, self.pFrameBuffer, FrameHead)
+            # 反转
+            if platform.system() == "Windows":
+                mvsdk.CameraFlipFrameBuffer(self.pFrameBuffer, FrameHead, 1)
+            # 此时图片已经存储在pFrameBuffer中， 对于彩色相机pFrameBuffer=RGB数据， 黑白相机pFrameBuffer=8位灰度数据
+            # 将图像数据转换为OpenCV格式
+            image_array = (mvsdk.c_ubyte * FrameHead.uBytes).from_address(
+                self.pFrameBuffer
+            )
+
+            if self.monoCamera:
+                image_np = np.array(image_array).reshape(
+                    FrameHead.iHeight, FrameHead.iWidth
+                )
+            else:
+                image_np = np.array(image_array).reshape(
+                    FrameHead.iHeight, FrameHead.iWidth, 3
+                )
+            # Convert to OpenCV format
+            if FrameHead.uiMediaType == CAMERA_MEDIA_TYPE_MONO8:
+                image = np.frombuffer(image_np, dtype=np.uint8).reshape(
+                    FrameHead.iHeight, FrameHead.iWidth
+                )
+            else:
+                image = np.frombuffer(image_np, dtype=np.uint8).reshape(
+                    FrameHead.iHeight, FrameHead.iWidth, 3
+                )
+
+            CameraReleaseImageBuffer(self.hCamera, pRawData)
+            return image
+
+        except CameraException as e:
+            print("CameraGetImageBuffer failed({}): {}".format(e.error_code, e.message))
+            return None
+
+    def __del__(self):
+        if self.hCamera:
+            CameraUnInit(self.hCamera)
+        if self.pFrameBuffer:
+            CameraAlignFree(self.pFrameBuffer)
+
+
+# class MainWindow(QMainWindow):
+#     def __init__(self, camera):
+#         super().__init__()
+#         self.setWindowTitle("MindVision Camera")
+#         self.setGeometry(100, 100, 800, 600)
+#         self.label = QLabel(self)
+#         self.label.setAlignment(Qt.AlignCenter)
+#         self.label.setScaledContents(True)  # 自动缩放图像以适应标签大小
+#         self.camera = camera
+#         self.timer = QTimer(self)
+#         self.timer.timeout.connect(self.update_frame)
+#         self.timer.start(30)  # Update frame every 30ms
+
+#     def closeEvent(self, event):
+#         self.timer.stop()
+#         self.camera.__del__()
+#         event.accept()
+
+#     def resizeEvent(self, event):
+#         super().resizeEvent(event)
+#         self.update_frame()  # 窗口大小改变时更新图像
+
+#     def update_frame(self):
+#         image = self.camera.capture()
+#         if image is not None:
+#             if self.camera.monoCamera:
+#                 qimage = QImage(image.data, image.shape[1], image.shape[0], QImage.Format_Grayscale8)
+#             else:
+#                 qimage = QImage(image.data, image.shape[1], image.shape[0], QImage.Format_BGR888)
+#             pixmap = QPixmap.fromImage(qimage)
+#             self.label.setPixmap(pixmap)
+#             self.label.resize(self.size())  # 调整标签大小以适应窗口
+
+# if __name__ == '__main__':
+#     app = QApplication(sys.argv)
+#     cam = MindVisionCamera("054042423002")
+#     window = MainWindow(cam)
+#     window.show()
+#     sys.exit(app.exec())