LidarMeasure/main.cpp

//
// Created by zx on 2019/12/28.
//
#include <iostream>

#include "./laser/Laser.h"
#include "./laser/LivoxLaser.h"
#include "./error_code/error_code.h"
#include "LogFiles.h"
#include <string.h>
#include <fcntl.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
//#include </usr/local/include/google/protobuf/io/zero_copy_stream_impl.h>
using google::protobuf::io::FileInputStream;
using google::protobuf::io::FileOutputStream;
using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::ZeroCopyOutputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::Message;

#define O_RDONLY	     00

#define LIVOX_NUMBER	     2

//读取protobuf 配置文件
//file:文件
//parameter:要读取的配置
bool read_proto_param(std::string file, ::google::protobuf::Message& parameter)
{
	int fd = open(file.c_str(), O_RDONLY);
	if (fd == -1) return false;
	FileInputStream* input = new FileInputStream(fd);
	bool success = google::protobuf::TextFormat::Parse(input, &parameter);
	delete input;
	close(fd);
	return success;
}



int main(int argc,char* argv[])
{


	std::cout << "huli 101 main start!" << std::endl;


	Laser_base * m_laser_vector_array[LIVOX_NUMBER];
	Error_manager err;

	for (int i = 0; i < LIVOX_NUMBER; ++i)
	{
		Laser_base *m_laser_vector;

		Laser_proto::laser_parameter laser_param;
		laser_param.set_frame_num(1000);
		laser_param.set_type("Livox");
		if ( i==0 )
		{
			laser_param.set_sn("0TFDFG700601881");
		}
		else if ( i==1 )
		{
			laser_param.set_sn("0TFDFCE00502001");
		}

//	int i = 0;
		m_laser_vector_array[i] = m_laser_vector = LaserRegistory::CreateLaser(laser_param.type(), i,
																			   laser_param);


		if (m_laser_vector != NULL)
		{
			err = m_laser_vector->connect_laser();
			if ( err != Error_code::SUCCESS)
			{
				char description[255] = {0};
				sprintf(description, "Laser %d connect failed...", i);
				err.error_manager_reset(LASER_CONNECT_FAILED, NORMAL, description);

			}
			else
			{
				std::cout << "huli 301 connect_laser Error_code::SUCCESS" << std::endl;
			}

			cout << "huli 311 " << err.to_string() << endl;




		}
		else
		{
			cout << "huli: 601 :" << err.to_string() << endl;

		}

	}


	if (!Start()) {
		Uninit();
		printf("Livox-SDK init fail!\n");
	}
	else
	{
		std::cout << "huli 201 Livox-SDK start" << std::endl;
	}



	while(1)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud(new pcl::PointCloud<pcl::PointXYZ>);
		std::mutex cloud_lock;

		Laser_task *laser_task = new Laser_task();
		//
		laser_task->task_init(TASK_CREATED, scan_cloud, &cloud_lock);
		laser_task->set_task_frame_maxnum(1000);

//		//保存文件初始化
//		char time_string[256] = { 0 };
//		sprintf(time_string, "../data/%d/", 0);
//
//		laser_task->set_task_save_path(time_string);

		std::cout << "huli 401 connect_laser Error_code::SUCCESS" << std::endl;

		std::cout << " press to start" << std::endl;
		char ch ;
//		= getchar();
		std::cin >> ch ;
		if ( ch == 'b' )
		{
			std::cout << "  end scan ------------" << std::endl;
			break;
		}
		std::cout << "  start scan ------------" << std::endl;

//		err = m_laser_vector_array[0]->execute_task(laser_task);
//		usleep(3000 * 1000);
//		cout << "huli: 501 :" << err.to_string() << endl;
		int n = 5;
		while(n)
		{
			n--;

			for (int i = 0; i < LIVOX_NUMBER; ++i)
			{
				char time_string[256] = { 0 };
				sprintf(time_string, "../data/%d/", i);
				laser_task->set_task_save_path(time_string);
				laser_task->set_task_save_flag(true);

				err = m_laser_vector_array[i]->execute_task(laser_task);
//				usleep(3000 * 1000);
			}

			usleep(3000 * 1000);
			cout << "huli: 501 :" << err.to_string() << endl;
			cout << "huli: 501 :" << err.to_string() << endl;
		}

		cout << "huli: 601 :" << err.to_string() << endl;
		//发送任务单给雷达
		std::this_thread::sleep_for(std::chrono::seconds(5));

	}




	for (int i = 0; i < LIVOX_NUMBER; ++i)
	{
		std::this_thread::sleep_for(std::chrono::seconds(5));
		cout << "huli: 701 :" << err.to_string() << endl;

		m_laser_vector_array[i]->disconnect_laser();
		cout << "huli: 801 :" << err.to_string() << endl;

		m_laser_vector_array[i]->close_save_path();
		cout << "huli: 901 :" << err.to_string() << endl;

		std::this_thread::sleep_for(std::chrono::seconds(2));

	}

	Uninit();

	cout << "huli 1234 main end" << endl;
}