Measure/Modules/MeasureNode/velodyne_lidar/velodyne_driver/velodyne_lidar_device.cpp

#include "velodyne_lidar_device.h"

// 万集雷达封装类构造函数
Velodyne_lidar_device::Velodyne_lidar_device()
{
	m_velodyne_lidar_device_status = E_UNKNOWN;
	mp_velodyne_lidar_scan_task = NULL;
}

// 万集雷达封装类析构函数
Velodyne_lidar_device::~Velodyne_lidar_device()
{
	uninit();
}

void Velodyne_lidar_device::dev_info_config(std::string model, int rpm)
{
	double packet_rate; // packet frequency (Hz)
	std::string model_full_name;
	if ((model == "VLS128"))
	{
		packet_rate = 6253.9; //  3 firing cycles in a data packet. 3 x 53.3 μs = 0.1599 ms is the accumulation delay per packet.
							  //   1 packet/0.1599 ms = 6253.9 packets/second

		model_full_name = model;
	}
	else if ((model == "64E_S2") ||
			 (model == "64E_S2.1")) // generates 1333312 points per second
	{								// 1 packet holds 384 points
		packet_rate = 3472.17;		// 1333312 / 384
		model_full_name = std::string("HDL-") + model;
	}
	else if (model == "64E")
	{
		packet_rate = 2600.0;
		model_full_name = std::string("HDL-") + model;
	}
	else if (model == "64E_S3") // generates 2222220 points per second (half for strongest and half for lastest)
	{							// 1 packet holds 384 points
		packet_rate = 5787.03;	// 2222220 / 384
		model_full_name = std::string("HDL-") + model;
	}
	else if (model == "32E")
	{
		packet_rate = 1808.0;
		model_full_name = std::string("HDL-") + model;
	}
	else if (model == "32C")
	{
		packet_rate = 1507.0;
		model_full_name = std::string("VLP-") + model;
	}
	else if (model == "VLP16")
	{
		packet_rate = 754; // 754 Packets/Second for Last or Strongest mode 1508 for dual (VLP-16 User Manual)
		model_full_name = "VLP-16";
	}
	else
	{
		LOG(WARNING) << "unknown Velodyne LIDAR model: " << model;
		packet_rate = 2600.0;
	}
	std::string deviceName(std::string("Velodyne ") + model_full_name);

	LOG(INFO) << deviceName << " rotating at " << rpm << " RPM";
	double frequency = (rpm / 60.0); // expected Hz rate

	// default number of packets for each scan is a single revolution
	// (fractions rounded up)
	m_num_packet_per_scan = (int)ceil(packet_rate / frequency);
	LOG(INFO) << "publishing " << m_num_packet_per_scan << " packets per scan";
}

// 初始化
Error_manager Velodyne_lidar_device::init(velodyne::velodyneLidarParams params)
{
	m_params = params;
	Error_manager t_error;
	LOG(INFO) << " Velodyne_lidar_device::init " << this;
	m_num_packet_inside = 0;
	dev_info_config(params.model(), params.rpm());
	m_velodyne_lidar_device_status = E_UNKNOWN;
	m_velodyne_socket_status = E_UNKNOWN;
	m_velodyne_protocol_status = E_UNKNOWN;
	m_lidar_id = params.lidar_id();

	// 设置雷达内参矩阵
	Eigen::AngleAxisd rot_x = Eigen::AngleAxisd(params.calib().r()*M_PI/180.0, Eigen::Vector3d::UnitX());
	Eigen::AngleAxisd rot_y = Eigen::AngleAxisd(params.calib().p()*M_PI/180.0, Eigen::Vector3d::UnitY());
	Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(params.calib().y()*M_PI/180.0, Eigen::Vector3d::UnitZ());
	Eigen::Vector3d trans(params.calib().cx(), params.calib().cy(), params.calib().cz());
	m_calib_matrix << rot_x.toRotationMatrix() * rot_y.toRotationMatrix() * rot_z.toRotationMatrix(), trans, 0.0, 0.0, 0.0, 1.0;
	// LOG(INFO) <<"lidar "<<m_lidar_id<<",,,\n"<< rot_x.toRotationMatrix()<<"------------------\n"<<rot_y.toRotationMatrix()<<"-----------------\n"<<rot_z.toRotationMatrix()<<"---------------\n"<<m_calib_matrix;

	//唤醒队列
	m_communication_data_queue.wake_queue();

//	//初始化通信协议
//	t_error = m_protocol.setup(params.model(), params.calibrationfile(), params.max_range(), params.min_range(), params.min_angle(), params.max_angle()) == true ? SUCCESS : Error_manager(PARAMETER_ERROR, MINOR_ERROR, "protocol parameter error");
//	if (t_error != SUCCESS)
//	{
//		return t_error;
//	}
	m_velodyne_protocol_status = E_READY;

	//初始化雷达通信模块
//	std::string t_ip = params.ip();
//	int t_port = params.port();

    m_cloud_data = new CloudDataMqttAsync;
	t_error = m_cloud_data->init() ? SUCCESS : Error_manager(VELODYNE_LIDAR_CONNECT_FAILED, MINOR_ERROR, "socket connect failed");
	if (t_error != SUCCESS)
	{
//        LOG(ERROR) << "Velodyne_lidar_device::init failed: ip =  " << t_ip << " : " << t_port;
        LOG(ERROR) << "Velodyne_lidar_device::init failed: error =  " << t_error.to_string();
		return t_error;
	}
	m_velodyne_socket_status = E_READY;

	//启动 内部线程。默认wait。
	m_execute_condition.reset(false, false, false);
	mp_execute_thread = new std::thread(&Velodyne_lidar_device::execute_thread_fun, this);

	m_velodyne_lidar_device_status = E_READY;

	return t_error;
}

//反初始化
Error_manager Velodyne_lidar_device::uninit()
{
    // //终止队列，防止 wait_and_pop 阻塞线程。
	m_communication_data_queue.termination_queue();
	m_communication_data_queue.clear();
	//关闭线程
	//回收线程的资源
	if (mp_execute_thread)
	{
		m_execute_condition.kill_all();
		mp_execute_thread->join();
		delete mp_execute_thread;
		mp_execute_thread = nullptr;
	}
    delete m_cloud_data;
    m_cloud_data = nullptr;
	//清空队列
	m_communication_data_queue.clear_and_delete();

	if (m_velodyne_lidar_device_status != E_FAULT)
	{
		m_velodyne_lidar_device_status = E_UNKNOWN;
	}
	return Error_code::SUCCESS;
}

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

	//检查指针
	if (p_velodyne_lidar_scan_task == nullptr)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Velodyne_lidar_device::execute_task failed, POINTER_IS_NULL");
	}
	//检查任务类型，
	if (p_velodyne_lidar_scan_task->get_task_type() != VELODYNE_LIDAR_SCAN)
	{
		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 " task type error ！= VELODYNE_LIDAR_SCAN ");
	}

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

		//检查消息内容是否正确，
		//检查三维点云指针
		if (mp_velodyne_lidar_scan_task->get_task_point_cloud().get() == nullptr)
		{
			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
										Error_level::MINOR_ERROR,
										"execute_task mp_task_point_cloud is null");
			t_result.compare_and_cover_error(t_error);
		}
		else if (mp_velodyne_lidar_scan_task->get_task_cloud_lock() == nullptr)
		{
			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
										Error_level::MINOR_ERROR,
										"execute_task mp_task_cloud_lock is null");
			t_result.compare_and_cover_error(t_error);
		}
		else
		{
			m_velodyne_lidar_device_status = E_BUSY;
			//启动雷达管理模块，的核心工作线程
			m_execute_condition.notify_all(true);
			//通知 thread_work 子线程启动。

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

	return t_result;
}

//检查雷达状态,是否正常运行
Error_manager Velodyne_lidar_device::check_status()
{
	updata_status();
	if (m_velodyne_lidar_device_status == E_READY)
	{
		return Error_code::SUCCESS;
	}
	else if (m_velodyne_lidar_device_status == E_BUSY)
	{
		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
							 "  Velodyne_lidar_device::check_status() error ");
	}
	else
	{
		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " Velodyne_lidar_device::check_status() error ");
	}
	return Error_code::SUCCESS;
}

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

	if (m_velodyne_lidar_device_status == E_BUSY)
	{
		m_velodyne_lidar_device_status = E_READY;
	}
	//else 状态不变

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

	return Error_code::SUCCESS;
}

//取消任务单，由发送方提前取消任务单
Error_manager Velodyne_lidar_device::cancel_task(Task_Base *p_velodyne_lidar_scan_task)
{
	if (p_velodyne_lidar_scan_task == mp_velodyne_lidar_scan_task)
	{
		m_execute_condition.notify_all(false);

		if (m_velodyne_lidar_device_status == E_BUSY)
		{
			m_velodyne_lidar_device_status = E_READY;
		}
		//else 状态不变

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

		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
							 " Velodyne_lidar_device::cancel_task PARAMRTER ERROR ");
	}
}

//判断是否为待机，如果已经准备好，则可以执行任务。
bool Velodyne_lidar_device::is_ready()
{
	updata_status();
	return m_velodyne_lidar_device_status == E_READY;
}

//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
Error_manager Velodyne_lidar_device::get_new_cloud_and_wait(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
															std::chrono::steady_clock::time_point command_time, int timeout_ms)
{
	if (p_mutex == NULL || p_cloud.get() == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	//判断是否超时
	while (std::chrono::steady_clock::now() - command_time < std::chrono::milliseconds(timeout_ms))
	{
		//获取指令时间之后的点云, 如果没有就会循环
		if (m_parse_ring_time > command_time)
		{
			std::unique_lock<std::mutex> lck(*p_mutex);
			{
				std::lock_guard<std::mutex> lck(m_cloud_mutex);
                m_cloud_data->getCloudData("", p_cloud);
                if (p_cloud->empty()) {
                    return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
                }
			}
			return SUCCESS;
		}
		//else 等待下一次数据更新

		//等1ms
		std::this_thread::sleep_for(std::chrono::milliseconds(1));
	}
	//超时退出就报错
	return Error_manager(Error_code::VELODYNE_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
						 " Velodyne_lidar_device::get_new_cloud_and_wait error ");
}

//外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
Error_manager Velodyne_lidar_device::get_current_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
													   std::chrono::steady_clock::time_point command_time)
{
	if (p_mutex == NULL || p_cloud.get() == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	//获取指令时间之后的点云, 如果没有就会报错, 不会等待
	if (m_parse_time > command_time)
	{
		std::unique_lock<std::mutex> lck(*p_mutex);
		{
			std::lock_guard<std::mutex> lck(m_cloud_mutex);
			m_cloud_data->getCloudData("", p_cloud);
            if (p_cloud->empty()) {
                return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
            }
		}
		return SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
							 " Velodyne_lidar_device::get_current_cloud error ");
	}
}

//外部调用获取最新的点云, 如果没有就会报错, 不会等待
Error_manager Velodyne_lidar_device::get_last_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
													std::chrono::steady_clock::time_point command_time)
{
	if (p_mutex == nullptr || p_cloud.get() == nullptr)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	if (m_parse_ring_time > command_time - std::chrono::milliseconds(int(60000.0/(m_params.rpm()*0.5))))
	{
		std::unique_lock<std::mutex> lck(*p_mutex);
		{
			std::lock_guard<std::mutex> lck(m_cloud_mutex);
			m_cloud_data->getCloudData("", p_cloud);
            if (p_cloud->empty()) {
                return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
            }
		}
		return SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
							 " Velodyne_lidar_device::get_current_cloud error ");
	}
}

//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
//注:函数内部不加锁, 由外部统一加锁.
Error_manager Velodyne_lidar_device::get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
													std::chrono::steady_clock::time_point command_time)
{
	if (p_cloud.get() == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	if (m_parse_ring_time > command_time - std::chrono::milliseconds(int(60000.0/(m_params.rpm()*0.5))))
	{
		std::lock_guard<std::mutex> lck(m_cloud_mutex);
        m_cloud_data->getCloudData("", p_cloud);
        if (p_cloud->empty()) {
            return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
        }
		return SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
							 " Velodyne_lidar_device::get_current_cloud error ");
	}
}

Velodyne_lidar_device::Velodyne_lidar_device_status Velodyne_lidar_device::get_status()
{
	updata_status();
	return m_velodyne_lidar_device_status;
}

void Velodyne_lidar_device::updata_status()
{
	if (m_velodyne_socket_status == E_FAULT || m_velodyne_protocol_status == E_FAULT)
	{
		m_velodyne_lidar_device_status = E_FAULT;
	}else if (m_velodyne_socket_status == E_DISCONNECT)
	{
		m_velodyne_lidar_device_status = E_DISCONNECT;
	}
	else if(m_velodyne_socket_status == E_BUSY || m_velodyne_protocol_status == E_BUSY)
    {
        m_velodyne_lidar_device_status = E_BUSY;
    }
	else if (m_velodyne_socket_status == E_READY && m_velodyne_protocol_status == E_READY)
	{
		if (m_execute_condition.get_pass_ever())
		{
			m_velodyne_lidar_device_status = E_BUSY;
		}
		else
		{
			m_velodyne_lidar_device_status = E_READY;
		}
	}
	else
	{
//	    LOG(WARNING)<<"unknown????? "<<m_velodyne_socket_status<<", "<<m_velodyne_protocol_status;
		m_velodyne_lidar_device_status = E_UNKNOWN;
	}
}

//mp_laser_manager_thread 线程执行函数，
//thread_work 内部线程负责获取点云结果
void Velodyne_lidar_device::execute_thread_fun()
{
	LOG(INFO) << " Velodyne_lidar_device::execute_thread_fun() start " << this;
	Error_manager t_error;

	//雷达管理的独立线程，负责控制管理所有的雷达。
	while (m_execute_condition.is_alive())
	{
		m_execute_condition.wait();
		if (m_execute_condition.is_alive())
		{
			std::this_thread::yield();

			if (mp_velodyne_lidar_scan_task == nullptr)
			{
				//没有任务就忽略, 直接关闭线程
				m_velodyne_lidar_device_status = E_READY;
				m_execute_condition.notify_all(false);
			}
			else
			{
				if (mp_velodyne_lidar_scan_task->is_over_time())
				{
					t_error.error_manager_reset(TASK_TIMEOVER, MINOR_ERROR, "Velodyne_lidar_device::get_cloud_thread() TASK_TIMEOVER");
					mp_velodyne_lidar_scan_task->compare_and_cover_task_error_manager(t_error);
					//故障结束任务
					end_task();
				}
				else
				{
					//获取这个时间后面的点云.
					if (m_parse_ring_time > mp_velodyne_lidar_scan_task->get_command_time())
					{
						std::mutex *tp_mutex = mp_velodyne_lidar_scan_task->get_task_cloud_lock();
						pcl::PointCloud<pcl::PointXYZ>::Ptr tp_cloud = mp_velodyne_lidar_scan_task->get_task_point_cloud();
						{
							std::unique_lock<std::mutex> lck(*tp_mutex);
							{
								std::lock_guard<std::mutex> lck(m_cloud_mutex);
								// 转换lidar points为pcl点云
                                m_cloud_data->getCloudData("", tp_cloud);
							}
						}
						mp_velodyne_lidar_scan_task->compare_and_cover_task_error_manager(t_error);
						//正常结束任务
						end_task();
					}
					else
					{
						//等1ms
						std::this_thread::sleep_for(std::chrono::milliseconds(1));
					}
				}
			}
		}
	}
	LOG(INFO) << " Velodyne_lidar_device::execute_thread_fun() end :" << this;
	return;
}