DataAcquisition-Python/SDK/Vzense/VzenseDS_api.py

import math

from SDK.Vzense.VzenseDS_define import *
import platform
import os
import ctypes
from ctypes import *

gCallbackFuncList = []


class VzenseTofCam():
    device_handle = c_void_p(0)

    def __init__(self):
        system_ = platform.system().lower()
        machine_ = platform.machine().lower()
        architecture_ = platform.architecture()[0]
        # print('system:',system_)
        # print('machine_:',machine_)
        # print('architecture:',architecture_)
        if system_ == 'linux':
            if machine_ == 'x86_64':
                os_info = os.uname()
                # print('os_info:',type(os_info))
                system_info = os_info.version
                # print('version:',system_info)
                if system_info.find('18.04') != -1 or system_info.find('20.04') != -1:
                    libpath = os.path.dirname(os.path.abspath(__file__)) + "/Ubuntu18.04/Lib/libNebula_api.so"
                    # print("load lib from ", libpath)
                    self.vz_cam_lib = cdll.LoadLibrary(libpath)
                else:
                    libpath = os.path.dirname(os.path.abspath(__file__)) + "/Ubuntu16.04/Lib/libNebula_api.so"
                    # print(libpath)
                    self.vz_cam_lib = cdll.LoadLibrary(libpath)
            elif machine_ == 'aarch64':
                libpath = os.path.dirname(os.path.abspath(__file__)) + "/AArch64/Lib/libNebula_api.so"
                # print(libpath)
                self.vz_cam_lib = cdll.LoadLibrary(libpath)
            else:
                # print('do not supported OS', system_, machine_)
                exit()
        elif platform.system() == 'Windows':
            if machine_ == 'amd64':
                if architecture_ == '64bit':
                    libpath = os.path.dirname(os.path.abspath(__file__)) + "/Windows/Bin/x64/Nebula_api.dll"
                    # print(libpath)
                    self.vz_cam_lib = cdll.LoadLibrary(libpath)
                else:
                    libpath = os.path.dirname(os.path.abspath(__file__)) + "/Windows/Bin/x86/Nebula_api.dll"
                    # print(libpath)
                    self.vz_cam_lib = cdll.LoadLibrary(libpath)
            else:
                # print('do not supported OS', system_, machine_)
                exit()
        else:
            # print('do not supported OS', system_, machine_)
            exit()

        self.device_handle = c_void_p(0)
        self.vz_cam_lib.VZ_Initialize()

    def __del__(self):
        self.vz_cam_lib.VZ_Shutdown()

    def VZ_GetSDKVersion(self):
        tmp = c_char * 64
        version = tmp()
        return self.vz_cam_lib.VZ_GetSDKVersion(version), version.value

    def VZ_GetDeviceCount(self):
        count = c_int()
        self.vz_cam_lib.VZ_GetDeviceCount(byref(count))
        return count.value

    def VZ_GetDeviceInfoList(self, cam_count=1):
        tmp = VzDeviceInfo * cam_count
        device_infolist = tmp()
        return self.vz_cam_lib.VZ_GetDeviceInfoList(cam_count, device_infolist), device_infolist

    def VZ_GetDeviceInfo(self, cam_index=0):
        device_info = VzDeviceInfo()
        return self.vz_cam_lib.VZ_GetDeviceInfo(cam_index, byref(device_info)), device_info

    def VZ_OpenDeviceByUri(self, uri=c_char_p()):
        if uri:
            return self.vz_cam_lib.VZ_OpenDeviceByUri(uri, byref(self.device_handle))
        else:
            return VzReturnStatus.VzRetInputPointerIsNull

    def VZ_OpenDeviceByAlias(self, alias=c_char_p()):
        if alias:
            return self.vz_cam_lib.VZ_OpenDeviceByAlias(alias, byref(self.device_handle))
        else:
            return VzReturnStatus.VzRetInputPointerIsNull

    def VZ_OpenDeviceByIP(self, ip=c_char_p()):
        if ip:
            return self.vz_cam_lib.VZ_OpenDeviceByIP(ip, byref(self.device_handle))
        else:
            return VzReturnStatus.VzRetInputPointerIsNull, 0

    def VZ_CloseDevice(self):
        return self.vz_cam_lib.VZ_CloseDevice(byref(self.device_handle))

    def VZ_StartStream(self):
        return self.vz_cam_lib.VZ_StartStream(self.device_handle)

    def VZ_StopStream(self):
        return self.vz_cam_lib.VZ_StopStream(self.device_handle)

    def VZ_GetFrameReady(self, waitTime=c_uint16(33)):
        frameready = VzFrameReady()
        return self.vz_cam_lib.VZ_GetFrameReady(self.device_handle, waitTime, byref(frameready)), frameready

    def VZ_GetFrame(self, frametype=VzFrameType.VzDepthFrame):
        Vzframe = VzFrame()
        return self.vz_cam_lib.VZ_GetFrame(self.device_handle, frametype.value, byref(Vzframe)), Vzframe

    def VZ_SetWorkMode(self, mode=VzWorkMode.ActiveMode):
        return self.vz_cam_lib.VZ_SetWorkMode(self.device_handle, mode.value)

    def VZ_GetWorkMode(self):
        mode = VzWorkMode(0)
        return self.vz_cam_lib.VZ_GetWorkMode(self.device_handle, byref(mode)), mode

    def VZ_SetSoftwareSlaveTrigger(self):
        return self.vz_cam_lib.VZ_SetSoftwareSlaveTrigger(self.device_handle)

    def VZ_RebootDevie(self):
        return self.vz_cam_lib.VZ_RebootDevie(self.device_handle)

    def VZ_GetSensorIntrinsicParameters(self, sensorType=VzSensorType.VzToFSensor):
        CameraParameters = VzSensorIntrinsicParameters()
        return self.vz_cam_lib.VZ_GetSensorIntrinsicParameters(self.device_handle, sensorType.value,
                                                               byref(CameraParameters)), CameraParameters

    def VZ_GetSensorExtrinsicParameters(self):
        CameraExtrinsicParameters = VzSensorExtrinsicParameters()
        return self.vz_cam_lib.VZ_GetSensorExtrinsicParameters(self.device_handle, byref(
            CameraExtrinsicParameters)), CameraExtrinsicParameters

    def VZ_GetFirmwareVersion(self):
        tmp = c_char * 64
        fw = tmp()
        return self.vz_cam_lib.VZ_GetFirmwareVersion(self.device_handle, fw, 63), fw.value

    def VZ_GetDeviceMACAddress(self):
        tmp = c_char * 18
        mac = tmp()
        return self.vz_cam_lib.VZ_GetDeviceMACAddress(self.device_handle, mac), mac.value

    def VZ_SetIRGMMGain(self, gmmgain=c_uint8(20)):
        return self.vz_cam_lib.VZ_SetIRGMMGain(self.device_handle, gmmgain)

    def VZ_GetIRGMMGain(self):
        gmmgain = c_uint8(1)
        return self.vz_cam_lib.VZ_GetIRGMMGain(self.device_handle, byref(gmmgain)), gmmgain.value

    def VZ_SetColorPixelFormat(self, pixelFormat=VzPixelFormat.VzPixelFormatBGR888):
        return self.vz_cam_lib.VZ_SetColorPixelFormat(self.device_handle, pixelFormat)

    def VZ_SetColorResolution(self, w=c_int32(1600), h=c_int32(1200)):
        return self.vz_cam_lib.VZ_SetColorResolution(self.device_handle, w, h)

    def VZ_GetColorResolution(self):
        w = c_int32(1600)
        h = c_int32(1200)
        return self.vz_cam_lib.VZ_GetColorResolution(self.device_handle, byref(w), byref(h)), w, h

    def VZ_SetFrameRate(self, value=c_uint8(30)):
        return self.vz_cam_lib.VZ_SetFrameRate(self.device_handle, value)

    def VZ_GetFrameRate(self):
        value = c_uint8(1)
        return self.vz_cam_lib.VZ_GetFrameRate(self.device_handle, byref(value)), value.value

    def VZ_SetExposureControlMode(self, sensorType=VzSensorType.VzToFSensor,
                                  mode=VzExposureControlMode.VzExposureControlMode_Manual):
        return self.vz_cam_lib.VZ_SetExposureControlMode(self.device_handle, sensorType.value, mode.value)

    def VZ_SetExposureTime(self, sensorType=VzSensorType.VzToFSensor, params=c_int32(0)):
        ExposureTime = VzExposureTimeParams(VzExposureControlMode.VzExposureControlMode_Manual.value, params)
        return self.vz_cam_lib.VZ_SetExposureTime(self.device_handle, sensorType.value, ExposureTime)

    def VZ_GetExposureTime(self, sensorType=VzSensorType.VzToFSensor):
        params = VzExposureTimeParams(VzExposureControlMode.VzExposureControlMode_Manual.value, 0)
        return self.vz_cam_lib.VZ_GetExposureTime(self.device_handle, sensorType.value, byref(params)), params

    def VZ_SetTimeFilterParams(self, params=VzTimeFilterParams()):
        return self.vz_cam_lib.VZ_SetTimeFilterParams(self.device_handle, params)

    def VZ_GetTimeFilterParams(self):
        params = VzTimeFilterParams()
        return self.vz_cam_lib.VZ_GetTimeFilterParams(self.device_handle, byref(params)), params

    def VZ_SetConfidenceFilterParams(self, params=VzConfidenceFilterParams()):
        return self.vz_cam_lib.VZ_SetConfidenceFilterParams(self.device_handle, params)

    def VZ_GetConfidenceFilterParams(self):
        params = VzConfidenceFilterParams()
        return self.vz_cam_lib.VZ_GetConfidenceFilterParams(self.device_handle, byref(params)), params

    def VZ_SetFlyingPixelFilterParams(self, params=VzFlyingPixelFilterParams()):
        return self.vz_cam_lib.VZ_SetFlyingPixelFilterParams(self.device_handle, params)

    def VZ_GetFlyingPixelFilterParams(self):
        params = VzFlyingPixelFilterParams()
        return self.vz_cam_lib.VZ_GetFlyingPixelFilterParams(self.device_handle, byref(params)), params

    def VZ_SetFillHoleFilterEnabled(self, enable=c_bool(True)):
        return self.vz_cam_lib.VZ_SetFillHoleFilterEnabled(self.device_handle, enable)

    def VZ_GetFillHoleFilterEnabled(self):
        enable = c_bool(True)
        return self.vz_cam_lib.VZ_GetFillHoleFilterEnabled(self.device_handle, byref(enable)), enable.value

    def VZ_SetSpatialFilterEnabled(self, enable=c_bool(True)):
        return self.vz_cam_lib.VZ_SetSpatialFilterEnabled(self.device_handle, enable)

    def VZ_GetSpatialFilterEnabled(self):
        enable = c_bool(True)
        return self.vz_cam_lib.VZ_GetSpatialFilterEnabled(self.device_handle, byref(enable)), enable.value

    def VZ_SetTransformColorImgToDepthSensorEnabled(self, enabled=c_bool(True)):
        return self.vz_cam_lib.VZ_SetTransformColorImgToDepthSensorEnabled(self.device_handle, enabled)

    def VZ_GetTransformColorImgToDepthSensorEnabled(self):
        enabled = c_bool(True)
        return self.vz_cam_lib.VZ_GetTransformColorImgToDepthSensorEnabled(self.device_handle, byref(enabled)), enabled

    def VZ_SetTransformDepthImgToColorSensorEnabled(self, enabled=c_bool(True)):
        return self.vz_cam_lib.VZ_SetTransformDepthImgToColorSensorEnabled(self.device_handle, enabled)

    def VZ_GetTransformDepthImgToColorSensorEnabled(self):
        enabled = c_bool(True)
        return self.vz_cam_lib.VZ_GetTransformDepthImgToColorSensorEnabled(self.device_handle, byref(enabled)), enabled

    def VZ_ConvertDepthFrameToPointCloudVector(self, depthFrame=VzFrame()):
        len = depthFrame.width * depthFrame.height
        tmp = VzVector3f * len
        pointlist = tmp()
        return self.vz_cam_lib.VZ_ConvertDepthFrameToPointCloudVector(self.device_handle, byref(depthFrame),
                                                                      pointlist), pointlist

    def VZ_ConvertDepthToPointCloud(self, depth = VzDepthVector3()):
        point = VzVector3f()
        ret, params = self.VZ_GetSensorIntrinsicParameters()
        ret = self.vz_cam_lib.VZ_ConvertDepthToPointCloud(self.device_handle, id(depth), id(point), 1, params)

        print("-------------------------")
        return ret, point

    def VZ_SetHotPlugStatusCallback(self, callbackfunc=c_void_p):
        callbackFunc_ = ctypes.CFUNCTYPE(c_void_p, POINTER(VzDeviceInfo), c_int32)(callbackfunc)
        gCallbackFuncList.append(callbackFunc_)
        return self.vz_cam_lib.VZ_SetHotPlugStatusCallback(callbackFunc_)

    def VZ_GetManaulMaxExposureTime(self):
        tmp = r"Py_ToFExposureTimeMax"
        propertyKey = (c_char * 100)(*bytes(tmp, 'utf-8'))
        maxExposureTime = VzExposureTimeParams(VzExposureControlMode.VzExposureControlMode_Manual.value)
        return self.vz_cam_lib.VZ_GetProperty(self.device_handle, byref(propertyKey), byref(maxExposureTime),
                                              8), maxExposureTime.exposureTime

    def VZ_SetParamsByJson(self, imgpath):
        pimgpath = (c_char * 1000)(*bytes(imgpath, 'utf-8'))
        return self.vz_cam_lib.VZ_SetParamsByJson(self.device_handle, byref(pimgpath))

    def VZ_SetColorGain(self, params=c_float(1.0)):
        return self.vz_cam_lib.VZ_SetColorGain(self.device_handle, params)

    def VZ_GetColorGain(self):
        tmp = c_float * 1
        params = tmp()
        return self.vz_cam_lib.VZ_GetColorGain(self.device_handle, params), params

    def VZ_GetProperty(self, propertyKey=c_char_p(), pData=c_void_p, dataSize=c_int32(8)):
        return self.vz_cam_lib.VZ_GetProperty(self.device_handle, propertyKey, pData, dataSize), pData

    def VZ_GetAutoMaxExposureTime(self):
        tmp = r"Py_RGBExposureTimeMax"
        propertyKey = (c_char * 100)(*bytes(tmp, 'utf-8'))
        maxExposureTime = VzExposureTimeParams(VzExposureControlMode.VzExposureControlMode_Auto.value)
        return self.vz_cam_lib.VZ_GetProperty(self.device_handle, byref(propertyKey), byref(maxExposureTime),
                                              8), maxExposureTime.exposureTime

    def VZ_GetManualMaxExposureTime(self):
        tmp = r"Py_RGBExposureTimeMax"
        propertyKey = (c_char * 100)(*bytes(tmp, 'utf-8'))
        maxExposureTime = VzExposureTimeParams(VzExposureControlMode.VzExposureControlMode_Manual.value)
        return self.vz_cam_lib.VZ_GetProperty(self.device_handle, byref(propertyKey), byref(maxExposureTime),
                                              8), maxExposureTime.exposureTime

    def VZ_SetAutoExposureTime(self, sensorType=VzSensorType.VzColorSensor, params=c_int32(0)):
        ExposureTime = VzExposureTimeParams(VzExposureControlMode.VzExposureControlMode_Auto.value, params)
        return self.vz_cam_lib.VZ_SetExposureTime(self.device_handle, sensorType.value, ExposureTime)

    def VZ_GetAutoExposureTime(self, sensorType=VzSensorType.VzColorSensor):
        params = VzExposureTimeParams(VzExposureControlMode.VzExposureControlMode_Auto.value, 0)
        return self.vz_cam_lib.VZ_GetExposureTime(self.device_handle, sensorType.value, byref(params)), params