Measure/Modules/ClampSafety/lidar/wj_716N_lidar_protocol.h

#pragma once

#include <iostream>
#include <mutex>
#include <chrono>
#include <math.h>

#include "glog/logging.h"
#include "string.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 <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"

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 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() const;

    // 获取扫描点云
    Error_manager get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out);

    // 获取扫描点云更新时间点
    std::chrono::system_clock::time_point get_scan_time() const;

    bool setConfig(wj_716_lidarConfig &new_config, uint32_t level);

    void update_data(int index);

private:
    //解析线程函数
    void thread_analysis();

    void movedata(DataCache &sdata);

    bool dataProcess(unsigned char *data, int reclen);

    bool OnRecvProcess(unsigned char *data, int len);

    bool checkXor(unsigned char *recvbuf, int recvlen);

    bool protocl(unsigned char *data, int len);


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;            // 点云获取互斥锁
    std::mutex m_scan_mutex;                // 扫描点的锁

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

    // 雷达扫描点平面坐标
    pcl::PointCloud<pcl::PointXYZ>::Ptr mp_scan_points;                //扫描点的点云, 内存由本类管理

    // 控制参数
    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;          //解析线程的条件变量

private:
    DataCache m_sdata;
    wj_716_lidarConfig config_;
    unsigned int m_u32PreFrameNo;
    unsigned int m_u32ExpectedPackageNo;
    int   m_n32currentDataNo;
    float scandata[1081];
    float scanintensity[1081];

};