smokeboxidentification/src/LXCamera.cpp

#include "LXCamera.h"

#include <iostream>
#include <thread>
#include <sstream>
#include "lxCamera/lx_camera_api.h"

#pragma comment(lib,"LxCameraApi.lib")


static char wait_key = '0';
static DcHandle handle = 0;
#define checkTC(state) {LX_STATE val=state;                            \
    if(val != LX_SUCCESS){                                             \
        if(val == LX_E_RECONNECTING){                                  \
			qDebug()<< " device reconnecting";}                        \
        else if(val == LX_E_NOT_SUPPORT){                              \
            qDebug()<< " not support";}                                \
        else{                                                          \
            qDebug()<< DcGetErrorString(val);                          \
            DcCloseDevice(handle);                                     \
        }                                                              \
    }                                                                  \
}


LXCamera::~LXCamera()
{
	destroyCamera();
}

bool LXCamera::initCamera(const QString& serialNumber)
{
	int device_num = 0;
	LxDeviceInfo* p_device_list = NULL;
	checkTC(DcGetDeviceList(&p_device_list, &device_num));
	if (device_num <= 0)
	{
		qDebug() << "Not found any tof device.";
		return false;
	}
	m_handle = serialNumber.toStdString();
	qDebug() << "Find tof device: " << p_device_list;
	return true;
}

void LXCamera::destroyCamera()
{
	stopCamera();
	closeCamera();
}

bool LXCamera::openCamera()
{
	LxDeviceInfo device_info;
	LX_STATE lx_state = DcOpenDevice(LX_OPEN_MODE::OPEN_BY_ID, m_handle.c_str(), &handle, &device_info);
	if (LX_SUCCESS != lx_state) {
		QString str = QString("open device failed, open_param: %1").arg(m_handle.c_str());
		qDebug() << str;
		return false;
	}
	return true;
}

bool LXCamera::closeCamera()
{
	return DcCloseDevice(handle);
}

bool LXCamera::startCamera()
{
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	bool test_depth = false, test_rgb = false;
	checkTC(DcGetBoolValue(handle, LX_BOOL_ENABLE_3D_DEPTH_STREAM, &test_depth));
	checkTC(DcGetBoolValue(handle, LX_BOOL_ENABLE_2D_STREAM, &test_rgb));

	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	checkTC(DcStartStream(handle));

	std::thread pthread = std::thread();
	//pthread.detach();

	//auto _time = std::chrono::system_clock::now();

	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	auto ret = DcSetCmd(handle, LX_CMD_GET_NEW_FRAME);

	if (LX_SUCCESS != ret)
	{
		if (LX_E_RECONNECTING == ret) {
			std::cout << "device reconnecting" << std::endl;
		}
		std::this_thread::sleep_for(std::chrono::seconds(1));
		qDebug() << "TofCamera Open Failed!";
		return false;
	}

	qDebug() << "TofCamera Open Succeed!";
	return true;
}

bool LXCamera::stopCamera()
{
	return DcStopStream(handle);
}

bool LXCamera::setTriggerSource(int mode)
{
	return true;
}

bool LXCamera::takeAPic(cv::Mat& imgMat, cv::Mat&imgMat3D)
{
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	auto ret = DcSetCmd(handle, LX_CMD_GET_NEW_FRAME);
	if (LX_SUCCESS != ret)
	{
		if (LX_E_RECONNECTING == ret) {
			qDebug() << "device reconnecting";
		}
		std::this_thread::sleep_for(std::chrono::seconds(1));
		//continue;
	}

	FrameInfo* frame_ptr = nullptr;
	checkTC(DcGetPtrValue(handle, LX_PTR_FRAME_DATA, (void**)&frame_ptr));
	if (!frame_ptr || frame_ptr->frame_state != LX_SUCCESS) {
		qDebug() << "new frame not correct";
		return false;
	}

	//Depth
	if (frame_ptr->depth_data.frame_data != nullptr)
	{
		auto depth_data = frame_ptr->depth_data;
		cv::Mat depth_image = cv::Mat(depth_data.frame_height, depth_data.frame_width,
			CV_MAKETYPE(depth_data.frame_data_type, depth_data.frame_channel), depth_data.frame_data);
		//cv::Mat show;
		depth_image.convertTo(imgMat, CV_8U, 1.0 / 16);
		//applyColorMap(show, show, COLORMAP_JET);
		//cv::namedWindow("depth", 0);
		//cv::resizeWindow("depth", 640, 480);
		//cv::imshow("depth", show);
	}

	//RGB
	if (frame_ptr->rgb_data.frame_data != nullptr)
	{
		auto rgb_data = frame_ptr->rgb_data;
		imgMat3D = cv::Mat(rgb_data.frame_height, rgb_data.frame_width,
			CV_MAKETYPE(rgb_data.frame_data_type, rgb_data.frame_channel), rgb_data.frame_data);

		//cv::namedWindow("rgb", 0);
		//cv::resizeWindow("rgb", 640, 480);
		//cv::imshow("rgb", rgb_image);
	}
}