from ctypes import *
import VSensorLaserSDK
GetResultCallBack = VSensorLaserSDK.CALLBACK_FUNC_TYPE(None, c_void_p)
def result_callback(pResult):
	result = cast(pResult, POINTER(VSensorLaserSDK.VSensorLaserResult)).contents
	VSensorLaserSDK.Save3DCloud("./callback.pcd", result, VSensorLaserSDK.POINT_TYPR_DISORDER)
	VSensorLaserSDK.SaveGrayMap("./callback.bmp", result)
CALL_BACK_FUN = GetResultCallBack(result_callback)
import time
def main():
	#获取SDK版本号
	version = VSensorLaserSDK.GetSdkVersionString()
	print("sdk version: {}".format(version))
	#获取列表
	devList = VSensorLaserSDK.GetDeviceList()
	err_code = VSensorLaserSDK.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 = VSensorLaserSDK.DeviceConnect(0)
	if err_code != 0:
		print("connect device failed status = {}".format(err_code))
		return
	else:
		print("connect device success")
	#注册回调函数
	#VSensorLaserSDK.RegisterCallbackFuncGetResult(CALL_BACK_FUN)
	#获取标定参数
	# para = (c_double * 17)()
	# num = VSensorLaserSDK.GetPara(para)
	# print("para num {}".format(num))
	# for i in range(0, num):
	# 	print("{}".format(para[i]))
	#设置投影等级
	err_code = VSensorLaserSDK.SetProjectLight(1)
	if err_code != 0:
		print("set project light failed status = {}".format(err_code))
		return
	else:
		print("set project light success")
	#设置曝光时间
	err_code = VSensorLaserSDK.SetExposureTime(1.0)
	if err_code != 0:
		print("set exposure time failed status = {}".format(err_code))
		return
	else:
		print("set exposure time success")
	#设置为采集模式
	err_code = VSensorLaserSDK.SetCaptureMode(VSensorLaserSDK.MODE_SOFT)
	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 = VSensorLaserSDK.VSensorLaserResult()
	result = VSensorLaserSDK.SingleRestruction(result, VSensorLaserSDK.OUTPUT_MODE_ALL)
	err_code = VSensorLaserSDK.GetLastError()
	if err_code != 0:
		print("single restruction failed status = {}".format(err_code))
		return
	else:
		print("single restruction success")
    #获取分辨率
	resolution = VSensorLaserSDK.VSensorLaserImageResolution()
	VSensorLaserSDK.GetImageResolution(resolution)
	size1 = resolution.Width
	size2 = resolution.Width * resolution.Height
	#结构体获取数据
	# buf = (c_ubyte * size1).from_address(result.pointx)
	# buf = (c_ubyte * size1).from_address(result.pointy)
	# buf = (c_ubyte * size1).from_address(result.pointz)
	# buf = (c_bool * size1).from_address(result.mask)
	# buf = (c_ubyte * size2).from_address(result.GraySrcMap)
	# for x in buf:
	# 	print(x)
	#保存点云为pcd格式
	err_code = VSensorLaserSDK.Save3DCloud("./test_soft.pcd", result, VSensorLaserSDK.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 = VSensorLaserSDK.SaveGrayMap("./gray.bmp", result)
	if err_code != 0:
		print("save gray image failed status = {}".format(err_code))
		return
	else:
		print("save gray image success")
	#录制点云
	err_code = VSensorLaserSDK.Record3DCloud("./test_record.pcd", VSensorLaserSDK.RECORD_DIS, 10.0)
	if err_code != 0:
		print("set record 3D cloud failed status = {}".format(err_code))
		return
	else:
		print("set record 3D cloud success")
	#连续采集
	print("start continuous capture...")
	VSensorLaserSDK.ContinuousRestructionStart(VSensorLaserSDK.OUTPUT_MODE_ALL)
	for num in range(0, 5):
		result = VSensorLaserSDK.CaptureFrame(result, 2000)
		print("{}  num = {}".format(num + 1, result.nPointNum))
		VSensorLaserSDK.Save3DCloud("./test_soft" + "{}".format(num + 1) +".pcd", result, VSensorLaserSDK.POINT_TYPR_DISORDER)
	VSensorLaserSDK.ContinuousRestructionStop()
	#获取录制点云
	cloudRecordList = VSensorLaserSDK.GetRecord3DCloud()
	err_code = VSensorLaserSDK.GetLastError()
	if err_code != 0:
		print("get record 3D cloud failed, status = {}".format(err_code))
		return
	else:
		print("get record 3D cloud success, num  = {}".format(cloudRecordList.size))
	#处理返回结果
	#for i in range(cloudRecordList.size):
	#	arr = cloudRecordList.data[i]
	#	print(arr.data[0], arr.data[1], arr.data[2])
	#外触发采集
	print("start external capture...")
	err_code = VSensorLaserSDK.SetCaptureMode(VSensorLaserSDK.MODE_EXTERNAL)
	if err_code != 0:
		print("set capture mode failed status = {}".format(err_code))
		return
	else:
		print("set capture mode success")
	#设置缓存区大小
	bufferNum = 500
	err_code = VSensorLaserSDK.SetCloudBufferNum(bufferNum)
	if err_code != 0:
		print("set cloud buffer num failed status = {}".format(err_code))
		return
	else:
		print("set cloud buffer num success")
    #预留时间进行硬件触发采集
	print("wait external trigger...")
	time.sleep(5)
	#获取缓存区触发的点云
	cloudBufferList = VSensorLaserSDK.GetCloudBuffer(bufferNum)
	err_code = VSensorLaserSDK.GetLastError()
	if err_code != 0:
		print("get cloud buffer failed, status = {}".format(err_code))
		return
	else:
		print("get cloud buffer success, num  = {}".format(len(cloudBufferList)))
	for i, cloudBuffer in enumerate(cloudBufferList):
		VSensorLaserSDK.Save3DCloud("./test_external" + "{}".format(i + 1) +".pcd", cloudBuffer, VSensorLaserSDK.POINT_TYPR_DISORDER)
	#设置为预览模式
	err_code = VSensorLaserSDK.SetCaptureMode(VSensorLaserSDK.MODE_CONTINUE)
	if err_code != 0:
		print("set capture mode failed status = {}".format(err_code))
		return
	else:
		print("set capture mode success")
    #获取预览图
	bufferGray = (c_ubyte * size2)()
	err_code = VSensorLaserSDK.CameraOut(bufferGray)
	if err_code != 0:
		print("camera out failed status = {}".format(err_code))
		return
	else:
		print("camera out success")
	#设备反初始化
	err_code = VSensorLaserSDK.DeviceUnInit()
	if err_code != 0:
		print("device release failed status = {}".format(err_code))
		return
	else:
		print("device release success")
main()

