camera_driver/CameraPublisher.cpp.bak

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <dlfcn.h> 
#include "IMVApi.h"
#include "IMVDefines.h"

#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>

extern "C"{

#define	MONO_CHANNEL_NUM	1
#define	RGB_CHANNEL_NUM		3
#define	BGR_CHANNEL_NUM		3

#define sleep(ms)	usleep(1000 * ms)

typedef const char*	(IMV_CALL * DLL_GetVersion)							();
typedef int			(IMV_CALL * DLL_EnumDevices)						(OUT IMV_DeviceList *pDeviceList, IN unsigned int interfaceType);
typedef int			(IMV_CALL * DLL_EnumDevicesByUnicast)				(OUT IMV_DeviceList *pDeviceList, IN const char* pIpAddress);
typedef int			(IMV_CALL * DLL_CreateHandle)						(OUT IMV_HANDLE* handle, IN IMV_ECreateHandleMode mode, IN void* pIdentifier);
typedef int			(IMV_CALL * DLL_DestroyHandle)						(IN IMV_HANDLE handle);
typedef int			(IMV_CALL * DLL_GetDeviceInfo)						(IN IMV_HANDLE handle, OUT IMV_DeviceInfo *pDevInfo);
typedef int			(IMV_CALL * DLL_Open)								(IN IMV_HANDLE handle);
typedef int			(IMV_CALL * DLL_OpenEx)								(IN IMV_HANDLE handle, IN IMV_ECameraAccessPermission accessPermission);
typedef bool		(IMV_CALL * DLL_IsOpen)								(IN IMV_HANDLE handle);
typedef int			(IMV_CALL * DLL_Close)								(IN IMV_HANDLE handle);
typedef int			(IMV_CALL * DLL_GIGE_ForceIpAddress)				(IN IMV_HANDLE handle, IN const char* pIpAddress, IN const char* pSubnetMask, IN const char* pGateway);
typedef int			(IMV_CALL * DLL_GIGE_GetAccessPermission)			(IN IMV_HANDLE handle, IMV_ECameraAccessPermission* pAccessPermission);
typedef int			(IMV_CALL * DLL_GIGE_SetAnswerTimeout)				(IN IMV_HANDLE handle, IN unsigned int timeout);
typedef int			(IMV_CALL * DLL_DownLoadGenICamXML)					(IN IMV_HANDLE handle, IN const char* pFullFileName);
typedef int			(IMV_CALL * DLL_SaveDeviceCfg)						(IN IMV_HANDLE handle, IN const char* pFullFileName);
typedef int			(IMV_CALL * DLL_LoadDeviceCfg)						(IN IMV_HANDLE handle, IN const char* pFullFileName, OUT IMV_ErrorList* pErrorList);
typedef int			(IMV_CALL * DLL_WriteUserPrivateData)				(IN IMV_HANDLE handle, IN void* pBuffer, IN_OUT unsigned int* pLength);
typedef int			(IMV_CALL * DLL_ReadUserPrivateData)				(IN IMV_HANDLE handle, OUT void* pBuffer, IN_OUT unsigned int* pLength);
typedef int			(IMV_CALL * DLL_WriteUARTData)						(IN IMV_HANDLE handle, IN void* pBuffer, IN_OUT unsigned int* pLength);
typedef int			(IMV_CALL * DLL_ReadUARTData)						(IN IMV_HANDLE handle, OUT void* pBuffer, IN_OUT unsigned int* pLength);
typedef int			(IMV_CALL * DLL_SubscribeConnectArg)				(IN IMV_HANDLE handle, IN IMV_ConnectCallBack proc, IN void* pUser);
typedef int			(IMV_CALL * DLL_SubscribeParamUpdateArg)			(IN IMV_HANDLE handle, IN IMV_ParamUpdateCallBack proc, IN void* pUser);
typedef int			(IMV_CALL * DLL_SubscribeStreamArg)					(IN IMV_HANDLE handle, IN IMV_StreamCallBack proc, IN void* pUser);
typedef int			(IMV_CALL * DLL_SubscribeMsgChannelArg)				(IN IMV_HANDLE handle, IN IMV_MsgChannelCallBack proc, IN void* pUser);
typedef int			(IMV_CALL * DLL_SetBufferCount)						(IN IMV_HANDLE handle, IN unsigned int nSize);
typedef int			(IMV_CALL * DLL_ClearFrameBuffer)					(IN IMV_HANDLE handle);
typedef int			(IMV_CALL * DLL_GIGE_SetInterPacketTimeout)			(IN IMV_HANDLE handle, IN unsigned int nTimeout);
typedef int			(IMV_CALL * DLL_GIGE_SetSingleResendMaxPacketNum)	(IN IMV_HANDLE handle, IN unsigned int maxPacketNum);
typedef int			(IMV_CALL * DLL_GIGE_SetMaxLostPacketNum)			(IN IMV_HANDLE handle, IN unsigned int maxLostPacketNum);
typedef int			(IMV_CALL * DLL_StartGrabbing)						(IN IMV_HANDLE handle);
typedef int			(IMV_CALL * DLL_StartGrabbingEx)					(IN IMV_HANDLE handle, IN uint64_t maxImagesGrabbed, IN IMV_EGrabStrategy strategy);
typedef bool		(IMV_CALL * DLL_IsGrabbing)							(IN IMV_HANDLE handle);
typedef int			(IMV_CALL * DLL_StopGrabbing)						(IN IMV_HANDLE handle);
typedef int			(IMV_CALL * DLL_AttachGrabbing)						(IN IMV_HANDLE handle, IN IMV_FrameCallBack proc, IN void* pUser);
typedef int			(IMV_CALL * DLL_GetFrame)							(IN IMV_HANDLE handle, OUT IMV_Frame* pFrame, IN unsigned int timeoutMS);
typedef int			(IMV_CALL * DLL_ReleaseFrame)						(IN IMV_HANDLE handle, IN IMV_Frame* pFrame);
typedef int			(IMV_CALL * DLL_CloneFrame)							(IN IMV_HANDLE handle, IN IMV_Frame* pFrame, OUT IMV_Frame* pCloneFrame);
typedef int			(IMV_CALL * DLL_GetChunkDataByIndex)				(IN IMV_HANDLE handle, IN IMV_Frame* pFrame, IN unsigned int index, OUT IMV_ChunkDataInfo *pChunkDataInfo);
typedef int			(IMV_CALL * DLL_GetStatisticsInfo)					(IN IMV_HANDLE handle, OUT IMV_StreamStatisticsInfo* pStreamStatsInfo);
typedef int			(IMV_CALL * DLL_ResetStatisticsInfo)				(IN IMV_HANDLE handle);
typedef bool		(IMV_CALL * DLL_FeatureIsAvailable)					(IN IMV_HANDLE handle, IN const char* pFeatureName);
typedef bool		(IMV_CALL * DLL_FeatureIsReadable)					(IN IMV_HANDLE handle, IN const char* pFeatureName);
typedef bool		(IMV_CALL * DLL_FeatureIsWriteable)					(IN IMV_HANDLE handle, IN const char* pFeatureName);
typedef bool		(IMV_CALL * DLL_FeatureIsStreamable)				(IN IMV_HANDLE handle, IN const char* pFeatureName);
typedef bool		(IMV_CALL * DLL_FeatureIsValid)						(IN IMV_HANDLE handle, IN const char* pFeatureName);
typedef int			(IMV_CALL * DLL_GetIntFeatureValue)					(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT int64_t* pIntValue);
typedef int			(IMV_CALL * DLL_GetIntFeatureMin)					(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT int64_t* pIntValue);
typedef int			(IMV_CALL * DLL_GetIntFeatureMax)					(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT int64_t* pIntValue);
typedef int			(IMV_CALL * DLL_GetIntFeatureInc)					(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT int64_t* pIntValue);
typedef int			(IMV_CALL * DLL_SetIntFeatureValue)					(IN IMV_HANDLE handle, IN const char* pFeatureName, IN int64_t intValue);
typedef int			(IMV_CALL * DLL_GetDoubleFeatureValue)				(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT double* pDoubleValue);
typedef int			(IMV_CALL * DLL_GetDoubleFeatureMin)				(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT double* pDoubleValue);
typedef int			(IMV_CALL * DLL_GetDoubleFeatureMax)				(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT double* pDoubleValue);
typedef int			(IMV_CALL * DLL_SetDoubleFeatureValue)				(IN IMV_HANDLE handle, IN const char* pFeatureName, IN double doubleValue);
typedef int			(IMV_CALL * DLL_GetBoolFeatureValue)				(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT bool* pBoolValue);
typedef int			(IMV_CALL * DLL_SetBoolFeatureValue)				(IN IMV_HANDLE handle, IN const char* pFeatureName, IN bool boolValue);
typedef int			(IMV_CALL * DLL_GetEnumFeatureValue)				(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT uint64_t* pEnumValue);
typedef int			(IMV_CALL * DLL_SetEnumFeatureValue)				(IN IMV_HANDLE handle, IN const char* pFeatureName, IN uint64_t enumValue);
typedef int			(IMV_CALL * DLL_GetEnumFeatureSymbol)				(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT IMV_String* pEnumSymbol);
typedef int			(IMV_CALL * DLL_SetEnumFeatureSymbol)				(IN IMV_HANDLE handle, IN const char* pFeatureName, IN const char* pEnumSymbol);
typedef int			(IMV_CALL * DLL_GetEnumFeatureEntryNum)				(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT unsigned int* pEntryNum);
typedef int			(IMV_CALL * DLL_GetEnumFeatureEntrys)				(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT IMV_EnumEntryList* pEnumEntryList);
typedef int			(IMV_CALL * DLL_GetStringFeatureValue)				(IN IMV_HANDLE handle, IN const char* pFeatureName, OUT IMV_String* pStringValue);
typedef int			(IMV_CALL * DLL_SetStringFeatureValue)				(IN IMV_HANDLE handle, IN const char* pFeatureName, IN const char* pStringValue);
typedef int			(IMV_CALL * DLL_ExecuteCommandFeature)				(IN IMV_HANDLE handle, IN const char* pFeatureName);
typedef int			(IMV_CALL * DLL_PixelConvert)						(IN IMV_HANDLE handle, IN_OUT IMV_PixelConvertParam* pstPixelConvertParam);
typedef int			(IMV_CALL * DLL_OpenRecord)							(IN IMV_HANDLE handle, IN IMV_RecordParam *pstRecordParam);
typedef int			(IMV_CALL * DLL_InputOneFrame)						(IN IMV_HANDLE handle, IN IMV_RecordFrameInfoParam *pstRecordFrameInfoParam);
typedef int			(IMV_CALL * DLL_CloseRecord)						(IN IMV_HANDLE handle);



// ***********开始： 这部分处理与SDK操作相机无关，用于显示设备列表 ***********
// ***********BEGIN: These functions are not related to API call and used to display device info***********
// 数据帧回调函数
// Data frame callback function
static void onGetFrame(IMV_Frame* pFrame, void* pUser)
{
	if (pFrame == NULL)
	{
		printf("pFrame is NULL\n");
		return;
	}

	printf("Get frame blockId = %llu\n", pFrame->frameInfo.blockId);

	return;
}

static void displayDeviceInfo(IMV_DeviceList deviceInfoList)
{
	IMV_DeviceInfo*	pDevInfo = NULL;
	unsigned int cameraIndex = 0;
	char vendorNameCat[11];
	char cameraNameCat[16];

	// 打印Title行 
	// Print title line 
	printf("\nIdx Type Vendor     Model      S/N             DeviceUserID    IP Address    \n");
	printf("------------------------------------------------------------------------------\n");

	for (cameraIndex = 0; cameraIndex < deviceInfoList.nDevNum; cameraIndex++)
	{
		pDevInfo = &deviceInfoList.pDevInfo[cameraIndex];
		// 设备列表的相机索引  最大表示字数：3
		// Camera index in device list, display in 3 characters 
		printf("%-3d", cameraIndex + 1);

		// 相机的设备类型（GigE，U3V，CL，PCIe）
		// Camera type 
		switch (pDevInfo->nCameraType)
		{
			case typeGigeCamera:printf(" GigE");break;
			case typeU3vCamera:printf(" U3V ");break;
			case typeCLCamera:printf(" CL  ");break;
			case typePCIeCamera:printf(" PCIe");break;
			default:printf("     ");break;
		}

		// 制造商信息  最大表示字数：10 
		// Camera vendor name, display in 10 characters 
		if (strlen(pDevInfo->vendorName) > 10)
		{
			memcpy(vendorNameCat, pDevInfo->vendorName, 7);
			vendorNameCat[7] = '\0';
			strcat(vendorNameCat, "...");
			printf(" %-10.10s", vendorNameCat);
		}
		else
		{
			printf(" %-10.10s", pDevInfo->vendorName);
		}

		// 相机的型号信息 最大表示字数：10 
		// Camera model name, display in 10 characters 
		printf(" %-10.10s", pDevInfo->modelName);

		// 相机的序列号 最大表示字数：15 
		// Camera serial number, display in 15 characters 
		printf(" %-15.15s", pDevInfo->serialNumber);

		// 自定义用户ID 最大表示字数：15 
		// Camera user id, display in 15 characters 
		if (strlen(pDevInfo->cameraName) > 15)
		{
			memcpy(cameraNameCat, pDevInfo->cameraName, 12);
			cameraNameCat[12] = '\0';
			strcat(cameraNameCat, "...");
			printf(" %-15.15s", cameraNameCat);
		}
		else
		{
			printf(" %-15.15s", pDevInfo->cameraName);
		}

		// GigE相机时获取IP地址 
		// IP address of GigE camera 
		if (pDevInfo->nCameraType == typeGigeCamera)
		{
			printf(" %s", pDevInfo->DeviceSpecificInfo.gigeDeviceInfo.ipAddress);
		}

		printf("\n");
	}

	return;
}

static char* trim(char* pStr)
{
	char* pDst = pStr;
	char* pTemStr = NULL;

	if (pDst != NULL)
	{
		pTemStr = pDst + strlen(pStr) - 1;
		while (*pDst == ' ')
		{
			pDst++;
		}
		while ((pTemStr > pDst) && (*pTemStr == ' '))
		{
			*pTemStr-- = '\0';
		}
	}
	return pDst;
}

static int isInputValid(char* pInpuStr)
{
	char numChar;
	char* pStr = pInpuStr;
	while (*pStr != '\0')
	{
		numChar = *pStr;
		if ((numChar > '9') || (numChar < '0'))
		{
			return -1;
		}
		pStr++;
	}
	return 0;
}

static unsigned int selectDevice(unsigned int cameraCnt)
{
	char inputStr[256];
	char* pTrimStr;
	int inputIndex = -1;
	int ret = -1;
	char* find = NULL;

	printf("\nPlease input the camera index: ");
	while (1)
	{
		memset(inputStr, 0, sizeof(inputStr));
		fgets(inputStr, sizeof(inputStr), stdin);

		// 清空输入缓存
		// clear flush
		fflush(stdin);

		// fgets比gets多吃一个换行符号，取出换行符号 
		// fgets eats one more line feed symbol than gets, and takes out the line feed symbol
		find = strchr(inputStr, '\n');
		if (find) { *find = '\0'; }

		pTrimStr = trim(inputStr);
		ret = isInputValid(pTrimStr);
		if (ret == 0)
		{
			inputIndex = atoi(pTrimStr);
			// 输入的序号从1开始 
			// Input index starts from 1 
			inputIndex -= 1;
			if ((inputIndex >= 0) && (inputIndex < (int)cameraCnt))
			{
				break;
			}
		}

		printf("Input invalid! Please input the camera index: ");
	}
	return (unsigned int)inputIndex;
}

// ***********结束： 这部分处理与SDK操作相机无关，用于显示设备列表 ***********
// ***********END: These functions are not related to API call and used to display device info***********
static int setSoftTriggerConf(void* libHandle, IMV_HANDLE devHandle)
{
	int ret = IMV_OK;
  // 获取设置触发源为软触发函数地址 
  DLL_SetEnumFeatureSymbol DLLSetEnumFeatureSymbol = (DLL_SetEnumFeatureSymbol)dlsym(libHandle, "IMV_SetEnumFeatureSymbol");
  if (NULL == DLLSetEnumFeatureSymbol)
  {
    printf("Get IMV_SetEnumFeatureSymbol address failed!\n");
    return ret;
  }

	// 设置触发源为软触发 
	// Set trigger source to Software 
	ret = DLLSetEnumFeatureSymbol(devHandle, "TriggerSource", "Software");
	if (IMV_OK != ret)
	{
		printf("Set triggerSource value failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 设置触发器 
	// Set trigger selector to FrameStart 
	ret = DLLSetEnumFeatureSymbol(devHandle, "TriggerSelector", "FrameStart");
	if (IMV_OK != ret)
	{
		printf("Set triggerSelector value failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 设置触发模式 
	// Set trigger mode to On 
	ret = DLLSetEnumFeatureSymbol(devHandle, "TriggerMode", "On");
	if (IMV_OK != ret)
	{
		printf("Set triggerMode value failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	return ret;
}

// Image convert
static int imageConvert(void* libHandle, IMV_HANDLE devHandle, IMV_Frame frame, IMV_EPixelType convertFormat)
{
	IMV_PixelConvertParam stPixelConvertParam;
	unsigned char*			pDstBuf = NULL;
	unsigned int			nDstBufSize = 0;
	int						ret = IMV_OK;
	FILE*					hFile = NULL;
	const char*				pFileName = NULL;
	const char*				pConvertFormatStr = NULL;

  // 获取设置触发源为软触发函数地址 
  DLL_PixelConvert DLLPixelConvert = (DLL_PixelConvert)dlsym(libHandle, "IMV_PixelConvert");
  if (NULL == DLLPixelConvert)
  {
    printf("Get IMV_PixelConvert address failed!\n");
    return ret;
  }

	switch (convertFormat)
	{
		case gvspPixelRGB8:
			nDstBufSize = sizeof(unsigned char) * frame.frameInfo.width * frame.frameInfo.height * 3;
			pFileName = (const char*)"convertRGB8.bmp";
			pConvertFormatStr = (const char*)"RGB8";
			break;

		case gvspPixelBGR8:
			nDstBufSize = sizeof(unsigned char) * frame.frameInfo.width * frame.frameInfo.height * 3;
			pFileName = (const char*)"convertBGR8.bmp";
			pConvertFormatStr = (const char*)"BGR8";
			break;
		case gvspPixelBGRA8:
			nDstBufSize = sizeof(unsigned char) * frame.frameInfo.width * frame.frameInfo.height * 4;
			pFileName = (const char*)"convertBGRA8.bmp";
			pConvertFormatStr = (const char*)"BGRA8";
			break;
		case gvspPixelMono8:
		default:
			nDstBufSize = sizeof(unsigned char) * frame.frameInfo.width * frame.frameInfo.height;
			pFileName = (const char*)"convertMono8.bmp";
			pConvertFormatStr = (const char*)"Mono8";
			break;
	}

	pDstBuf = (unsigned char*)malloc(nDstBufSize);
	if (NULL == pDstBuf)
	{
		printf("malloc pDstBuf failed!\n");
		return -1;
	}

	// 图像转换成BGR8
	// convert image to BGR8
	memset(&stPixelConvertParam, 0, sizeof(stPixelConvertParam));
	stPixelConvertParam.nWidth = frame.frameInfo.width;
	stPixelConvertParam.nHeight = frame.frameInfo.height;
	stPixelConvertParam.ePixelFormat = frame.frameInfo.pixelFormat;
	stPixelConvertParam.pSrcData = frame.pData;
	stPixelConvertParam.nSrcDataLen = frame.frameInfo.size;
	stPixelConvertParam.nPaddingX = frame.frameInfo.paddingX;
	stPixelConvertParam.nPaddingY = frame.frameInfo.paddingY;
	stPixelConvertParam.eBayerDemosaic = demosaicNearestNeighbor;
	stPixelConvertParam.eDstPixelFormat = convertFormat;
	stPixelConvertParam.pDstBuf = pDstBuf;
	stPixelConvertParam.nDstBufSize = nDstBufSize;


	ret = DLLPixelConvert(devHandle, &stPixelConvertParam);
	if (IMV_OK == ret)
	{
		printf("image convert to %s successfully! nDstDataLen (%u)\n", 
			pConvertFormatStr, stPixelConvertParam.nDstBufSize);

    cv::Mat im(stPixelConvertParam.nHeight, stPixelConvertParam.nWidth, CV_8UC3, stPixelConvertParam.pDstBuf);

	// // for test
	// // 内参矩阵
	// cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 11057.154, 0, 4538.85, 0, 11044.943, 3350.918, 0, 0, 1);
    // // 设置畸变系数
    // cv::Mat distCoeffs = (cv::Mat_<double>(1, 5) << 0.311583980, -14.5864013, -0.00630134677, -0.00466401902, 183.662957);
	// // 准备输出图像
    // cv::Mat undistortedImg;
    //  // 使用cv::undistort函数进行校正
    // cv::undistort(im, undistortedImg, cameraMatrix, distCoeffs);


    cv::imwrite("/home/caowei/catkin_ws/output.png", im);
    cv::imwrite("output.png", im);

		// hFile = fopen(pFileName, "wb");
		// if (hFile != NULL)
		// {
		// 	fwrite((void*)pDstBuf, 1, stPixelConvertParam.nDstBufSize, hFile);
		// 	fclose(hFile);
		// }
		// else
		// {
		// 	// 如果打开失败，请用root权限执行
		// 	// If opefailed, Run as root
		// 	printf("Open file (%s) failed!\n", pFileName);
		// }
	}
	else
	{
		printf("image convert to %s failed! ErrorCode[%d]\n", pConvertFormatStr, ret);
	}

	if (pDstBuf)
	{
		free(pDstBuf);
		pDstBuf = NULL;
	}

	return IMV_OK;
}

static void sendToRos(IMV_Frame frame)
{
	IMV_FlipImageParam		stFlipImageParam;
	unsigned int			nChannelNum = 0;
	int						ret = IMV_OK;
	FILE*					hFile = NULL;

	memset(&stFlipImageParam, 0, sizeof(stFlipImageParam));

  if (gvspPixelBGR8 == frame.frameInfo.pixelFormat)
	{
		stFlipImageParam.pSrcData = frame.pData;
		stFlipImageParam.nSrcDataLen = frame.frameInfo.width * frame.frameInfo.height * BGR_CHANNEL_NUM;
		stFlipImageParam.ePixelFormat = frame.frameInfo.pixelFormat;
		nChannelNum = BGR_CHANNEL_NUM;
	}
	else
	{
			printf("image convert to BGR8 failed! ErrorCode[%d]\n", ret);
	}

  // 向ros发送/image消息
	do
	{
    
	} while (false);

}


int main()
{
	int ret = IMV_OK;
	unsigned int cameraIndex = 0;
	IMV_HANDLE devHandle = NULL;
	void* libHandle = NULL;
	DLL_DestroyHandle DLLDestroyHandle = NULL;
  IMV_Frame frame;

  // 加载SDK库  
  // Load SDK library
  libHandle = dlopen("libMVSDK.so", RTLD_LAZY);
  if (NULL == libHandle)
  {
    printf("Load MVSDKmd.dll library failed!\n");
    return 0;
  }

  // 获取发现设备接口函数地址 
  // Get discover camera interface address
  DLL_EnumDevices DLLEnumDevices = (DLL_EnumDevices)dlsym(libHandle, "IMV_EnumDevices");
  if (NULL == DLLEnumDevices)
  {
    printf("Get IMV_EnumDevices address failed!\n");
    return 0;
  }

  // 获取创建设备句柄接口函数地址 
  // Get create Device Handle interface address
  DLL_CreateHandle DLLCreateHandle = (DLL_CreateHandle)dlsym(libHandle, "IMV_CreateHandle");
  if (NULL == DLLCreateHandle)
  {
    printf("Get IMV_CreateHandle address failed!\n");
    return 0;
  }

  // 获取销毁设备句柄接口函数地址 
  // Get destroy Device Handle interface address
  DLLDestroyHandle = (DLL_DestroyHandle)dlsym(libHandle, "IMV_DestroyHandle");
  if (NULL == DLLDestroyHandle)
  {
    printf("Get IMV_DestroyHandle address failed!\n");
    return 0;
  }

  // 获取打开相机接口函数地址 
  // Get open camera interface address
  DLL_Open DLLOpen = (DLL_Open)dlsym(libHandle, "IMV_Open");
  if (NULL == DLLOpen)
  {
    printf("Get IMV_Open address failed!\n");
    return 0;
  }

  // 获取注册数据帧回调接口函数地址 
  // Get register data frame callback interface address
  DLL_AttachGrabbing DLLAttachGrabbing = (DLL_AttachGrabbing)dlsym(libHandle, "IMV_AttachGrabbing");
  if (NULL == DLLAttachGrabbing)
  {
    printf("Get IMV_AttachGrabbing address failed!\n");
    return 0;
  }

  // 获取开始拉流接口函数地址 
  // Get start grabbing interface address
  DLL_StartGrabbing DLLStartGrabbing = (DLL_StartGrabbing)dlsym(libHandle, "IMV_StartGrabbing");
  if (NULL == DLLStartGrabbing)
  {
    printf("Get IMV_StartGrabbing address failed!\n");
    return 0;
  }

  // 获取停止拉流接口函数地址 
  // Get stop grabbing interface address
  DLL_StopGrabbing DLLStopGrabbing = (DLL_StopGrabbing)dlsym(libHandle, "IMV_StopGrabbing");
  if (NULL == DLLStopGrabbing)
  {
    printf("Get IMV_StopGrabbing address failed!\n");
    return 0;
  }

  // 获取

  // 获取关闭相机接口函数地址 
  // Get close camera interface address
  DLL_Close DLLClose = (DLL_Close)dlsym(libHandle, "IMV_Close");
  if (NULL == DLLClose)
  {
    printf("Get IMV_Close address failed!\n");
    return 0;
  }

  // 获取获取一帧图像函数地址 
  DLL_GetFrame DLLGetFrame = (DLL_GetFrame)dlsym(libHandle, "IMV_GetFrame");
  if (NULL == DLLGetFrame)
  {
    printf("Get IMV_GetFrame address failed!\n");
    return 0;
  }

  DLL_ReleaseFrame DLLReleaseFrame = (DLL_ReleaseFrame)dlsym(libHandle, "IMV_ReleaseFrame");
  if (NULL == DLLReleaseFrame)
  {
    printf("Get IMV_ReleaseFrame address failed!\n");
    return 0;
  }

  DLL_ClearFrameBuffer DLLClearFrameBuffer = (DLL_ClearFrameBuffer)dlsym(libHandle, "IMV_ClearFrameBuffer");
  if (NULL == DLLClearFrameBuffer)
  {
    printf("Get IMV_ClearFrameBuffer address failed!\n");
    return 0;
  }

  DLL_ExecuteCommandFeature DLLExecuteCommandFeature = (DLL_ExecuteCommandFeature)dlsym(libHandle, "IMV_ExecuteCommandFeature");
  if (NULL == DLLExecuteCommandFeature)
  {
    printf("Get IMV_ExecuteCommandFeature address failed!\n");
    return 0;
  }

  DLL_SetIntFeatureValue DLLSetIntFeatureValue = (DLL_SetIntFeatureValue)dlsym(libHandle, "IMV_SetIntFeatureValue");
  if (NULL == DLLSetIntFeatureValue)
  {
    printf("Get IMV_SetIntFeatureValue address failed!\n");
    return 0;
  }

  DLL_SetDoubleFeatureValue DLLSetDoubleFeatureValue = (DLL_SetDoubleFeatureValue)dlsym(libHandle, "IMV_SetDoubleFeatureValue");
  if (NULL == DLLSetDoubleFeatureValue)
  {
    printf("Get IMV_SetDoubleFeatureValue address failed!\n");
    return 0;
  }

  

////////////////////// 检查接口结束
		// 发现设备 
		// discover camera 
  IMV_DeviceList deviceInfoList;
  ret = DLLEnumDevices(&deviceInfoList, interfaceTypeAll);
  if (IMV_OK != ret)
  {
    printf("Enumeration devices failed! ErrorCode[%d]\n", ret);
    return 0;
  }

  if (deviceInfoList.nDevNum < 1)
  {
    printf("no camera\n");
    return 0;
  }

  // 打印相机基本信息（序号,类型,制造商信息,型号,序列号,用户自定义ID,IP地址） 
  // Print camera info (Index, Type, Vendor, Model, Serial number, DeviceUserID, IP Address) 

  displayDeviceInfo(deviceInfoList);
  // 选择需要连接的相机 
  // Select one camera to connect to  
  // cameraIndex = selectDevice(deviceInfoList.nDevNum);
  cameraIndex = 0; // 第一个相机

  // 创建设备句柄
  // Create Device Handle
  ret = DLLCreateHandle(&devHandle, modeByIndex, (void*)&cameraIndex);
  if (IMV_OK != ret)
  {
    printf("Create devHandle failed! ErrorCode[%d]\n", ret);
    return 0;
  }

  // 打开相机 
  ret = DLLOpen(devHandle);
  if (IMV_OK != ret)
  {
    printf("Open camera failed! ErrorCode[%d]\n", ret);
    return 0;
  }

  // 设置软触发模式
  ret = setSoftTriggerConf(libHandle, devHandle);
  if (IMV_OK != ret)
  {
    printf("setSoftTriggerConf failed! ErrorCode[%d]\n", ret);
    return 0;
  }

  ret = DLLSetIntFeatureValue(devHandle, "Width", 9344);
	if (IMV_OK != ret)
	{
		printf("Set feature value Width failed! ErrorCode[%d]\n", ret);
		return ret;
	}

  ret = DLLSetIntFeatureValue(devHandle, "Height", 7000);
	if (IMV_OK != ret)
	{
		printf("Set feature value Height failed! ErrorCode[%d]\n", ret);
		return ret;
	}

  ret = DLLSetIntFeatureValue(devHandle, "OffsetX", 0);
	if (IMV_OK != ret)
	{
		printf("Set feature value OffsetX failed! ErrorCode[%d]\n", ret);
		return ret;
	}

  ret = DLLSetIntFeatureValue(devHandle, "OffsetY", 0);
	if (IMV_OK != ret)
	{
		printf("Set feature value OffsetY failed! ErrorCode[%d]\n", ret);
		return ret;
	}

  // 设置属性值曝光
	// Set feature value 
	ret = DLLSetDoubleFeatureValue(devHandle, "ExposureTime", 12*10000);
	if (IMV_OK != ret)
	{
		printf("Set feature value failed! ErrorCode[%d]\n", ret);
		return ret;
	}

  // // 非多线程，不需要注册回调
  // // 注册数据帧回调函数
  // // Register data frame callback function
  // ret = DLLAttachGrabbing(devHandle, onGetFrame, NULL);
  // if (IMV_OK != ret)
  // {
  //   printf("Attach grabbing failed! ErrorCode[%d]\n", ret);
  //   return 0;
  // }

  // 开始拉流 
  // Start grabbing 
  ret = DLLStartGrabbing(devHandle);
  if (IMV_OK != ret)
  {
    printf("Start grabbing failed! ErrorCode[%d]\n", ret);
    return 0;
  }

  /////////////////////////获取一帧图像////////////////////////////

  // 清除帧数据缓存
	// Clear frame buffer
	ret = DLLClearFrameBuffer(devHandle);
	if (IMV_OK != ret)
	{
		printf("Clear frame buffer failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 执行软触发
	// Execute soft trigger 
	ret = DLLExecuteCommandFeature(devHandle, "TriggerSoftware");
	if (IMV_OK != ret)
	{
		printf("Execute TriggerSoftware failed! ErrorCode[%d]\n", ret);
		return ret;
	}

sleep(2000);
  // 获取一帧图像, TIMEOUT 5000ms
  ret = DLLGetFrame(devHandle, &frame, 5000);
  if (IMV_OK != ret)
  {
    printf("Get frame failed! ErrorCode[%d]\n", ret);
    return 0;
  }

  printf("width %d\n", frame.frameInfo.width);
  printf("Height %d\n", frame.frameInfo.height);

  ret = imageConvert(libHandle, devHandle, frame, gvspPixelBGR8);
  if (IMV_OK != ret)
  {
    printf("imageConvert failed! ErrorCode[%d]\n", ret);
    return 0;
  }
  // printf("11111111111111\n", ret);
  // cv::Mat im(frame.frameInfo.width, frame.frameInfo.height, CV_8UC3, frame.pData);
  // cv::imwrite("/home/caowei/catkin_ws/output.png", im);
  // cv::imwrite("output.png", im);
  // printf("22222222222222\n", ret);
  // // 向ros送 /image消息
  // sendToRos(frame);

  // 释放图像缓存
  ret = DLLReleaseFrame(devHandle, &frame);
  if (IMV_OK != ret)
  {
    printf("Release frame failed! ErrorCode[%d]\n", ret);
    return 0;
  }
  ////////////////////////////////////////////////////////////////

  // // 取图2秒
  // // get frame 2 seconds
  // sleep(2000);

  // 停止拉流 
  // Stop grabbing 
  ret = DLLStopGrabbing(devHandle);
  if (IMV_OK != ret)
  {
    printf("Stop grabbing failed! ErrorCode[%d]\n", ret);
    return 0;
  }

  // 关闭相机
  // Close camera 
  ret = DLLClose(devHandle);
  if (IMV_OK != ret)
  {
    printf("Close camera failed! ErrorCode[%d]\n", ret);
    return 0;
  }

	// 销毁设备句柄
	// Destroy Device Handle
	if (NULL != devHandle)
	{
		// 销毁设备句柄
		// Destroy Device Handle
		DLLDestroyHandle(devHandle);
	}

	if (NULL != libHandle)
	{
		dlclose(libHandle);
	}

	printf("end...\n");
	// getchar();

	return 0;
}
  
}