from ctypes import *
import VSensorSDK
GetResultCallBack = VSensorSDK.CALLBACK_FUNC_TYPE(None, c_void_p)
def result_callback(pResult):
	result = cast(pResult, POINTER(VSensorSDK.VSensorResult)).contents
	VSensorSDK.Save3DCloud("./callback.pcd", result, VSensorSDK.POINT_TYPR_DISORDER)
	VSensorSDK.SaveGrayMap("./callback.bmp", result, VSensorSDK.GRAY_TYPR_UNCORRECT)
CALL_BACK_FUN = GetResultCallBack(result_callback)
def main():
	#获取SDK版本号
	version = VSensorSDK.GetSdkVersionString()
	print("sdk version: {}".format(version))
	#获取列表
	devList = VSensorSDK.GetDeviceList()
	err_code = VSensorSDK.GetLastError()
	if err_code != 0:
		print("get device list failed, status = {}".format(err_code))
		return
	else:
		print("get device list success, device num  = {}".format(len(devList)))
	for i, devInfo in enumerate(devList):
		print("device num {}: name {}".format(i, devInfo.GetCameraName()))
	#连接相机 0为连接第一台设备
	err_code = VSensorSDK.DeviceConnect(0)
	if err_code != 0:
		print("connect device failed status = {}".format(err_code))
		return
	else:
		print("connect device success")
	#设备参数初始化，仅上电时需要进行此操作，初始化可通过GetDeviceParameterInitDelayTime接口获取
	print("waiting for parameter init ...")
	err_code = VSensorSDK.DeviceParameterInit()
	if err_code != 0:
		print("parameter init failed status = {}".format(err_code))
		return
	else:
		print("parameter init success")
	#注册回调函数
	#VSensorSDK.RegisterCallbackFuncGetResult(CALL_BACK_FUN)
	#获取标定参数
	# para = (c_double * 100)()
	# num = VSensorSDK.GetPara(para)
	# print("para num {}".format(num))
	# for i in range(0, num):
	# 	print("{}".format(para[i]))
	#设置曝光时间
	err_code = VSensorSDK.SetExposureTime(10.0)
	if err_code != 0:
		print("set exposure time failed status = {}".format(err_code))
		return
	else:
		print("set exposure time success")
	#设置为采集模式
	err_code = VSensorSDK.SetCaptureMode(VSensorSDK.MODE_CAPTURE)
	if err_code != 0:
		print("set capture mode failed status = {}".format(err_code))
		return
	else:
		print("set capture mode success")
    #单次采集测试
	print("start single capture...")
	result = VSensorSDK.VSensorResult()
	result = VSensorSDK.SingleRestruction(result, VSensorSDK.OUTPUT_MODE_ALL)
	err_code = VSensorSDK.GetLastError()
	if err_code != 0:
		print("single restruction failed status = {}".format(err_code))
		return
	else:
		print("single restruction success")
    #获取分辨率
	resolution = VSensorSDK.VSensorImageResolution()
	VSensorSDK.GetImageResolution(resolution)
	size = resolution.Width * resolution.Height
	#结构体获取数据
	# buf = (c_ubyte * size).from_address(result.pointx)
	# buf = (c_ubyte * size).from_address(result.pointy)
	# buf = (c_ubyte * size).from_address(result.pointz)
	# buf = (c_ubyte * size).from_address(result.pointr)
	# buf = (c_ubyte * size).from_address(result.pointg)
	# buf = (c_ubyte * size).from_address(result.pointb)
	# buf = (c_bool * size).from_address(result.mask)
	# buf = (c_ubyte * size * 2).from_address(result.GraySrcMap)
	# buf = (c_ushort * size).from_address(result.DepthMap)
	# for x in buf:
	# 	print(x)
	#保存点云为pcd格式
	err_code = VSensorSDK.Save3DCloud("./test.pcd", result, VSensorSDK.POINT_TYPR_DISORDER)
	if err_code != 0:
		print("save 3D points failed status = {}".format(err_code))
		return
	else:
		print("save 3D points success")
	#保存灰度图
	err_code = VSensorSDK.SaveGrayMap("./gray.bmp", result, VSensorSDK.GRAY_TYPR_UNCORRECT)
	if err_code != 0:
		print("save gray image failed status = {}".format(err_code))
		return
	else:
		print("save gray image success")
    #保存深度图
	err_code = VSensorSDK.SaveDepthMap("./depth.png", result)
	if err_code != 0:
		print("save depth image failed status = {}".format(err_code))
		return
	else:
		print("save depth image success")
	#连续采集
	print("start continuous capture...")
	VSensorSDK.ContinuousRestructionStart(VSensorSDK.OUTPUT_MODE_ALL)
	for num in range(0, 5):
		result = VSensorSDK.CaptureFrame(result, 2000)
		print("{}  num = {}".format(num + 1, result.nPointNum))
		VSensorSDK.Save3DCloud("./test" + "{}".format(num + 1) +".pcd", result, VSensorSDK.POINT_TYPR_DISORDER)
	VSensorSDK.ContinuousRestructionStop()
    #设置为预览模式
	err_code = VSensorSDK.SetCaptureMode(VSensorSDK.MODE_VIEW)
	if err_code != 0:
		print("set capture mode failed status = {}".format(err_code))
		return
	else:
		print("set capture mode success")
    #获取预览图
	bufferGray = (c_ubyte * size * 2)()
	bufferRgb = (c_ubyte * size * 3)()
	err_code = VSensorSDK.CameraOut1(bufferGray, bufferRgb)
	if err_code != 0:
		print("camera out failed status = {}".format(err_code))
		return
	else:
		print("camera out success")
	#设备反初始化
	err_code = VSensorSDK.DeviceUnInit()
	if err_code != 0:
		print("device release failed status = {}".format(err_code))
		return
	else:
		print("device release success")
main()

