CT_project/测量节点/wanji_lidar/wanji_manager.cpp

//
// Created by huli on 2020/9/9.
//

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

Wanji_manager::Wanji_manager()
{
	m_wanji_manager_status = E_UNKNOWN;

	// mp_cloud_collection = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
	m_cloud_update_time = std::chrono::system_clock::now() - std::chrono::hours(1);
	mp_collect_cloud_thread = NULL;

	mp_execute_thread = NULL;
	mp_wanji_manager_task = NULL;
}
Wanji_manager::~Wanji_manager()
{
	wanji_manager_uninit();
}


//初始化 雷达 管理模块。如下三选一
Error_manager Wanji_manager::wanji_manager_init(int terminal_id)
{
	return  wanji_manager_init_from_protobuf(WANJI_MANAGER_PARAMETER_PATH, terminal_id);
}
//初始化 雷达 管理模块。从文件读取
Error_manager Wanji_manager::wanji_manager_init_from_protobuf(std::string prototxt_path, int terminal_id)
{
	wj::wjManagerParams t_wanji_parameters;

	if(!  proto_tool::read_proto_param(prototxt_path,t_wanji_parameters) )
	{
		return Error_manager(WJ_MANAGER_READ_PROTOBUF_ERROR,MINOR_ERROR,"Wanji_manager read_proto_param  failed");
	}

	return wanji_manager_init_from_protobuf(t_wanji_parameters, terminal_id);
}
//初始化 雷达 管理模块。从protobuf读取
Error_manager Wanji_manager::wanji_manager_init_from_protobuf(wj::wjManagerParams& wanji_parameters, int terminal_id)
{
	LOG(INFO) << " ---wanji_manager_init_from_protobuf run--- "<< this;
	Error_manager t_error;
	if ( m_wanji_manager_status != E_UNKNOWN)
	{
		return Error_manager(Error_code::WJ_MANAGER_INIT_ERROR, Error_level::MINOR_ERROR,
							 " Wanji_manager::wanji_manager_init_from_protobuf init repeat error ");
	}

	m_terminal_id = terminal_id;

// 	//创建雷达设备
// 	int t_wanji_lidar_size = wanji_parameters.wj_lidar_size();
// 	for(int i=0;i<t_wanji_lidar_size;++i)
// 	{
// 		Wanji_lidar_device* p_wanji_lidar_device(new Wanji_lidar_device);
// 		t_error = p_wanji_lidar_device->init(wanji_parameters.wj_lidar(i));
// //		LOG(WARNING) << wanji_parameters.wj_lidar(i).DebugString();
// 		if ( t_error != Error_code::SUCCESS )
// 		{
// 			delete(p_wanji_lidar_device);
// 			return t_error;
// 		}
// 		m_wanji_lidar_device_map[i] = (p_wanji_lidar_device);
// 	}

// 	//创建预测算法
// 	int t_region_workers_size = wanji_parameters.regions_size();
// 	for(int i=0;i<t_region_workers_size;++i)
// 	{
// 		Region_worker* p_region_worker(new Region_worker);
// 		Point2D_tool::Point2D_box t_point2d_box;
// 		t_point2d_box.x_min = wanji_parameters.regions(i).minx();
// 		t_point2d_box.x_max = wanji_parameters.regions(i).maxx();
// 		t_point2d_box.y_min = wanji_parameters.regions(i).miny();
// 		t_point2d_box.y_max = wanji_parameters.regions(i).maxy();
// 		// changed by yct, 传入终端index,仅普爱使用
// 		t_error = p_region_worker->init(t_point2d_box, &m_cloud_collection_mutex, mp_cloud_collection, i);
// 		if ( t_error != Error_code::SUCCESS )
// 		{
// 			delete(p_region_worker);
// 			return t_error;
// 		}
// 		m_region_worker_map[i] = p_region_worker;
// 	}

	// 20220714 changed by yct
	// 根据是否处于分布式模式，决定初始化设备数量
	//创建预测算法
	std::set<int> lidar_selection_set;
	for (int i = 0; i < wanji_parameters.regions_size(); ++i)
	{
		// 非分布式则创建所有区域，否则只创建对应区域
		if (!wanji_parameters.distribution_mode() || (wanji_parameters.distribution_mode() && wanji_parameters.regions(i).region_id() == m_terminal_id))
		{
			// 分布式模式记录待创建雷达编号
			if(wanji_parameters.distribution_mode())
			{
				for (size_t j = 0; j < wanji_parameters.regions(i).lidar_exts_size(); j++)
				{
					lidar_selection_set.emplace(wanji_parameters.regions(i).lidar_exts(j).lidar_id());
				}
			}

			Region_worker *p_region_worker(new Region_worker);
			t_error = p_region_worker->init(wanji_parameters.regions(i));
			if (t_error != Error_code::SUCCESS)
			{
				delete (p_region_worker);
				return t_error;
			}
			m_region_worker_map[wanji_parameters.regions(i).region_id()] = p_region_worker;
			LOG(WARNING) << "region worker " << std::to_string(wanji_parameters.regions(i).region_id()) << " created!!!";
		}
	}
	// 检查是否存在该区域
	if(wanji_parameters.distribution_mode() && m_region_worker_map.find(m_terminal_id) == m_region_worker_map.end())
	{
		LOG(ERROR) << "param error, cannot find region "<<m_terminal_id;
		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
							 " wanji_manager::wanji_manager_init_from_protobuf region defined in param cannot be found.");
	}

	//创建雷达设备
	for(int i=0;i<wanji_parameters.wj_lidar_size();++i)
	{
		// 非分布式模式创建所有雷达，否则只创建对应区域标记的雷达
		if (!wanji_parameters.distribution_mode() || (wanji_parameters.distribution_mode() && lidar_selection_set.find(wanji_parameters.wj_lidar(i).lidar_id()) != lidar_selection_set.end() ))
		{
#if LIDAR_TYPE == 1
	Wanji_lidar_device *p_wj_lidar_device(new Wanji_lidar_device);
#else
	Wanji_716N_lidar_device *p_wj_lidar_device(new Wanji_716N_lidar_device);
#endif
			// Wanji_lidar_device *p_wj_lidar_device(new Wanji_lidar_device);
			t_error = p_wj_lidar_device->init(wanji_parameters.wj_lidar(i));
			if (t_error != Error_code::SUCCESS)
			{
				delete (p_wj_lidar_device);
				return t_error;
			}
			m_wanji_lidar_device_map[wanji_parameters.wj_lidar(i).lidar_id()] = p_wj_lidar_device;
			LOG(WARNING) << "wj lidar " << std::to_string(wanji_parameters.wj_lidar(i).lidar_id()) << " created!!!";

			// 根据创建的雷达与区域，初始化点云缓存map
			for (std::map<int, Region_worker *>::iterator iter = m_region_worker_map.begin(); iter != m_region_worker_map.end(); iter++)
			{
				wj::Region t_region_param = iter->second->get_param();
				std::string t_region_lidar_key = generate_region_lidar_key(iter->first, wanji_parameters.wj_lidar(i).lidar_id());
				// 遍历区域内激光内参列表，找到lidar_id对应内参
				for (size_t j = 0; j < t_region_param.lidar_exts_size(); j++)
				{
					if(t_region_param.lidar_exts(j).lidar_id() == wanji_parameters.wj_lidar(i).lidar_id())
					{
						// 未找到则创建
						if (m_region_laser_to_cloud_collection_map.find(t_region_lidar_key) == m_region_laser_to_cloud_collection_map.end())
						{
							m_region_laser_to_cloud_collection_map.emplace(std::pair<std::string,
																					 std::shared_ptr<Timestamped_cloud_wj>>(t_region_lidar_key, std::shared_ptr<Timestamped_cloud_wj>(new Timestamped_cloud_wj(t_region_param.lidar_exts(j).calib()))));
							LOG(WARNING) << "pair of (lidar " << std::to_string(wanji_parameters.wj_lidar(i).lidar_id()) <<", region "<<std::to_string(iter->first) << ") created!!!";
						}
						else
						{
							LOG(ERROR) << "region lidar repeated? " << t_region_lidar_key;
						}
					}
				}
			}
		}
	}
	// 检查设备与区域个数，不可为0
	if(m_region_worker_map.size()<=0 || m_wanji_lidar_device_map.size()<=0)
	{
		LOG(ERROR) << "param error, region or lidar size error";
		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
							 " wanji_manager::wanji_manager_init_from_protobuf region or lidar size is 0");
	}

	//启动 收集点云的线程。默认 。
	m_collect_cloud_condition.reset(false, false, false);
	mp_collect_cloud_thread = new std::thread(&Wanji_manager::collect_cloud_thread_fun, this);

	//启动 执行的线程。默认 。
	m_execute_condition.reset(false, false, false);
	mp_execute_thread = new std::thread(&Wanji_manager::execute_thread_fun, this);

	m_wanji_manager_status = E_READY;

	return Error_code::SUCCESS;
}
//反初始化 雷达 管理模块。
Error_manager Wanji_manager::wanji_manager_uninit()
{
	LOG(INFO) << " ---wanji_manager_uninit run--- "<< this;
	//关闭线程
	if (mp_collect_cloud_thread)
	{
		m_collect_cloud_condition.kill_all();
	}
	if (mp_execute_thread)
	{
		m_execute_condition.kill_all();
	}
	//回收线程的资源
	if (mp_collect_cloud_thread)
	{
		mp_collect_cloud_thread->join();
		delete mp_collect_cloud_thread;
		mp_collect_cloud_thread = NULL;
	}
	if (mp_execute_thread)
	{
		mp_execute_thread->join();
		delete mp_execute_thread;
		mp_execute_thread = NULL;
	}
	//回收雷达设备
	for (auto iter = m_wanji_lidar_device_map.begin(); iter != m_wanji_lidar_device_map.end(); ++iter)
	{
		delete(iter->second);
	}
	m_wanji_lidar_device_map.clear();

	for (auto iter = m_region_worker_map.begin(); iter != m_region_worker_map.end(); ++iter)
	{
		delete(iter->second);
	}
	m_region_worker_map.clear();

	m_wanji_manager_status = E_UNKNOWN;

	return Error_code::SUCCESS;
}


//对外的接口函数，负责接受并处理任务单，
Error_manager Wanji_manager::execute_task(Task_Base* p_wanji_manager_task)
{
	LOG(INFO) << " ---Wanji_manager::execute_task run---  "<< this;
	Error_manager t_error;
	Error_manager t_result;

//检查指针
	if (p_wanji_manager_task == NULL) {
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Wanji_manager::execute_task failed, POINTER_IS_NULL");
	}
//检查任务类型，
	if (p_wanji_manager_task->get_task_type() != WANJI_MANAGER_TASK)
	{
		return Error_manager(Error_code::WJ_MANAGER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 "laser task type error ！= WANJI_MANAGER_TASK ");
	}

//检查接收方的状态
	t_error = check_status();
	if ( t_error != SUCCESS )
	{
		t_result.compare_and_cover_error(t_error);
	}
	else
	{
//接受任务，并将任务的状态改为TASK_SIGNED已签收
		mp_wanji_manager_task = (Wanji_manager_task *) p_wanji_manager_task;
		mp_wanji_manager_task->set_task_statu(TASK_SIGNED);

//启动雷达管理模块，的核心工作线程
		m_wanji_manager_status = E_BUSY;
		m_execute_condition.notify_all(true);

//将任务的状态改为 TASK_WORKING 处理中
		mp_wanji_manager_task->set_task_statu(TASK_WORKING);


//return 之前要填充任务单里面的错误码.
		if (t_result != Error_code::SUCCESS)
		{
//忽略轻微故障
			if ( t_result.get_error_level() >= Error_level::MINOR_ERROR)
			{
//将任务的状态改为 TASK_ERROR 结束错误
				mp_wanji_manager_task->set_task_statu(TASK_ERROR);
			}
//返回错误码
			mp_wanji_manager_task->compare_and_cover_task_error_manager(t_result);
		}
	}

	return t_result;
}

//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
Error_manager Wanji_manager::end_task()
{
	m_execute_condition.notify_all(false);

	if ( m_wanji_manager_status == E_BUSY ||
	m_wanji_manager_status == E_ISSUED_SCAN ||
	m_wanji_manager_status == E_WAIT_SCAN ||
	m_wanji_manager_status == E_ISSUED_DETECT ||
	m_wanji_manager_status == E_WAIT_DETECT )
	{
		m_wanji_manager_status = E_READY;
	}
	//else 状态不变

	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
	if(mp_wanji_manager_task !=NULL)
	{
		//判断任务单的错误等级，
		if ( mp_wanji_manager_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
		{
			//强制改为TASK_OVER，不管它当前在做什么。
			mp_wanji_manager_task->set_task_statu(TASK_OVER);
		}
		else
		{
			//强制改为 TASK_ERROR，不管它当前在做什么。
			mp_wanji_manager_task->set_task_statu(TASK_ERROR);
		}
	}

	return Error_code::SUCCESS;
}
//取消任务单，由发送方提前取消任务单
Error_manager Wanji_manager::cancel_task(Task_Base* p_wanji_manager_task)
{
	//关闭子线程
	m_execute_condition.notify_all(false);
	//确保内部线程已经停下
	while (m_execute_condition.is_working())
	{

	}

	//强制改为 TASK_DEAD，不管它当前在做什么。
	mp_wanji_manager_task->set_task_statu(TASK_DEAD);

	if ( m_wanji_manager_status == E_BUSY ||
		 m_wanji_manager_status == E_ISSUED_SCAN ||
		 m_wanji_manager_status == E_WAIT_SCAN ||
		 m_wanji_manager_status == E_ISSUED_DETECT ||
		 m_wanji_manager_status == E_WAIT_DETECT )
	{
		m_wanji_manager_status = E_READY;
	}
	//else 状态不变

	return Error_code::SUCCESS;
}

//检查雷达状态,是否正常运行
Error_manager Wanji_manager::check_status()
{
	if ( m_wanji_manager_status == E_READY )
	{
		return Error_code::SUCCESS;
	}
	else if ( m_wanji_manager_status == E_BUSY ||
			  m_wanji_manager_status == E_ISSUED_SCAN ||
			  m_wanji_manager_status == E_WAIT_SCAN ||
			  m_wanji_manager_status == E_ISSUED_DETECT ||
			  m_wanji_manager_status == E_WAIT_DETECT )
	{
		return Error_manager(Error_code::WJ_MANAGER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
							 " Laser_manager::check_status error ");
	}
	else
	{
		return Error_manager(Error_code::WJ_MANAGER_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " Laser_manager::check_status error ");
	}
	return Error_code::SUCCESS;
}
//判断是否为待机，如果已经准备好，则可以执行任务。
bool Wanji_manager::is_ready()
{
	return  m_wanji_manager_status == E_READY;
}

Wanji_manager::Wanji_manager_status Wanji_manager::get_status()
{
	return m_wanji_manager_status;
}
#if LIDAR_TYPE == 1
	std::map<int, Wanji_lidar_device*> & Wanji_manager::get_wanji_lidar_device_map()
#else
	std::map<int, Wanji_716N_lidar_device*> & Wanji_manager::get_wanji_lidar_device_map()
#endif
{
	return m_wanji_lidar_device_map;
}

std::map<int, Region_worker *> & Wanji_manager::get_region_worker_map()
{
	return m_region_worker_map;
}






//自动收集点云的线程函数
void Wanji_manager::collect_cloud_thread_fun()
{
	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() start "<< this;
	Error_manager t_error;
	Error_manager t_result;
	static int cloud_count=-1;

	while (m_collect_cloud_condition.is_alive())
	{
		cloud_count++;
		//暂时固定为一个扫描周期, 就循环一次
		//后期可以根据每个小雷达的刷新情况, 实时更改总点云.....
		m_collect_cloud_condition.wait_for_millisecond(WANJI_716_SCAN_CYCLE_MS);
		if ( m_collect_cloud_condition.is_alive() )
		{
			std::chrono::system_clock::time_point t_command_time = std::chrono::system_clock::now();
			{
				//全局加锁, 并清空点云.
				std::unique_lock<std::mutex> t_lock(m_cloud_collection_mutex);
				// mp_cloud_collection->clear();

				//重新收集最近的点云, 不允许阻塞
				// added by yct, 测试雷达功能
				pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
				for (auto iter = m_wanji_lidar_device_map.begin(); iter != m_wanji_lidar_device_map.end(); ++iter)
				{
					t_cloud->clear();
					t_error = iter->second->get_last_cloud(t_cloud,t_command_time);
					if (t_error != Error_code::SUCCESS)
					{
						t_result.compare_and_cover_error(t_error);
						LOG_EVERY_N(WARNING, 9000) << "cloud timeout lidar id: "<<iter->first;
						// 将该雷达对应所有区域点云清空
					}else if(cloud_count==5){
						std::string t_filename = std::string("./wj_intrin_cloud_")+std::to_string(iter->first)+".txt";
						save_cloud_txt(t_cloud, t_filename);
						LOG(INFO) << "lidar intrin cloud has been saved in " + t_filename;
					}
					// 点云放入对应缓存
					for (std::map<int, Region_worker *>::iterator iter_region = m_region_worker_map.begin(); iter_region != m_region_worker_map.end(); iter_region++)
					{
						if (!iter_region->second->check_lidar_inside(iter->first))
							continue;
						std::string t_region_lidar_key = generate_region_lidar_key(iter_region->first, iter->first);
						// 未找到则报错
						if (m_region_laser_to_cloud_collection_map.find(t_region_lidar_key) == m_region_laser_to_cloud_collection_map.end())
						{
							LOG(ERROR) << "current lidar cloud not initialized: " << t_region_lidar_key;
						}
						else
						{
							if (t_error != SUCCESS)
							{
								m_region_laser_to_cloud_collection_map[t_region_lidar_key]->clearCloud();
								// LOG(WARNING) << "编号" << iter->first << "雷达在区域" << iter_region->first << "中点云清除 " << t_error.to_string();
							}
							else
							{
								// LOG(WARNING) << "map count: "<< m_region_laser_to_cloud_collection_map.size();
								m_region_laser_to_cloud_collection_map[t_region_lidar_key]->setCloud(t_cloud);
								// LOG(WARNING) << "编号" << iter->first << "雷达在区域" << iter_region->first << "中点云 " << t_cloud->size();
							}
						}
					}
				}
				update_region_cloud();

// 				if ( t_result == SUCCESS && mp_cloud_collection->size()>0 )
// 				{
// 					m_cloud_update_time = std::chrono::system_clock::now();
// 					//成功则唤醒预测算法
// 					start_auto_detect();
// 				}
// 				else
// 				{
// 					mp_cloud_collection->clear();
// 					//失败则关闭预测算法
// 					stop_auto_detect();
// //					LOG(WARNING) << t_result.to_string();
// 				}
			}
		}
        t_result.error_manager_clear_all();
	}
	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() end "<< this;
	return;
}

//更新点云，自行计算
Error_manager Wanji_manager::update_region_cloud()
{
	// static std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
	
	for (auto iter = m_region_worker_map.begin(); iter != m_region_worker_map.end(); ++iter)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr t_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
		// 每个区域需要所有雷达点云，任何一个缺失都不可进入计算流程。
		// 所有点云都缺失，则可能是当前区域空闲，此时不进行计算，也不弹出警告
		// 在遍历完雷达后，该数组中存在4个元素才可进入计算流程
		std::vector<int> corresponding_lidar_ids;
		std::vector<int> corresponding_lidar_cloud_size;
		// 遍历激光列表，找到终端对应点云数据，经过外参变换后叠加更新区域点云
		for (auto iter_lidar = m_wanji_lidar_device_map.begin(); iter_lidar != m_wanji_lidar_device_map.end(); ++iter_lidar)
		{
			if(!iter->second->check_lidar_inside(iter_lidar->first))
				continue;
			std::string t_region_lidar_key = generate_region_lidar_key(iter->first, iter_lidar->first);
			//将点云经过外参变换后，添加到对应区域中
			if(m_region_laser_to_cloud_collection_map.find(t_region_lidar_key) != m_region_laser_to_cloud_collection_map.end())
			{
				std::shared_ptr<Timestamped_cloud_wj> t_timed_cloud = m_region_laser_to_cloud_collection_map[t_region_lidar_key];
				if (t_timed_cloud->m_cloud->size() > 0)
				{
					t_region_cloud->operator+=(*t_timed_cloud->transCloud());
					corresponding_lidar_ids.push_back(iter_lidar->first);
					corresponding_lidar_cloud_size.push_back(t_timed_cloud->m_cloud->size());
					// LOG(WARNING) << "编号" << iter_lidar->first << "雷达在入口" << iter->first << "中点云已添加" << t_timed_cloud->m_cloud->size();
				}
				else
				{
					LOG_EVERY_N(WARNING, 1000) << "编号" << iter_lidar->first << "雷达在入口" << iter->first << "中未获得点云。";
					continue;
				}
			}
			else
			{
				LOG_EVERY_N(WARNING, 1) << "编号" << iter_lidar->first << "雷达在入口" << iter->first << "中未初始化完全。";
				continue;
			}
		}
		if(corresponding_lidar_ids.size() != corresponding_lidar_cloud_size.size())
		{
			LOG_EVERY_N(ERROR, 1) << "入口" << iter->first << "出现访问不可达路径情况，请检查！！！！！";
			continue;
		}

		// 必须传入空点云以避免入口状态不更新
		// wj 4角校验不可简单根据点数判断
		if(corresponding_lidar_ids.size() == 4 ){
			// 检查两边点云个数，若差异过大则清除点云，传入空点云以更新结果
			int max_cloud_size = std::max(corresponding_lidar_cloud_size[0], corresponding_lidar_cloud_size[1]);
			max_cloud_size = std::max(corresponding_lidar_cloud_size[2], max_cloud_size);
			max_cloud_size = std::max(corresponding_lidar_cloud_size[3], max_cloud_size);
			int min_cloud_size = std::min(corresponding_lidar_cloud_size[0], corresponding_lidar_cloud_size[1]);
			min_cloud_size = std::min(corresponding_lidar_cloud_size[2], min_cloud_size);
			min_cloud_size = std::min(corresponding_lidar_cloud_size[2], min_cloud_size);
			// 极限相差10倍，或一边仅不到10个点，则清除点云，传入空点云以更新结果
			if ((min_cloud_size < 10 && max_cloud_size > 30) || (min_cloud_size >= 10 && max_cloud_size > 10 * min_cloud_size + 10))
			{
				LOG_EVERY_N(ERROR, 60) << "入口" << iter->first << "两片点云差异过大，分别为:"<<max_cloud_size<<"个与"<<min_cloud_size<<"个，请检查！！！ ";
				// 清除点云
				t_region_cloud->clear();
			}
			else
			{
				// 不用修改点云内容
			}
		}
		else if (corresponding_lidar_ids.size() < 4)
		{
			LOG_EVERY_N(WARNING, 60) << "检测到入口" << iter->first << "点云缺失, 找到" << corresponding_lidar_ids.size() << "个雷达的点云";
			// 只有半边点云，将其清除
			t_region_cloud->clear();
		}
		else{
			// 此入口可能暂时空闲，左右都没有点
			// 清除点云，确保传入为空点云
			t_region_cloud->clear();
		}
		// 统一到此处更新点云，异常情况下将点云清除，保证入口正常更新
		iter->second->update_cloud(t_region_cloud);

		// std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
		// std::chrono::duration<double> time_used_update = std::chrono::duration_cast<std::chrono::duration<double>>(t1 - t0);
		// t0 = std::chrono::steady_clock::now();
		// LOG(WARNING) << "send to region " << iter->first << " with cloud size: " << t_region_cloud->size() ;
	}
	return Error_code::SUCCESS;
}

//开始自动预测的算法线程
Error_manager Wanji_manager::start_auto_detect()
{
	for (auto iter = m_region_worker_map.begin(); iter != m_region_worker_map.end(); ++iter)
	{
	    iter->second->start_auto_detect();
	}

	return Error_code::SUCCESS;
}
//关闭自动预测的算法线程
Error_manager Wanji_manager::stop_auto_detect()
{
	for (auto iter = m_region_worker_map.begin(); iter != m_region_worker_map.end(); ++iter)
	{
		iter->second->stop_auto_detect();
	}

	return Error_code::SUCCESS;
}

//执行外界任务的执行函数
void Wanji_manager::execute_thread_fun()
{
	LOG(INFO) << " Wanji_manager::execute_thread_fun() start "<< this;
	Error_manager t_error;
	Common_data::Car_wheel_information t_car_wheel_information;

	//雷达管理的独立线程，负责控制管理所有的雷达。
	while (m_execute_condition.is_alive())
	{
		m_execute_condition.wait();
		if ( m_execute_condition.is_alive() )
		{
			std::this_thread::yield();
			//重新循环必须清除错误码.
			t_error.error_manager_clear_all();

			if ( mp_wanji_manager_task == NULL )
			{
				//忽略任务, 直接停止线程
				m_wanji_manager_status = E_READY;
				m_execute_condition.notify_all(false);
			}

			//万集内部全自动运行, 只需要根据时间去获取想要的就行了.
			//目前楚天是唯一的预测算法, 如果后续有多个出入口,则使用任务单的终端id来获取指定的.
			if ( m_region_worker_map.size() > 0 && m_region_worker_map.find(mp_wanji_manager_task->get_terminal_id()) != m_region_worker_map.end())
			{
				t_error = m_region_worker_map[mp_wanji_manager_task->get_terminal_id()]->get_current_wheel_information(&t_car_wheel_information, mp_wanji_manager_task->get_command_time());
				if ( t_error == Error_code::SUCCESS )
				{
					//添加车轮的预测结果
					mp_wanji_manager_task->set_car_wheel_information(t_car_wheel_information);
					//正常完成任务
					end_task();
				}
				else
				{
					//如果在指令时间1秒后都没有成功获取结果, 返回错误原因
					if ( std::chrono::system_clock::now() - m_region_worker_map[mp_wanji_manager_task->get_terminal_id()]->get_detect_updata_time() < std::chrono::milliseconds(WANJI_MANAGER_EXECUTE_TIMEOUT_MS)  )
					{
						//没有超时, 那么就等1ms, 然后重新循环
						std::this_thread::sleep_for(std::chrono::milliseconds(1));
						std::this_thread::yield();
					}
					else
					{
						//因为故障,而提前结束任务.
						mp_wanji_manager_task->compare_and_cover_task_error_manager(t_error);
						end_task();
					}

				}
			}

		}
	}
	LOG(INFO) << " Wanji_manager::execute_thread_fun() end "<< this;
	return;
}