LidarMeasure/laser/Laser.cpp

#include "Laser.h"
#include "laser_message.pb.h"

Laser_base::Laser_base(int laser_id, Laser_proto::laser_parameter laser_param)
:m_laser_id(laser_id)
,m_laser_param(laser_param)
,m_laser_scan_flag(false)
,m_laser_statu(LASER_DISCONNECT)
,m_points_count(0)
,m_save_flag(false)
{
	mp_thread_receive = NULL;
	mp_thread_transform = NULL;
	mp_thread_publish = NULL;
	init_laser_matrix();
	mp_laser_task = NULL;
}
Laser_base::~Laser_base()
{
	
	disconnect_laser();
	close_save_path();
}

//雷达链接设备，为3个线程添加线程执行函数。
Error_manager Laser_base::connect_laser()
{
	//检查雷达状态
	if ( m_laser_statu != LASER_DISCONNECT )
	{
	    return Error_manager(Error_code::LASER_STATUS_ERROR, Error_level::MINOR_ERROR,
	    					" Laser_base::connect_laser  m_laser_statu != LASER_DISCONNECT ");
	}
	else
	{
		//这里不建议用detach，而是应该在disconnect里面使用join
		//创建接受缓存的线程，不允许重复创建
		if (mp_thread_receive != NULL)
		{
			return Error_manager(Error_code::LASER_CONNECT_FAILED, Error_level::MINOR_ERROR,
								 " mp_thread_receive is alive ");
		}
		else
		{
			//接受缓存的线程，线程存活，暂停运行
			m_condition_receive.reset(false, false, false);
			mp_thread_receive = new std::thread(&Laser_base::thread_receive, this);
		}
		//转化数据的线程，不允许重复创建
		if (mp_thread_transform != NULL)
		{
			return Error_manager(Error_code::LASER_CONNECT_FAILED, Error_level::MINOR_ERROR,
								 " mp_thread_transform is alive ");
		}
		else
		{
			//转化数据的线程，线程存活，暂停运行
			m_condition_transform.reset(false, false, false);
			mp_thread_transform = new std::thread(&Laser_base::thread_transform, this);
		}
		//发布信息的线程，不允许重复创建
		if (mp_thread_publish != NULL)
		{
			return Error_manager(Error_code::LASER_CONNECT_FAILED, Error_level::MINOR_ERROR,
								 " mp_thread_publish is alive ");
		}
		else
		{
			//发布信息的线程，线程存活，循环运行，
			//注：mp_thread_publish 需要线程持续不断的往上位机发送信息，不受任务的影响。
			m_condition_publish.reset(false, true, false);
			mp_thread_publish = new std::thread(&Laser_base::thread_publish, this);
		}

		m_laser_statu = LASER_READY;

	}
	return Error_code::SUCCESS;
}
//雷达断开链接，释放3个线程
Error_manager Laser_base::disconnect_laser()
{
	//终止队列，防止 wait_and_pop 阻塞线程。
	m_queue_laser_data.termination_queue();
	//杀死3个线程，强制退出
	if (mp_thread_receive)
	{
		m_condition_receive.kill_all();
	}
	if (mp_thread_transform)
	{
		m_condition_transform.kill_all();
	}
	if (mp_thread_publish)
	{
		m_condition_publish.kill_all();
	}
	//回收3个线程的资源
	if (mp_thread_receive)
	{
		mp_thread_receive->join();
		delete mp_thread_receive;
		mp_thread_receive = NULL;
	}
	if (mp_thread_transform)
	{
		mp_thread_transform->join();
		delete mp_thread_transform;
		mp_thread_transform = 0;
	}
	if (mp_thread_publish)
	{
		mp_thread_publish->join();
		delete mp_thread_publish;
		mp_thread_publish = NULL;
	}
	//清空队列
	m_queue_laser_data.clear_and_delete();

	if ( m_laser_statu != LASER_FAULT  )
	{
		m_laser_statu = LASER_DISCONNECT;
	}
	return Error_code::SUCCESS;
}


//对外的接口函数，负责接受并处理任务单，
//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
Error_manager Laser_base::execute_task(Task_Base* p_laser_task)
{
	//检查指针
	if(p_laser_task == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Laser_base::execute_task failed, POINTER_IS_NULL");
	}
	else
	{
		return Error_manager(Error_code::CLASS_BASE_FUNCTION_CANNOT_USE, Error_level::NEGLIGIBLE_ERROR,
							 "Laser_base::execute_task cannot use");
	}
}
//检查雷达状态,是否正常运行
Error_manager Laser_base::check_laser()
{
	if ( is_ready() )
	{
		return Error_code::SUCCESS;
	}
	return Error_manager(Error_code::LASER_CHECK_FAILED, Error_level::MINOR_ERROR,
						" check_laser failed ");
}
//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
Error_manager Laser_base::start_scan()
{
	LOG(INFO) << " Laser_base::start_scan "<< this;
	if ( is_ready() )
	{
		//启动雷达扫描
		m_laser_scan_flag=true;				//启动雷达扫描
		m_laser_statu = LASER_BUSY;			//雷达状态 繁忙
		m_condition_receive.notify_all(true);		//唤醒接受线程
		m_condition_transform.notify_all(true);	//唤醒转化线程
		//重置数据
		m_points_count=0;
		m_queue_laser_data.clear_and_delete();
		m_last_data.clear();
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::LASER_START_FAILED, Error_level::MINOR_ERROR,
							 "Laser_base::start_scan() is not ready ");
	}
}
//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
Error_manager Laser_base::stop_scan()
{
	LOG(INFO) << " Laser_base::stop_scan "<< this;
	//stop_scan 只是将 m_laser_scan_flag 改为 stop。
	//然后多线程内部判断 m_laser_scan_flag，然后自动停止线程
	m_laser_scan_flag=false;
	return Error_code::SUCCESS;
}

//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
Error_manager Laser_base::end_task()
{
	LOG(INFO) << " Laser_base::end_task "<< this;
	m_laser_scan_flag=false;
	m_condition_receive.notify_all(false);
	m_condition_transform.notify_all(false);
	m_points_count=0;
	m_queue_laser_data.clear_and_delete();
	m_last_data.clear();

	close_save_path();

	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
	if(mp_laser_task !=NULL)
	{
		//判断任务单的错误等级，
		if ( mp_laser_task->get_task_error_manager().get_error_level() <= Error_level::MINOR_ERROR)
		{
			if ( m_laser_statu == LASER_BUSY )
			{
				//故障等级为 NORMAL NEGLIGIBLE_ERROR MINOR_ERROR，则恢复待机，可以接受下一次的任务单
				m_laser_statu = LASER_READY;
			}

		}
		else
		{
			//故障等级为 MAJOR_ERROR CRITICAL_ERROR，则雷达故障，不可以再接受任务单。
			m_laser_statu = LASER_FAULT;
		}
		//强制改为TASK_OVER，不管它当前在做什么。
		mp_laser_task->set_task_statu(TASK_OVER);
	}
	return Error_code::SUCCESS;
}



//设置保存文件的路径，并打开文件，
Error_manager Laser_base::set_open_save_path(std::string save_path,bool is_save)
{
	m_save_flag = is_save;
	m_points_log_tool.close();
	m_binary_log_tool.close();

	if (is_save == false)
	{
		return Error_code::SUCCESS;
	}
	else
	{
		m_save_path = save_path;
		char pts_file[255] = { 0 };
		sprintf(pts_file, "%s/pts%d.txt", save_path.c_str(), m_laser_id+1);
		m_points_log_tool.open(pts_file);
		//std::cout << "huli m_points_log_tool path "<< pts_file << std::endl;

		char bin_file[255] = { 0 };
		sprintf(bin_file, "%s/laser%d.data", save_path.c_str(), m_laser_id+1);
		m_binary_log_tool.open(bin_file,true);
		return Error_code::SUCCESS;
	}
}
//关闭保存文件，推出前一定要执行
Error_manager Laser_base::close_save_path()
{
	m_save_flag = false;
	m_points_log_tool.close();
	m_binary_log_tool.close();
	return Error_code::SUCCESS;
}

//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
bool Laser_base::is_ready()
{
	return ( get_laser_statu() == LASER_READY );
}
//获取雷达状态
Laser_statu Laser_base::get_laser_statu()
{
	return m_laser_statu;
}
//获取雷达id
int Laser_base::get_laser_id()
{
	return m_laser_id;
}

//线程执行函数，将二进制消息存入队列缓存，
void Laser_base::thread_receive()
{
	LOG(INFO) << " thread_receive start "<< this;
	//接受雷达消息，每次循环只接受一个 Binary_buf
	while (m_condition_receive.is_alive())
	{

		m_condition_receive.wait();
		if ( m_condition_receive.is_alive() )
		{
			//tp_binaty_buf的内存 需要手动分配，然后存入链表。
			//m_queue_laser_data的内存由 Laser_base 基类管理。
			Binary_buf* tp_binaty_buf = new Binary_buf();
			//获取雷达的通信消息缓存
			if (this->receive_buf_to_queue(*tp_binaty_buf))
			{
				//将缓存 存入队列。
				//thread_receive存入前要手动分配内存，thread_transform取出后要记得释放。
				m_queue_laser_data.push(tp_binaty_buf);
			}
			else
			{
				delete tp_binaty_buf;
				tp_binaty_buf=NULL;

				//接受线程无法接受到数据的同时，也收到了雷达停止扫描的信息，那么线程停止
				//如果m_laser_scan_flag停止扫描，但是receive_buf_to_queue仍然能够接受到数据。那么线程继续运行。
				//m_laser_scan_flag停止的瞬间，可能还有残留的数据，需要继续执行。
				if ( !m_laser_scan_flag )
				{
					//停止线程，m_condition_receive.wait() 函数将会阻塞。
					m_condition_receive.set_pass_ever(false);
				}
			}			
		}
	}
	LOG(INFO) << " thread_receive end :"<<this;
	return;
}


//线程执行函数，转化并处理三维点云。
void Laser_base::thread_transform()
{
	LOG(INFO) << " thread_transform start "<< this;
	//转化雷达数据，每次循环只转化一个 Binary_buf，然后得到 t_point3D_cloud.size() 个三维点，
	while (m_condition_transform.is_alive())
	{
		m_condition_transform.wait();
		if ( m_condition_transform.is_alive() )
		{
			//tp_binaty_buf的内存 从链表取出时，就自带内存了。需要手动释放。
			//m_queue_laser_data的内存由 Laser_base 基类管理。
			Binary_buf* tp_binaty_buf=NULL;
			//第1步，从队列中取出缓存。
			int huli_test = m_queue_laser_data.size();
			//thread_receive存入前要手动分配内存，thread_transform取出后要记得释放。
			bool t_pop_flag = m_queue_laser_data.try_pop(tp_binaty_buf);
			/*if ( t_pop_flag )
			{
			    std::cout << "huli m_queue_laser_data.size() last = " << huli_test << std::endl;
				std::cout << "huli m_queue_laser_data.size() now = " << m_queue_laser_data.size() << std::endl;

			}*/

			//第2步，处理数据，缓存转化缓存为三维点。
			//如果获取到了 Binary_buf，或者上次有未完成的 m_last_data，那么就执行数据转化。
			//注注注注注意了，扫描停止后，m_last_data一定要保证清空，不能有垃圾数据残留，这会阻塞线程的暂停
			if (t_pop_flag || m_last_data.get_length() > 0)
			{
				//std::cout << "huli thread_transform " << m_queue_laser_data.size() << std::endl;
				//std::cout << "huli t_pop_flag = "<< t_pop_flag << std::endl;
				//缓存类型
				Buf_type t_buf_type = BUF_UNKNOW;
				//雷达扫描结果，三维点云（雷达自身坐标系）
				std::vector<CPoint3D> t_point3D_cloud;
				//第2.1步，缓存转化为三维点。
				if (t_pop_flag)
				{
					if (tp_binaty_buf == NULL)
					{
						//try_pop返回true，但是tp_binaty_buf没有数据，直接跳过。这个一般不可能出现。
						continue;
					}
					else
					{
						//保存雷达通信 二进制的源文件。
						if(m_save_flag)
						{
							m_binary_log_tool.write(tp_binaty_buf->get_buf(), tp_binaty_buf->get_length());
						}
						//缓存转化为三维点。传入新的缓存，
						t_buf_type= this->transform_buf_to_points(tp_binaty_buf, t_point3D_cloud);
					}					
				}
				else if(m_last_data.get_length() > 0)
				{
					//缓存转化为三维点。没有新消息，就传null，这将处理 m_last_data 上一次遗留的缓存。
					t_buf_type = this->transform_buf_to_points(NULL, t_point3D_cloud);
				}

				//第2.2步，判断t_buf_type，处理雷达数据。并存入任务单
				if (t_buf_type == BUF_UNKNOW || t_buf_type == BUF_READY)
				{
					//跳过这次循环。
					continue;
				}
				else if (t_buf_type == BUF_DATA || t_buf_type == BUF_START || t_buf_type == BUF_STOP)
				{
					//循环处理雷达数据
					for (int i = 0; i < t_point3D_cloud.size(); ++i)
					{
						//三维点的坐标变换，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
						CPoint3D t_point = transform_by_matrix(t_point3D_cloud[i]);
						//保存雷达扫描 三维点云的最终结果。
						if(m_save_flag) {
							char buf[64] = {0};
							sprintf(buf, "%f %f %f\n", t_point.x, t_point.y, t_point.z);
							m_points_log_tool.write(buf, strlen(buf));
							//std::cout << "huli m_points_log_tool.write" << std::endl;
						}

						//此时 t_point 为雷达采集数据的结果，转换后的三维点云。
						//将每个点存入任务单里面的点云容器。
						if(mp_laser_task!=NULL)
						{
							Error_manager t_error= mp_laser_task->task_push_point(pcl::PointXYZ(t_point.x,t_point.y,t_point.z));
							if ( t_error != Error_code::SUCCESS )
							{
								mp_laser_task->get_task_error_manager().compare_and_cover_error(t_error);
								stop_scan();
							}
						}
					}
					//统计扫描点个数。
					m_points_count += t_point3D_cloud.size();
					//思科雷达停止扫描。
					if (t_buf_type == BUF_STOP)
					{
						//注：这个只是思科的雷达停止处，不同的雷达在不同的地方调用 stop_scan
						stop_scan();
					}
				}
			}
			else
			{
				//转化线程 不需要数据的同时，也收到了雷达停止扫描的信息并且接受线程已经停止，那么停止转化线程，
				//注：必须先停止接受线程，再停止转化线程。转化线程停止之后即可结束任务。
				//如果m_laser_scan_flag停止扫描，接受线程和转化线程还有未完成的数据。那么线程继续运行。
				//m_laser_scan_flag停止的瞬间，可能还有残留的数据，需要继续执行。防止数据遗留。
				if ( !m_laser_scan_flag && m_condition_receive.get_pass_ever() == false)
				{
					//停止线程，m_condition_transform.wait() 函数将会阻塞。
					m_condition_transform.set_pass_ever(false);

					//mp_thread_transform 停止时，雷达扫描任务彻底完成，此时结束任务
					end_task();
				}
			}

			//第3步，手动释放缓存
			if ( tp_binaty_buf != NULL )
			{
				delete tp_binaty_buf;
			}
		}
	}
	LOG(INFO) << " thread_transform end "<< this;
	return;
}


//公开发布雷达信息的功能函数，
#include "../tool/MeasureTopicPublisher.h"
Error_manager Laser_base::publish_laser_to_message()
{
    //usleep(5*1000*1000);
	laser_message::laserMsg msg;
	laser_message::laserStatus status;
	if(get_laser_statu()==LASER_READY) status=laser_message::eLaserConnected;
	else if(get_laser_statu()==LASER_DISCONNECT) status=laser_message::eLaserDisconnected;
	else if(get_laser_statu()==LASER_BUSY) status=laser_message::eLaserBusy;
	else if(get_laser_statu()==LASER_FAULT) status=laser_message::eLaserDisconnected;
	else  status=laser_message::eLaserUnknown;

	msg.set_id(m_laser_id);
	msg.set_laser_status(status);
	msg.set_queue_data_count(m_queue_laser_data.size());
	msg.set_cloud_count(m_points_count);
	MeasureTopicPublisher::GetInstance()->Publish(msg.SerializeAsString());

	return SUCCESS;

}
//线程执行函数，公开发布雷达的相关信息，用作上位机的监视。
void Laser_base::thread_publish(Laser_base *p_laser)
{
	LOG(INFO) << " thread_publish start ";
	if(p_laser==NULL)
	{
		return;
	}

	while (p_laser->m_condition_publish.is_alive())
	{
		p_laser->m_condition_publish.wait();
		if ( p_laser->m_condition_publish.is_alive() )
		{
			p_laser->publish_laser_to_message();
			//每隔300ms，发送一次雷达信息状态。
			std::this_thread::sleep_for(std::chrono::milliseconds(300));
		}
	}

	LOG(INFO) << " thread_publish end ";
	return;
}



//初始化变换矩阵，设置默认值
Error_manager Laser_base::init_laser_matrix()
{
	if ( LASER_MATRIX_ARRAY_SIZE == 12 )
	{
		//详见转化的算法transfor。保证转化前后坐标一致。
		mp_laser_matrix[0] = 1;
		mp_laser_matrix[1] = 0;
		mp_laser_matrix[2] = 0;
		mp_laser_matrix[3] = 0;
		mp_laser_matrix[4] = 0;
		mp_laser_matrix[5] = 1;
		mp_laser_matrix[6] = 0;
		mp_laser_matrix[7] = 0;
		mp_laser_matrix[8] = 0;
		mp_laser_matrix[9] = 0;
		mp_laser_matrix[10] = 1;
		mp_laser_matrix[11] = 0;
	}
	else
	{
		for (int i = 0; i < LASER_MATRIX_ARRAY_SIZE; ++i)
		{
			//设为0之后，变换之后，新的点坐标全部为0
			mp_laser_matrix[i] = 0;
		}
	}
	return Error_code::SUCCESS;
}
//设置变换矩阵，用作三维点的坐标变换，
Error_manager Laser_base::set_laser_matrix(double* p_matrix, int size)
{
	if ( p_matrix == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 " set_laser_matrix p_matrix IS_NULL ");
	}
	else if ( size != LASER_MATRIX_ARRAY_SIZE )
	{
	    return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
	    					" set_laser_matrix  size is not Match");
	}
	else
	{
		memcpy(mp_laser_matrix, p_matrix, LASER_MATRIX_ARRAY_SIZE * sizeof(double));
		return Error_code::SUCCESS;
	}
}
//三维点的坐标变换的功能函数，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
CPoint3D Laser_base::transform_by_matrix(CPoint3D point)
{
	CPoint3D result;
	double x = point.x;
	double y = point.y;
	double z = point.z;
	result.x = x * mp_laser_matrix[0] + y*mp_laser_matrix[1] + z*mp_laser_matrix[2] + mp_laser_matrix[3];
	result.y = x * mp_laser_matrix[4] + y*mp_laser_matrix[5] + z*mp_laser_matrix[6] + mp_laser_matrix[7];
	result.z = x * mp_laser_matrix[8] + y*mp_laser_matrix[9] + z*mp_laser_matrix[10] + mp_laser_matrix[11];
	return result;
}