Measure/Modules/MeasureNode/wanji_lidar/wj_716_lidar_protocol.h

#ifndef WJ_716_LIDAR_PROTOCOL_H
#define WJ_716_LIDAR_PROTOCOL_H
#include <iostream>
#include "string.h"
#include <boost/shared_ptr.hpp>
#include <boost/asio.hpp>
#include <boost/asio/placeholders.hpp>
#include <boost/system/error_code.hpp>
#include <boost/bind/bind.hpp>
#include <mutex>
#include <chrono>
#include <math.h>

#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl/common/common.h>

#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
using google::protobuf::Message;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::io::FileInputStream;
using google::protobuf::io::FileOutputStream;
using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::ZeroCopyOutputStream;

#include "glog/logging.h"

#include "wj_lidar_conf.pb.h"

#include "error_code/error_code.hpp"
#include "tool/binary_buf.h"
#include "tool/thread_safe_queue.hpp"
#include "tool/thread_condition.h"
#include "tool/point2D_tool.h"
#include "tool/common_data.h"

class Wj_716_lidar_protocol
{
public:
	//万集雷达解析协议状态
	enum Wanji_lidar_protocol_status
	{//default E_UNKNOWN = 0
		E_UNKNOWN               = 0,    //未知
		E_READY               	= 1,    //正常待机

		E_FAULT					= 10,	//故障
	};
	//万集雷达扫描周期66ms, (频率15hz), 一般设置大一些
#define WANJI_FRAME_SCAN_CYCLE_MS 		WANJI_716_SCAN_CYCLE_MS

//万集716的通信数据长度, 每帧都是固定 898 byte
#define WANJI_FRAME_LENGTH_716 		898
//万集716的数据头
#define WANJI_FRAME_HEAD_716 		0x02020202
//万集716的点数, 一共811个点, 分开发送时, 前者405个点, 后者406个点
#define WANJI_SCAN_POINT_NUMBER_TOTAL 		811
#define WANJI_SCAN_POINT_NUMBER_FORMER		405
#define WANJI_SCAN_POINT_NUMBER_LATTER		406
//万集716的角度范围, 角度-135~135, 弧度-2.35619449~2.35619449
#define WANJI_SCAN_POINT_ANGLE_MIN			-2.35619449
#define WANJI_SCAN_POINT_ANGLE_MAX			2.35619449
//万集716的角度分辨率, (每一度3个点),  135*2/(811-1)=0.333333333, 弧度0.005817764
#define WANJI_SCAN_POINT_ANGLE_INCREMENT	0.005817764

public:
	// 构造
	Wj_716_lidar_protocol();
	// 析构
	~Wj_716_lidar_protocol();

	// 初始化
	Error_manager init(Thread_safe_queue<Binary_buf*>* p_communication_data_queue,
					   Point2D_tool::Polar_coordinates_box polar_coordinates_box,
					   Point2D_tool::Point2D_box point2D_box,
					   Point2D_tool::Point2D_transform 	point2D_transform);
	//反初始化
	Error_manager uninit();

	//检查状态
	Error_manager check_status();
	//判断是否为待机，如果已经准备好，则可以执行任务。
	bool is_ready();
public:
	//获取状态
	Wanji_lidar_protocol_status get_status();

	//获取扫描周期
	int get_scan_cycle();

	// 获取扫描点云
	Error_manager get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out);
	//获取二次回波点云
	Error_manager get_two_echo_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out);
	// 获取扫描点云更新时间点
	std::chrono::system_clock::time_point get_scan_time();
	std::chrono::system_clock::time_point get_two_echo_time();

private:
	//解析线程函数
	void thread_analysis();
	//检查数据头和长度, 注:返回失败只是表示消息格式不对不是需要处理的消息, 忽略就好了
	bool check_head_and_length(Binary_buf* p_binary_buf);
	// 消息完整性检查, 检查末尾的校验码, 注:返回失败只是表示消息格式不对不是需要处理的消息, 忽略就好了
	bool check_xor(Binary_buf* p_binary_buf);
	// 万集通信协议, 解析数据, 转化为点云
	Error_manager data_protocol_analysis(Binary_buf* p_binary_buf);
	// 把点到雷达的距离, 转化为标准坐标下的点云
	Error_manager polar_coordinates_transform_cloud(float* p_point_distance, int number, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out);

protected:
	//状态
	std::atomic<Wanji_lidar_protocol_status>			m_wanji_lidar_protocol_status;	//万集雷达解析协议状态

	//数据
	Thread_safe_queue<Binary_buf*>*		mp_communication_data_queue;	//通信数据队列, 内存由上级管理
	//雷达扫描点的极坐标
	float 								m_scan_point_distance[WANJI_SCAN_POINT_NUMBER_TOTAL];//雷达扫描点的距离, 核心数据
	float 								m_scan_point_intensity[WANJI_SCAN_POINT_NUMBER_TOTAL];//雷达扫描点的光强,
	float 								m_two_echo_point_distance[WANJI_SCAN_POINT_NUMBER_TOTAL];//二次反射点的距离
	unsigned int 						m_current_frame_number;		//当前数据的帧号

	//雷达扫描点平面坐标
	pcl::PointCloud<pcl::PointXYZ>::Ptr mp_scan_points;				//扫描点的点云, 内存由本类管理
	pcl::PointCloud<pcl::PointXYZ>::Ptr mp_two_echo_points;			//二次反射点的点云, 内存由本类管理
	// 点云获取互斥锁
	std::mutex 							m_scan_mutex;				//扫描点的锁
	std::mutex 							m_two_echo_mutex;			//二次反射点的锁
	// 扫描点云更新时间点
	std::chrono::system_clock::time_point m_scan_cloud_time;		//扫描点的更新时间
	std::chrono::system_clock::time_point m_two_echo_cloud_time;	//二次反射点的更新时间

	//控制参数
	Point2D_tool::Polar_coordinates_box m_polar_coordinates_box;	//极坐标的限定范围
	Point2D_tool::Point2D_box			m_point2D_box;				//平面坐标的限定范围
	Point2D_tool::Point2D_transform 	m_point2D_transform;		//平面坐标的转换矩阵

	//解析数据的线程, 自动从队列取出原始数据, 然后解析为点云
	std::thread *						mp_thread_analysis;     	// 解析线程, 内存由本模块管理
	Thread_condition					m_condition_analysis;		//解析线程的条件变量
};

#endif // WJ_716_LIDAR_PROTOCOL_H