puai_wj_2021/velodyne_lidar/velodyne_manager.cpp

/*
 * @Description: 
 * @Author: yct
 * @Date: 2021-07-23 16:39:04
 * @LastEditTime: 2021-10-10 21:46:59
 * @LastEditors: yct
 */
#include "velodyne_manager.h"
#include "../tool/proto_tool.h"

//初始化 雷达 管理模块。如下三选一
Error_manager Velodyne_manager::velodyne_manager_init(int terminal)
{
    return velodyne_manager_init_from_protobuf(VELODYNE_MANAGER_PARAMETER_PATH, terminal);
}

//初始化 雷达 管理模块。从文件读取
Error_manager Velodyne_manager::velodyne_manager_init_from_protobuf(std::string prototxt_path,int terminal)
{
    velodyne::velodyneManagerParams t_velo_params;
    if (!proto_tool::read_proto_param(prototxt_path, t_velo_params))
    {
        return Error_manager(VELODYNE_MANAGER_READ_PROTOBUF_ERROR, MINOR_ERROR, "velodyne_manager read_proto_param  failed");
    }

    return velodyne_manager_init_from_protobuf(t_velo_params, terminal);
}

//初始化 雷达 管理模块。从protobuf读取
Error_manager Velodyne_manager::velodyne_manager_init_from_protobuf(velodyne::velodyneManagerParams & velodyne_parameters,int terminal)
{
    LOG(INFO) << " ---velodyne_manager_init_from_protobuf run--- "<< this;
	Error_manager t_error;
	if ( m_velodyne_manager_status != E_UNKNOWN)
	{
		return Error_manager(Error_code::VELODYNE_MANAGER_INIT_ERROR, Error_level::MINOR_ERROR,
							 " velodyne_manager::velodyne_manager_init_from_protobuf init repeat error ");
	}
	m_terminal_id = terminal;
	// mp_cloud_collection = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);

	// 判断参数合法性
	if(!velodyne_parameters.has_left_model_path() || ! velodyne_parameters.has_right_model_path())
	{
		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
							 " velodyne_manager::velodyne_manager_init_from_protobuf param must contain model path");
	}
	pcl::PointCloud<pcl::PointXYZ>::Ptr left = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr right = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
	if (!read_pointcloud(velodyne_parameters.left_model_path(), left) || !read_pointcloud(velodyne_parameters.right_model_path(), right))
	{
		LOG(ERROR) << "cannot read left/right model ";
		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MAJOR_ERROR,
							 " velodyne_manager::velodyne_manager_init_from_protobuf left/right model path error");
	}

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

			Ground_region *p_ground_region(new Ground_region);
			t_error = p_ground_region->init(velodyne_parameters.region(i), left, right);
			if (t_error != Error_code::SUCCESS)
			{
				delete (p_ground_region);
				return t_error;
			}
			m_ground_region_map[velodyne_parameters.region(i).region_id()] = p_ground_region;
			LOG(WARNING) << "ground region " << std::to_string(velodyne_parameters.region(i).region_id()) << " created!!!";
		}
	}
	// 检查是否存在该区域
	if(velodyne_parameters.distribution_mode() && m_ground_region_map.find(m_terminal_id) == m_ground_region_map.end())
	{
		LOG(ERROR) << "param error, cannot find region "<<m_terminal_id;
		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
							 " velodyne_manager::velodyne_manager_init_from_protobuf region defined in param cannot be found.");
	}

	//创建雷达设备
	for(int i=0;i<velodyne_parameters.velodyne_lidars_size();++i)
	{
		// 非分布式模式创建所有雷达，否则只创建对应区域标记的雷达
		if (!velodyne_parameters.distribution_mode() || (velodyne_parameters.distribution_mode() && lidar_selection_set.find(velodyne_parameters.velodyne_lidars(i).lidar_id()) != lidar_selection_set.end() ))
		{
			Velodyne_lidar_device *p_velodyne_lidar_device(new Velodyne_lidar_device);
			t_error = p_velodyne_lidar_device->init(velodyne_parameters.velodyne_lidars(i));
			if (t_error != Error_code::SUCCESS)
			{
				delete (p_velodyne_lidar_device);
				return t_error;
			}
			m_velodyne_lidar_device_map[velodyne_parameters.velodyne_lidars(i).lidar_id()] = p_velodyne_lidar_device;
			LOG(WARNING) << "velodyne lidar " << std::to_string(velodyne_parameters.velodyne_lidars(i).lidar_id()) << " created!!!";

			// 根据创建的雷达与区域，初始化点云缓存map
			for (std::map<int, Ground_region *>::iterator iter = m_ground_region_map.begin(); iter != m_ground_region_map.end(); iter++)
			{
				velodyne::Region t_region_param = iter->second->get_param();
				std::string t_region_lidar_key = generate_region_lidar_key(iter->first, velodyne_parameters.velodyne_lidars(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() == velodyne_parameters.velodyne_lidars(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>>(t_region_lidar_key, std::shared_ptr<Timestamped_cloud>(new Timestamped_cloud(t_region_param.lidar_exts(j).calib()))));
							LOG(WARNING) << "pair of (lidar " << std::to_string(velodyne_parameters.velodyne_lidars(i).lidar_id()) <<", region "<<std::to_string(iter->first) << ") created!!!";
						}
						else
						{
							LOG(ERROR) << "region lidar repeated? " << t_region_lidar_key;
						}
					}
				}
			}
		}
	}
	// 检查设备与区域个数，不可为0
	if(m_ground_region_map.size()<=0 || m_velodyne_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,
							 " velodyne_manager::velodyne_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(&Velodyne_manager::collect_cloud_thread_fun, this);

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

	m_velodyne_manager_status = E_READY;

	return Error_code::SUCCESS;
}

// 反初始化
Error_manager Velodyne_manager::Velodyne_manager_uninit()
{
    LOG(INFO) << " ---velodyne_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_velodyne_lidar_device_map.begin(); iter != m_velodyne_lidar_device_map.end(); ++iter)
	{
		delete(iter->second);
	}
	m_velodyne_lidar_device_map.clear();

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

	m_velodyne_manager_status = E_UNKNOWN;

	return Error_code::SUCCESS;
}

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

	//检查指针
	if (p_velodyne_manager_task == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Velodyne_manager::execute_task failed, POINTER_IS_NULL");
	}
	//检查任务类型，
	if (p_velodyne_manager_task->get_task_type() != VELODYNE_MANAGER_TASK)
	{
		return Error_manager(Error_code::VELODYNE_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_velodyne_manager_task = (Velodyne_manager_task *)p_velodyne_manager_task;
		mp_velodyne_manager_task->set_task_statu(TASK_SIGNED);

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

		//将任务的状态改为 TASK_WORKING 处理中
		mp_velodyne_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_velodyne_manager_task->set_task_statu(TASK_ERROR);
			}
			//返回错误码
			mp_velodyne_manager_task->compare_and_cover_task_error_manager(t_result);
		}
	}

	return t_result;
}

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

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

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

	return Error_code::SUCCESS;
}

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

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

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

	return Error_code::SUCCESS;
}

//检查雷达状态,是否正常运行
Error_manager Velodyne_manager::check_status()
{
	if (m_velodyne_manager_status == E_READY)
	{
		return Error_code::SUCCESS;
	}
	else if (m_velodyne_manager_status == E_BUSY ||
			 m_velodyne_manager_status == E_ISSUED_SCAN ||
			 m_velodyne_manager_status == E_WAIT_SCAN ||
			 m_velodyne_manager_status == E_ISSUED_DETECT ||
			 m_velodyne_manager_status == E_WAIT_DETECT)
	{
		return Error_manager(Error_code::VELODYNE_MANAGER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
							 " Velodyne_manager::check_status error ");
	}
	else
	{
		return Error_manager(Error_code::VELODYNE_MANAGER_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " Velodyne_manager::check_status error ");
	}
	return Error_code::SUCCESS;
}

void Velodyne_manager::collect_cloud_thread_fun()
{
	static int cloud_count=-1;
	LOG(INFO) << " Velodyne_manager::collect_cloud_thread_fun() start "<< this;
	Error_manager t_error;
	Error_manager t_result;
	// std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();

	while (m_collect_cloud_condition.is_alive())
	{

		cloud_count++;
		//暂时固定为一个扫描周期, 就循环一次
		//后期可以根据每个小雷达的刷新情况, 实时更改总点云.....
		m_collect_cloud_condition.wait_for_millisecond(VLP16_SCAN_CYCLE_MS);
		if ( m_collect_cloud_condition.is_alive() )
		{

			// 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);
			// std::cout << "update cloud time: " << time_used_update.count() << std::endl;

			{
				//全局加锁
				std::unique_lock<std::mutex> t_lock(m_cloud_collection_mutex);
				// mp_cloud_collection->clear();

				//重新收集最近的点云, 不允许阻塞
				// added by yct, 测试雷达功能
				//				int get_cloud_count=0;
				pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
				for (auto iter = m_velodyne_lidar_device_map.begin(); iter != m_velodyne_lidar_device_map.end(); ++iter)
				{
					t_cloud->clear();
					t_error = iter->second->get_last_cloud(t_cloud, std::chrono::steady_clock::now());
					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("./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;
					}
					//                    get_cloud_count++;
					
					// 点云放入对应缓存
					for (std::map<int, Ground_region *>::iterator iter_region = m_ground_region_map.begin(); iter_region != m_ground_region_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())
						{
							// m_region_laser_to_cloud_collection_map.emplace(std::pair<std::string,
							// 														 std::shared_ptr<Timestamped_cloud>>(t_region_lidar_key, std::shared_ptr<Timestamped_cloud>(new Timestamped_cloud())));
							// if (t_error != SUCCESS)
							// {
							// 	m_region_laser_to_cloud_collection_map[t_region_lidar_key]->clearCloud();
							// }
							// else
							// {
							// 	m_region_laser_to_cloud_collection_map[t_region_lidar_key]->setCloud(t_cloud);
							// }
							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();
							}
						}
					}
				}
				// 20211010 changed by yct,无论是否存在获取失败的情况，都更新点云到区域，保证其他区域功能正常
				update_region_cloud();
				// if ( t_result == SUCCESS)
				// {
				// 	// m_cloud_update_time = std::chrono::system_clock::now();
				// 	//成功则唤醒预测算法
				// 	update_region_cloud();
				// }
				// else
				// {
				// 	// mp_cloud_collection->clear();
				// 	// LOG(WARNING) << t_result.to_string();
				// 	// // 未连接雷达时，可读入点云用于测试
				// 	// read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/build/comb_calib.txt", mp_cloud_collection);
				// 	// update_region_cloud();
				// }
			}
		}
        t_result.error_manager_clear_all();

		// t0 = std::chrono::steady_clock::now();
	}
	LOG(INFO) << " Velodyne_manager::collect_cloud_thread_fun() end "<< this;
	return;
}

//更新点云，自行计算
Error_manager Velodyne_manager::update_region_cloud()
{
	// static std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
	
	for (auto iter = m_ground_region_map.begin(); iter != m_ground_region_map.end(); ++iter)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr t_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);

		// 遍历激光列表，找到终端对应点云数据，经过外参变换后叠加更新区域点云
		for (auto iter_lidar = m_velodyne_lidar_device_map.begin(); iter_lidar != m_velodyne_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> 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());
					// LOG(WARNING) << "编号" << iter_lidar->first << "雷达在区域" << iter->first << "中点云已添加" << t_timed_cloud->m_cloud->size();
				}
				else
				{
					LOG_EVERY_N(WARNING, 100) << "编号" << iter_lidar->first << "雷达在区域" << iter->first << "中未获得点云。";
					continue;
				}
			}
			else
			{
				LOG_EVERY_N(WARNING, 100) << "编号" << iter_lidar->first << "雷达在区域" << iter->first << "中未初始化完全。";
				continue;
			}
		}

		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() << ", update cloud time ms: " << time_used_update.count();
	}
	return Error_code::SUCCESS;
}

//执行外界任务的执行函数
void Velodyne_manager::execute_thread_fun()
{
	LOG(INFO) << " Velodyne_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_velodyne_manager_task == NULL )
			{
				//忽略任务, 直接停止线程
				m_velodyne_manager_status = E_READY;
				m_execute_condition.notify_all(false);
			}

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

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