puai_wj_2021/main.cpp

//
// Created by zx on 2020/6/18.
//

#include <iostream>
#include "./error_code/error_code.h"
#include "LogFiles.h"
#include <glog/logging.h>

#include "./laser/laser_manager.h"
#include "./laser/Laser.h"
#include "./laser/LivoxLaser.h"

#include "./locate/locate_manager.h"

#include "./communication/communication_socket_base.h"

#include "./tool/thread_pool.h"
#include "./system/system_communication.h"
#include "./system/system_executor.h"

#include "./wanji_lidar/wanji_lidar_device.h"
#include "./tool/proto_tool.h"
#include "./wanji_lidar/wanji_manager.h"

#define LIVOX_NUMBER	     2

GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
{
	time_t tt;
	time( &tt );
	tt = tt + 8*3600;  // transform the time zone
	tm* t= gmtime( &tt );
	char buf[255]={0};
	sprintf(buf,"./%d%02d%02d-%02d%02d%02d-dump.txt",
			t->tm_year + 1900,
			t->tm_mon + 1,
			t->tm_mday,
			t->tm_hour,
			t->tm_min,
			t->tm_sec);

	FILE* tp_file=fopen(buf,"w");
	fprintf(tp_file,data,strlen(data));
	fclose(tp_file);

}

void prev_test_pred_task()
{
	char ch;
	while (1)
	{
		std::cin >> ch;

		if (ch == 'b')
		{
			break;
		}

		//第一步, 创建点云缓存, 和 指定雷达,目前就一个
		std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> point_cloud_map;
		std::mutex cloud_lock;

		pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud(new pcl::PointCloud<pcl::PointXYZ>);

		std::ifstream is;
		is.open("/home/huli/huli/Terminal_project/hl_measure/Terminal_process_CT/test_data/car_cloud/20200907-084334_src - Cloud.txt", std::ios::out);
		if(!is.is_open())
			continue;
		char t_buf[256];
		while (!is.eof())
		{
			is.getline(t_buf, 256);
			pcl::PointXYZ t_point;
			sscanf(t_buf, "%f %f %f", &t_point.x, &t_point.y, &t_point.z);
			t_point.x = t_point.x / 1000.0;
			t_point.y = t_point.y / 1000.0;
			t_point.z = t_point.z / 1000.0;

			p_cloud->push_back(t_point);
		}
		std::cout << " huli test :::: "
				  << " p_cloud->size() = " << p_cloud->size() << std::endl;
		point_cloud_map[0] = p_cloud;

		Locate_manager_task locate_manager_task;
		Common_data::Car_measure_information t_car_information_by_livox;

		//第五步, 创建定位模块的任务单, 并发送到 Locate_manager
		locate_manager_task.task_init(TASK_CREATED, "", NULL, std::chrono::milliseconds(15000),
									  true, "/home/huli/huli/Terminal_project/hl_measure/Terminal_process_CT/data",
									  &cloud_lock, &point_cloud_map, true);
		Error_manager t_error = Task_command_manager::get_instance_references().execute_task(&locate_manager_task);

		//第六步, 等待任务单完成
		if (t_error != Error_code::SUCCESS)
		{
			LOG(INFO) << " Locate_manager  execute_task error ";
		}
		else
		{
			//判断任务是否结束, 阻塞 等待, 直到任务完成或者超时
			while (locate_manager_task.is_task_end() == false)
			{
				if (locate_manager_task.is_over_time())
				{
					//超时处理。取消任务。
					Locate_manager::get_instance_pointer()->cancel_task();
					locate_manager_task.set_task_statu(TASK_DEAD);
					t_error.error_manager_reset(Error_code::LOCATER_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
												" locate_manager_task is_over_time ");
					locate_manager_task.set_task_error_manager(t_error);
				}
				else
				{
					//继续等待
					std::this_thread::sleep_for(std::chrono::microseconds(1));
					std::this_thread::yield();
				}
			}
			//提取任务单 的错误码
			t_error = locate_manager_task.get_task_error_manager();
			if (t_error == Error_code::SUCCESS)
			{
				t_car_information_by_livox.center_x = locate_manager_task.get_task_locate_information_ex()->locate_x;
				t_car_information_by_livox.center_y = locate_manager_task.get_task_locate_information_ex()->locate_y;
				t_car_information_by_livox.car_angle = locate_manager_task.get_task_locate_information_ex()->locate_angle;
				t_car_information_by_livox.car_length = locate_manager_task.get_task_locate_information_ex()->locate_length;
				t_car_information_by_livox.car_width = locate_manager_task.get_task_locate_information_ex()->locate_width;
				t_car_information_by_livox.car_height = locate_manager_task.get_task_locate_information_ex()->locate_height;
				t_car_information_by_livox.wheel_base = locate_manager_task.get_task_locate_information_ex()->locate_wheel_base;
				t_car_information_by_livox.wheel_width = locate_manager_task.get_task_locate_information_ex()->locate_wheel_width;
				t_car_information_by_livox.front_theta = 0;
				t_car_information_by_livox.correctness = true;

				std::cout << " huli test :::: "
						  << " t_car_information_by_livox.center_x = " << t_car_information_by_livox.center_x << std::endl;
				std::cout << " huli test :::: "
						  << " t_car_information_by_livox.center_y = " << t_car_information_by_livox.center_y << std::endl;
				std::cout << " huli test :::: "
						  << " t_car_information_by_livox.car_angle = " << t_car_information_by_livox.car_angle << std::endl;
				std::cout << " huli test :::: "
						  << " t_car_information_by_livox.car_length = " << t_car_information_by_livox.car_length << std::endl;
				std::cout << " huli test :::: "
						  << " t_car_information_by_livox.car_width = " << t_car_information_by_livox.car_width << std::endl;
				std::cout << " huli test :::: "
						  << " t_car_information_by_livox.car_height = " << t_car_information_by_livox.car_height << std::endl;
				std::cout << " huli test :::: "
						  << " t_car_information_by_livox.wheel_base = " << t_car_information_by_livox.wheel_base << std::endl;
				std::cout << " huli test :::: "
						  << " t_car_information_by_livox.wheel_width = " << t_car_information_by_livox.wheel_width << std::endl;
				std::cout << " huli test :::: "
						  << " t_car_information_by_livox.front_theta = " << t_car_information_by_livox.front_theta << std::endl;
				std::cout << " huli test :::: "
						  << " t_car_information_by_livox.correctness = " << t_car_information_by_livox.correctness << std::endl;
			}
			else
			{
				LOG(INFO) << " locate_manager_task error :::::::" << locate_manager_task.get_task_error_manager().to_string();
			}
		}
	}
}

void prev_test_sample_pred_process()
{
	std::cout << "sample pred process" << std::endl;
	Error_manager t_error;
	Point_sift_segmentation *mp_point_sift;
	int point_size = 8192;
	int cls_num = 3;
	double freq = 20.0;
	std::string graph = "../model_param/seg_model_404500.ckpt.meta";
	std::string cpkt = "../model_param/seg_model_404500.ckpt";
	pcl::PointXYZ minp(-10000, -10000, -10000);
	pcl::PointXYZ maxp(10000, 10000, 10000);

	while (1)
	{
		std::cout << "huli 401 connect_laser Error_code::SUCCESS" << std::endl;

		std::cout << " press to start" << std::endl;
		char ch;
		//		= getchar();

		std::this_thread::sleep_for(std::chrono::seconds(2));
		std::cin >> ch;

		if (ch == 'b')
		{
			std::cout << "  end scan ------------" << std::endl;
			break;
		}
		std::cout << "  start scan ------------" << std::endl;

		int n = 1;
		while (n)
		{
			n--;

			std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> point_cloud_map;
			std::mutex cloud_lock;
			std::vector<int> select_laser_id_vector;
			select_laser_id_vector.push_back(0);
			select_laser_id_vector.push_back(1);
			select_laser_id_vector.push_back(2);
			select_laser_id_vector.push_back(3);

			time_t nowTime;
			nowTime = time(NULL);
			struct tm *sysTime = localtime(&nowTime);
			char t_save_path[256] = {0};
			sprintf(t_save_path, "../data/%04d%02d%02d_%02d%02d%02d",
					sysTime->tm_year + 1900, sysTime->tm_mon + 1, sysTime->tm_mday, sysTime->tm_hour, sysTime->tm_min, sysTime->tm_sec);

			Laser_manager_task *laser_manager_task = new Laser_manager_task;
			laser_manager_task->task_init(TASK_CREATED, "", NULL, std::chrono::milliseconds(15000),
										  true, t_save_path, &cloud_lock, &point_cloud_map, false,
										  1000, false, select_laser_id_vector);

			t_error = Task_command_manager::get_instance_references().execute_task(laser_manager_task);
			//			tp_laser_manager->execute_task(laser_manager_task);

			if (t_error != Error_code::SUCCESS)
			{
				std::cout << "huli Laser_manager:::::" << t_error.to_string() << std::endl;
			}
			else
			{
				while (laser_manager_task->is_task_end() == false)
				{
					if (laser_manager_task->is_over_time())
					{
						//超时处理。取消任务。
						Laser_manager::get_instance_pointer()->cancel_task();
						laser_manager_task->set_task_statu(TASK_DEAD);
						t_error.error_manager_reset(Error_code::LASER_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
													" laser_manager_task is_over_time ");
						laser_manager_task->set_task_error_manager(t_error);
					}
					else
					{
						std::this_thread::yield();
					}
				}
				std::cout << "huli task_error::::" << laser_manager_task->get_task_error_manager().to_string() << std::endl;
			}
			delete (laser_manager_task);

			std::cout << "huli laser result---------------------" << t_error.to_string() << std::endl;

			/*
			pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud ;
			scan_cloud =  point_cloud_map[0];

			//locate
			pcl::getMinMax3D(*scan_cloud,minp,maxp);
			std::cout << "huli 112" << std::endl;
			mp_point_sift = new Point_sift_segmentation(point_size,cls_num,freq,minp,maxp);
			std::cout << t_error.to_string() << std::endl;

			t_error = mp_point_sift->init(graph,cpkt);
			std::cout << t_error.to_string() << std::endl;

			std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> segmentation_clouds;
			t_error = mp_point_sift->segmentation(scan_cloud, segmentation_clouds, true, "../data/locate_data");
			std::cout << t_error.to_string() << std::endl;
			*/

			Locate_manager_task *locate_manager_task = new Locate_manager_task;
			locate_manager_task->task_init(TASK_CREATED, "", NULL, std::chrono::milliseconds(15000),
										   true, t_save_path, &cloud_lock, &point_cloud_map, false);
			t_error = Task_command_manager::get_instance_references().execute_task(locate_manager_task);

			if (t_error != Error_code::SUCCESS)
			{
				std::cout << "huli Locate_manager:::::" << t_error.to_string() << std::endl;
			}
			else
			{
				while (locate_manager_task->is_task_end() == false)
				{
					if (locate_manager_task->is_over_time())
					{
						//超时处理。取消任务。
						Locate_manager::get_instance_pointer()->cancel_task();
						locate_manager_task->set_task_statu(TASK_DEAD);
						t_error.error_manager_reset(Error_code::LOCATER_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
													" locate_manager_task is_over_time ");
						locate_manager_task->set_task_error_manager(t_error);
					}
					else
					{
						std::this_thread::yield();
					}
				}

				std::cout << "huli task_error:::::" << locate_manager_task->get_task_error_manager().to_string() << std::endl;

				std::cout << "locate_x = " << locate_manager_task->get_task_locate_information_ex()->locate_x << std::endl;
				std::cout << "locate_y = " << locate_manager_task->get_task_locate_information_ex()->locate_y << std::endl;
				std::cout << "locate_length = " << locate_manager_task->get_task_locate_information_ex()->locate_length << std::endl;
				std::cout << "locate_width = " << locate_manager_task->get_task_locate_information_ex()->locate_width << std::endl;
				std::cout << "locate_height = " << locate_manager_task->get_task_locate_information_ex()->locate_height << std::endl;
				std::cout << "locate_wheel_base = " << locate_manager_task->get_task_locate_information_ex()->locate_wheel_base << std::endl;
				std::cout << "locate_wheel_width = " << locate_manager_task->get_task_locate_information_ex()->locate_wheel_width << std::endl;
				std::cout << "locate_angle = " << locate_manager_task->get_task_locate_information_ex()->locate_angle << std::endl;
				std::cout << "locate_correct = " << locate_manager_task->get_task_locate_information_ex()->locate_correct << std::endl;
			}
			delete (locate_manager_task);
			std::cout << "huli locate result:::::" << t_error.to_string() << std::endl;
		}

		std::cout << "err code :" << t_error.to_string() << std::endl;
		std::this_thread::sleep_for(std::chrono::seconds(2));
	}
}

void test_whole_process()
{
	System_executor::get_instance_references().execute_for_measure("key_for_test_only", 5, std::chrono::system_clock::now());
}

int main(int argc,char* argv[])
{
	Error_manager t_error;
	Error_manager t_result ;


	const char* logPath = "./";
	google::InitGoogleLogging("LidarMeasurement");
	google::SetStderrLogging(google::INFO);
	google::SetLogDestination(0, logPath);
	google::SetLogFilenameExtension("zxlog");
	google::InstallFailureSignalHandler();
	google::InstallFailureWriter(&shut_down_logging);
	FLAGS_colorlogtostderr = true;        // Set log color
	FLAGS_logbufsecs = 0;                // Set log output speed(s)
	FLAGS_max_log_size = 1024;            // Set max log file size(GB)
	FLAGS_stop_logging_if_full_disk = true;

	// Wanji_manager::get_instance_references().wanji_manager_init();
	// std::cout << " huli test :::: " << " wanji_manager_init = " << 1 << std::endl;

	// 定义服务的终端id
	int t_terminal_id = 0;
	if ( argc == 2 )
	{
		std::cout << " huli test :::: " << " argv[1] = " << argv[1] << std::endl;
		t_terminal_id = atoi(argv[1]);
	}
	std::cout << " huli test :::: " << " t_terminal_id = " << t_terminal_id << std::endl;

	// 初始化
	Laser_manager::get_instance_references().laser_manager_init();
	std::cout << "Laser_manager = " << Laser_manager::get_instance_references().get_laser_manager_status() << std::endl;
	Locate_manager::get_instance_references().Locate_manager_init();
	std::cout << "Locate_manager = " << Locate_manager::get_instance_references().get_locate_manager_status() << std::endl;
	System_executor::get_instance_references().system_executor_init(4, t_terminal_id);
	std::cout << "System_executor = " << System_executor::get_instance_references().get_system_executor_status() << std::endl;

	System_communication::get_instance_references().communication_init();

	// prev_test_pred_task();
	test_whole_process();

	// 反初始化
	System_communication::get_instance_references().communication_uninit();
	System_executor::get_instance_references().system_executor_uninit();
	Locate_manager::get_instance_references().Locate_manager_uninit();
	Laser_manager::get_instance_references().laser_manager_uninit();

	return 0;
}