puai_wj_2021/laser/Laser.cpp

#include "Laser.h"
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <time.h>
#include <stdint.h>
#include <stdio.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()
{
	LOG(INFO) << " ---Laser_base::connect_laser() run--- "<< this;
	//检查雷达状态
	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
	{
		//清空队列
		m_queue_laser_data.clear_and_delete();
		//唤醒队列，
		m_queue_laser_data.wake_queue();


		//这里不建议用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()
{
	LOG(INFO) << " ---Laser_base::disconnect_laser() run--- "<< this;
	//终止队列，防止 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)
{
	LOG(INFO) << " --Laser_base::execute_task run--- "<< this;
	//检查指针
	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_status()
{
	if ( get_laser_statu() == LASER_READY  )
	{
		return Error_code::SUCCESS;
	}
	else if ( get_laser_statu() == LASER_BUSY  )
	{
		return Error_manager(Error_code::LASER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
							 " CLivoxLaser::check_status error ");
	}
	else
	{
		return Error_manager(Error_code::LASER_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " CLivoxLaser::check_status error ");
	}
	return Error_code::SUCCESS;
}
//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
Error_manager Laser_base::start_scan()
{
	LOG(INFO) << " ---Laser_base::start_scan ---"<< this;
	if ( is_ready() )
	{
		//重置数据
		m_points_count=0;
		m_queue_laser_data.clear_and_delete();
		m_last_data.clear();

		//启动雷达扫描
		m_laser_scan_flag=true;				//启动雷达扫描
		m_laser_statu = LASER_BUSY;			//雷达状态 繁忙
		m_condition_receive.notify_all(true);		//唤醒接受线程
		m_condition_transform.notify_all(true);	//唤醒转化线程

		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()
{
	if(m_laser_scan_flag )
	{
		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;
	stop_scan();
	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;
			}
			mp_laser_task->set_task_statu(TASK_OVER);
		}
		else
		{
			//故障等级为 MAJOR_ERROR CRITICAL_ERROR，则雷达故障，不可以再接受任务单。
			m_laser_statu = LASER_FAULT;
			mp_laser_task->set_task_statu(TASK_ERROR);
		}
	}
	return Error_code::SUCCESS;
}

//取消任务单，由发送方提前取消任务单
Error_manager Laser_base::cancel_task()
{
	end_task();
	//强制改为 TASK_DEAD，不管它当前在做什么。
	mp_laser_task->set_task_statu(TASK_DEAD);
	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
	{
		uint32_t beginCmpPath = 0;
		uint32_t endCmpPath = 0;
		std::string fullPath = save_path;

		if (fullPath[fullPath.size() - 1] != '/')
		{
			fullPath += "/";
		}
		fullPath += "laser_data/";
		endCmpPath = fullPath.size();

		//create dirs;
		for(uint32_t i = beginCmpPath; i < endCmpPath ; i++ )
		{
			if('/' == fullPath[i])
			{
				std::string curPath = fullPath.substr(0, i);
				if(access(curPath.c_str(), F_OK) != 0)
				{
					if(mkdir(curPath.c_str(),0777) == -1)
					{
						printf("mkdir(%s) failed\n", curPath.c_str());
						return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
											 "set_open_save_path mkdir error ");
					}
				}
			}
		}


		time_t nowTime;
		nowTime = time(NULL);
		struct tm* sysTime = localtime(&nowTime);
		char time_string[256] = { 0 };
		sprintf(time_string, "%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);

		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);
		sprintf(pts_file, "%s/laser_pst_%s__%d.txt", fullPath.c_str(), time_string, m_laser_id+0);
		m_points_log_tool.open(pts_file);

//		char bin_file[255] = { 0 };
////		sprintf(bin_file, "%s/laser%d.data", save_path.c_str(), m_laser_id+1);
//		sprintf(bin_file, "%s/laser_binary_%s__%d.data", fullPath.c_str(),time_string, m_laser_id+1);
//		m_binary_log_tool.open(bin_file,true);
//		std::cout << "huli m_binary_log_tool path "<< bin_file << std::endl;

		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;
	Error_manager t_error;

	//接受雷达消息，每次循环只接受一个 Binary_buf
	while (m_condition_receive.is_alive())
	{
		m_condition_receive.wait();
		if ( m_condition_receive.is_alive() )
		{
			std::this_thread::yield();
			//获取雷达的通信消息缓存, ，并将其存入 m_queue_laser_data, 每次只转化一个CBinaryData，(循环执行)
			t_error = receive_buf_to_queue();
			if (t_error == SUCCESS)
			{
				//这里非常特殊,receive_buf_to_queue会为 Binary_buf 分配内存. 是new出来的
				//此时 Binary_buf 的内存管理权限转交给 m_queue_laser_data .之后由 Laser_base 负责管理.
				;
			}
				//接受线程无法接受到数据的同时，也收到了雷达停止扫描的信息，那么线程停止
				//如果m_laser_scan_flag停止扫描，但是receive_buf_to_queue仍然能够接受到数据。那么线程继续运行。
				//m_laser_scan_flag停止的瞬间，可能还有残留的数据，需要继续执行。
			else if(t_error == NO_DATA  )
			{
				if ( !m_laser_scan_flag )
				{
					//停止线程，m_condition_receive.wait() 函数将会阻塞。正常停止接受线程
					m_condition_receive.set_pass_ever(false);
					//注:此时thread_transform线程仍然继续,任务也没有停止.
				}
			}
			else
			{
				//因为故障,而提前结束任务.
				mp_laser_task->compare_and_cover_task_error_manager(t_error);
				end_task();
			}
		}
	}
	LOG(INFO) << " thread_receive end :"<<this;
	return;
}


//线程执行函数，转化并处理三维点云。
void Laser_base::thread_transform()
{
	LOG(INFO) << " thread_transform start "<< this;
	Error_manager t_error;

	//转化雷达数据，每次循环只转化一个 Binary_buf，然后得到 t_point3D_cloud.size() 个三维点，
	while (m_condition_transform.is_alive())
	{
		m_condition_transform.wait();
		if ( m_condition_transform.is_alive() )
		{
			std::this_thread::yield();
			t_error = transform_buf_to_points();
			if ( t_error == SUCCESS )
			{
				;
			}
				//转化线程 不需要数据的同时，也收到了雷达停止扫描的信息并且接受线程已经停止，那么停止转化线程，
				//注：必须先停止接受线程，再停止转化线程。转化线程停止之后即可结束任务。
				//如果m_laser_scan_flag停止扫描，接受线程和转化线程还有未完成的数据。那么线程继续运行。
				//m_laser_scan_flag停止的瞬间，可能还有残留的数据，需要继续执行。防止数据遗留。
			else if (t_error == NO_DATA )
			{
				if(m_laser_scan_flag == false && m_condition_receive.get_pass_ever() == false)
				{
					//停止转化线程，m_condition_transform.wait() 函数将会阻塞。
					m_condition_transform.set_pass_ever(false);

					//mp_thread_transform 停止时，雷达扫描任务彻底完成，此时任务正常结束
					end_task();
				}
			}
			else
			{
				//因为故障,而提前结束任务.
				mp_laser_task->compare_and_cover_task_error_manager(t_error);
				end_task();
			}

		}
	}
	LOG(INFO) << " thread_transform end "<< this;
	return;
}


//公开发布雷达信息的功能函数，
Error_manager Laser_base::publish_laser_to_message()
{
	return Error_code::SUCCESS;
	/*
	globalmsg::msg msg;
	msg.set_msg_type(globalmsg::eLaser);

	globalmsg::laserStatus status;
	if(GetStatu()==eLaser_ready) status=globalmsg::eLaserConnected;
	else if(GetStatu()==eLaser_disconnect) status=globalmsg::eLaserDisconnected;
	else if(GetStatu()==eLaser_busy) status=globalmsg::eLaserBusy;
	else  status=globalmsg::eLaserUnknown;

	msg.mutable_laser_msg()->set_id(m_laser_id);
	msg.mutable_laser_msg()->set_laser_status(status);
	msg.mutable_laser_msg()->set_queue_data_count(m_queue_laser_data.size());
	msg.mutable_laser_msg()->set_cloud_count(m_points_count);
	MeasureTopicPublisher::GetInstance()->Publish(msg.SerializeAsString());
	 */
}
//线程执行函数，公开发布雷达的相关信息，用作上位机的监视。
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;
	}
	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;
}