clamp_safety/lidar/wj_716N_lidar_protocol.h

#ifndef WJ_716N_LIDAR_PROTOCOL_HH
#define WJ_716N_LIDAR_PROTOCOL_HH
#include <iostream>
#include <mutex>
#include <chrono>
#include <math.h>

#include "glog/logging.h"
#include "string.h"

#include "../error_code/error_code.h"
#include "../tool/binary_buf.h"
#include "../tool/thread_safe_queue.h"
#include "../tool/thread_condition.h"
#include "../tool/point2D_tool.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 "pcl/point_types.h"
#include "pcl/point_cloud.h"
#include "pcl/conversions.h"
#include "pcl/common/common.h"

// using namespace std;

typedef struct {
    std::string frame_id="laser";
    double min_ang=-2.35619449;
    double max_ang=2.35619449;
    double range_min=0.0;
    double range_max=30.0;
    int frequency_scan=1;
}wj_716_lidarConfig;

#define MAX_LENGTH_DATA_PROCESS 200000
typedef struct TagDataCache
{
    unsigned char m_acdata[MAX_LENGTH_DATA_PROCESS];
    unsigned int m_u32in;
    unsigned int m_u32out;
}DataCache;


struct Stamp            //    Laser内参
{
    //long secs;
    long msecs;
};

struct Header           //    Laser内参
{
    int seq;
    //替换成只用ms表示
    Stamp stamp;
    std::string frame_id;
};
struct Laser            //    雷达数据及雷达参数
{
    Header header;
    double angle_min;
    double angle_max;
    double angle_increment;

    double range_min;
    double range_max;

    double time_increment;
    long long scan_time;

    std::vector<double> ranges;
    std::vector<double> x;
    std::vector<double> y;
    std::vector<double> angle_to_point;
    std::vector<double> intensities;
};


class wj_716N_lidar_protocol{
public:
    wj_716N_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();

    //获取扫描周期
	int get_scan_cycle(){return (config_.frequency_scan==1?67:40);}
	// 获取扫描点云
	Error_manager get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out);
	// 获取扫描点云更新时间点
	std::chrono::system_clock::time_point get_scan_time(){return std::chrono::time_point<std::chrono::system_clock, std::chrono::milliseconds>(std::chrono::milliseconds(scan.header.stamp.msecs));}

    bool dataProcess(unsigned char *data,const int reclen);
    bool protocl(unsigned char *data,const int len);
    bool OnRecvProcess(unsigned char *data, int len);
    bool checkXor(unsigned char *recvbuf, int recvlen);
    void send_scan(const char *data,const int len);

    bool setConfig(wj_716_lidarConfig &new_config,uint32_t level);

    void update_data(int index);

//    bool heartstate;

//    wj_716N_lidar::Laser scan;
//    std::mutex m_read_write_mtx;


private:

    //解析线程函数
	void thread_analysis();
    void movedata(DataCache &sdata);
    DataCache   m_sdata;
    wj_716_lidarConfig config_;
    unsigned int m_u32PreFrameNo;
    unsigned int m_u32ExpectedPackageNo;
    int m_n32currentDataNo;
    float scandata[1081];
    float scanintensity[1081];

 public:
    Laser scan;
    int freq_scan;
    int resizeNum;
    int index_start;
    int index_end;
    bool heartstate;
    int total_point;
    std::mutex m_read_write_mtx;

    Thread_safe_queue<Binary_buf*>*		mp_communication_data_queue;	//通信数据队列, 内存由上级管理

    //雷达扫描点平面坐标
	pcl::PointCloud<pcl::PointXYZ>::Ptr mp_scan_points;				//扫描点的点云, 内存由本类管理
	// 点云获取互斥锁
	std::mutex 							m_scan_mutex;				//扫描点的锁

    //控制参数
	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_716N_LIDAR_PROTOCOL_HH