puai_wj_2021/velodyne_lidar/velodyne_manager.cpp

/*
 * @Description: 
 * @Author: yct
 * @Date: 2021-07-23 16:39:04
 * @LastEditTime: 2021-08-01 16:34:26
 * @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(WJ_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_ids_size(); j++)
				{
					lidar_selection_set.emplace(velodyne_parameters.region(i).lidar_ids(j));
				}
			}

			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;
		}
	}
	// 检查是否存在该区域
	if(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;
		}
	}
	// 检查设备与区域个数，不可为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()
{
	LOG(INFO) << " Velodyne_manager::collect_cloud_thread_fun() start "<< this;
	Error_manager t_error;
	Error_manager t_result;

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

				//重新收集最近的点云, 不允许阻塞
				// added by yct, 测试雷达功能
//				int get_cloud_count=0;
				for (auto iter = m_velodyne_lidar_device_map.begin(); iter != m_velodyne_lidar_device_map.end(); ++iter)
				{
					t_error = iter->second->get_last_cloud(mp_cloud_collection, std::chrono::steady_clock::now());
					if ( t_error != Error_code::SUCCESS )
					{
						t_result.compare_and_cover_error(t_error);
//						LOG(WARNING) << "cloud timeout: "<<get_cloud_count;
					}
//                    get_cloud_count++;
				}
				if ( t_result == SUCCESS && mp_cloud_collection->size()>0 )
				{
					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();
	}
	LOG(INFO) << " Velodyne_manager::collect_cloud_thread_fun() end "<< this;
	return;
}

//更新点云，自行计算
Error_manager Velodyne_manager::update_region_cloud()
{
	for (auto iter = m_ground_region_map.begin(); iter != m_ground_region_map.end(); ++iter)
	{
		iter->second->update_cloud(mp_cloud_collection);
	}
	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;
}