Measure/Modules/ClampSafety/lidar/wj_716N_lidar_protocol.cpp

#include "wj_716N_lidar_protocol.h"
#include <iostream>


wj_716N_lidar_protocol::wj_716N_lidar_protocol() {
    memset(&m_sdata, 0, sizeof(m_sdata));
    long scan_time = std::chrono::duration_cast<std::chrono::milliseconds>(
            std::chrono::system_clock::now().time_since_epoch()).count();
    scan.header.stamp.msecs = scan_time;

    mp_communication_data_queue = nullptr;

    freq_scan = 1;
    m_u32PreFrameNo = 0;
    m_u32ExpectedPackageNo = 0;
    m_n32currentDataNo = 0;
    total_point = 1081;

    scan.header.frame_id = "wj_716N_lidar_frame";
    scan.angle_min = -2.35619449;
    scan.angle_max = 2.35619449;
    scan.angle_increment = 0.017453 / 4;
    scan.time_increment = 1 / 15.00000000 / 1440;
    scan.range_min = 0;
    scan.range_max = 30;
    scan.ranges.resize(1081);
    scan.intensities.resize(1081);
    scan.x.resize(1081);
    scan.y.resize(1081);
    scan.angle_to_point.resize(1081);

    index_start = (config_.min_ang + 2.35619449) / scan.angle_increment;
    // adjust angle_max to max_ang config param
    index_end = 1081 - ((2.35619449 - config_.max_ang) / scan.angle_increment);

    mp_scan_points = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    mp_thread_analysis = nullptr;

    std::cout << "wj_716N_lidar_protocl start success" << std::endl;
}

bool wj_716N_lidar_protocol::setConfig(wj_716_lidarConfig &new_config, uint32_t level) {

    config_ = new_config;
    scan.header.frame_id = config_.frame_id;
    scan.angle_min = config_.min_ang;
    scan.angle_max = config_.max_ang;
    scan.range_min = config_.range_min;
    scan.range_max = config_.range_max;
    freq_scan = config_.frequency_scan;

    scan.angle_increment = 0.017453 / 4;
    if (freq_scan == 1) //0.25°_15hz
    {
        scan.time_increment = 1 / 15.00000000 / 1440;
        total_point = 1081;
    } else if (freq_scan == 2) //0.25°_25hz
    {
        scan.time_increment = 1 / 25.00000000 / 1440;
        total_point = 1081;
    }

    // adjust angle_min to min_ang config param
    index_start = (config_.min_ang + 2.35619449) / scan.angle_increment;

    // adjust angle_max to max_ang config param
    index_end = 1081 - ((2.35619449 - config_.max_ang) / scan.angle_increment);
    int samples = index_end - index_start;

    std::cout << "frame_id:" << scan.header.frame_id << std::endl;
    std::cout << "min_ang:" << scan.angle_min << std::endl;
    std::cout << "max_ang:" << scan.angle_max << std::endl;
    std::cout << "angle_increment:" << scan.angle_increment << std::endl;
    std::cout << "time_increment:" << scan.time_increment << std::endl;
    std::cout << "range_min:" << scan.range_min << std::endl;
    std::cout << "range_max:" << scan.range_max << std::endl;
    std::cout << "samples_per_scan:" << samples << std::endl;
    return true;
}

// 初始化
Error_manager wj_716N_lidar_protocol::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) {
    if (p_communication_data_queue == NULL) {
        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                             "  POINTER IS NULL ");
    }
    wj_716_lidarConfig t_config;
    t_config.min_ang = polar_coordinates_box.angle_min;
    t_config.max_ang = polar_coordinates_box.angle_max;
    t_config.range_min = polar_coordinates_box.distance_min;
    t_config.range_max = polar_coordinates_box.distance_max;
    t_config.frequency_scan = 1;
    setConfig(t_config, 0);

    mp_communication_data_queue = p_communication_data_queue;
    m_point2D_box = point2D_box;
    std::cout << "box xmin:" << m_point2D_box.x_min << std::endl;
    std::cout << "box xmax:" << m_point2D_box.x_max << std::endl;
    std::cout << "box ymin:" << m_point2D_box.y_min << std::endl;
    std::cout << "box ymax:" << m_point2D_box.y_max << std::endl;
    m_point2D_transform = point2D_transform;
    //接受线程, 默认开启循环, 在内部的wait_and_pop进行等待
    m_condition_analysis.reset(false, true, false);
    mp_thread_analysis = new std::thread(&wj_716N_lidar_protocol::thread_analysis, this);

    return Error_code::SUCCESS;
}

//反初始化
Error_manager wj_716N_lidar_protocol::uninit() {
    LOG(INFO) << " ---wj_716N_lidar_protocol uninit --- " << this;

    if (mp_communication_data_queue) {
        //终止队列，防止 wait_and_pop 阻塞线程。
        mp_communication_data_queue->termination_queue();
    }

    //关闭线程
    if (mp_thread_analysis) {
        m_condition_analysis.kill_all();
    }
    //回收线程的资源
    if (mp_thread_analysis) {
        mp_thread_analysis->join();
        delete mp_thread_analysis;
        mp_thread_analysis = nullptr;
    }

    if (mp_communication_data_queue) {
        //清空队列
        mp_communication_data_queue->clear_and_delete();
    }
    //队列的内存由上级管理, 这里写空就好了.
    mp_communication_data_queue = nullptr;

    return Error_code::SUCCESS;
}

//获取扫描点云
Error_manager wj_716N_lidar_protocol::get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out) {
    if (p_cloud_out.get() == nullptr) {
        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                             " Wj_716_lidar_protocol::get_scan_points POINTER IS NULL ");
    }
    std::unique_lock<std::mutex> lck(m_scan_mutex);
    //将扫描点云追加到p_cloud_out后面, 不要清除p_cloud_out原有的数据
    (*p_cloud_out) += (*mp_scan_points);

    return Error_code::SUCCESS;
}

//解析线程函数
void wj_716N_lidar_protocol::thread_analysis() {
    LOG(INFO) << " ---Wj_716_lidar_protocol::thread_analysis start ---" << this;

    Error_manager t_error;
    //接受雷达消息，包括连接,重连和接受数据
    while (m_condition_analysis.is_alive()) {
        m_condition_analysis.wait();
        if (m_condition_analysis.is_alive()) {
            std::this_thread::yield();

            if (mp_communication_data_queue != nullptr) {
                Binary_buf *tp_binary_buf = nullptr;
                bool is_pop = mp_communication_data_queue->wait_and_pop(tp_binary_buf);
                if (is_pop && tp_binary_buf != nullptr) {
                    if (dataProcess((unsigned char *) tp_binary_buf->get_buf(), tp_binary_buf->get_length())) {
                        t_error = SUCCESS;
                    } else {
                        t_error = ERROR;
                    }
                    //else 错误不管, 当做无效消息处理
                    delete (tp_binary_buf);
                }
            }
        }
    }
    LOG(INFO) << " -Wj_716_lidar_protocol::thread_analysis end :" << this;
}


bool wj_716N_lidar_protocol::dataProcess(unsigned char *data, const int reclen) {

    if (reclen > MAX_LENGTH_DATA_PROCESS) {
        m_sdata.m_u32out = 0;
        m_sdata.m_u32in = 0;
        return false;
    }

    if (m_sdata.m_u32in + reclen > MAX_LENGTH_DATA_PROCESS) {
        m_sdata.m_u32out = 0;
        m_sdata.m_u32in = 0;
        return false;
    }
    memcpy(&m_sdata.m_acdata[m_sdata.m_u32in], data, reclen * sizeof(char));
    m_sdata.m_u32in += reclen;
    while (m_sdata.m_u32out < m_sdata.m_u32in) {
        if (m_sdata.m_acdata[m_sdata.m_u32out] == 0xFF && m_sdata.m_acdata[m_sdata.m_u32out + 1] == 0xAA) {
            unsigned l_u32reallen = (m_sdata.m_acdata[m_sdata.m_u32out + 2] << 8) |
                                    (m_sdata.m_acdata[m_sdata.m_u32out + 3] << 0);
            l_u32reallen = l_u32reallen + 4;

            if (l_u32reallen <= (m_sdata.m_u32in - m_sdata.m_u32out + 1)) {
                if (OnRecvProcess(&m_sdata.m_acdata[m_sdata.m_u32out], l_u32reallen)) {
                    m_sdata.m_u32out += l_u32reallen;
                } else {
                    LOG_EVERY_N(WARNING, 10) << "continuous search frame header";
                    m_sdata.m_u32out++;
                }
            } else if (l_u32reallen >= MAX_LENGTH_DATA_PROCESS) {
                m_sdata.m_u32out++;
            } else {
                break;
            }
        } else {
            m_sdata.m_u32out++;
        }
    } //end while(m_sdata.m_u32out < m_sdata.m_u32in)

    if (m_sdata.m_u32out >= m_sdata.m_u32in) {
        m_sdata.m_u32out = 0;
        m_sdata.m_u32in = 0;
    } else if (m_sdata.m_u32out < m_sdata.m_u32in && m_sdata.m_u32out != 0) {
        movedata(m_sdata);
    }
    return true;
}

void wj_716N_lidar_protocol::movedata(DataCache &sdata) {
    for (int i = sdata.m_u32out; i < sdata.m_u32in; i++) {
        sdata.m_acdata[i - sdata.m_u32out] = sdata.m_acdata[i];
    }
    sdata.m_u32in = sdata.m_u32in - sdata.m_u32out;
    sdata.m_u32out = 0;
}

bool wj_716N_lidar_protocol::OnRecvProcess(unsigned char *data, int len) {
    if (len > 0) {
        if (checkXor(data, len)) {
            protocl(data, len);
        } else {
            return false;
        }
    } else {
        return false;
    }
    return true;
}

bool wj_716N_lidar_protocol::protocl(unsigned char *data, const int len) {

    if ((data[22] == 0x02 && data[23] == 0x02) || (data[22] == 0x02 && data[23] == 0x01)) //command type:0x02 0x01/0X02
    {

        heartstate = true;
        int l_n32TotalPackage = data[80];
        int l_n32PackageNo = data[81];
        unsigned int l_u32FrameNo = (data[75] << 24) + (data[76] << 16) + (data[77] << 8) + data[78];
        int l_n32PointNum = (data[83] << 8) + data[84];
        int l_n32Frequency = data[79];

        if (l_n32Frequency != freq_scan) {
            std::cout << "The scan frequency " << l_n32Frequency << " does not match the one you setted " << freq_scan
                      << " !" << std::endl;
            return false;
        }


        if (m_u32PreFrameNo != l_u32FrameNo) {
            m_u32PreFrameNo = l_u32FrameNo;
            m_u32ExpectedPackageNo = 1;
            m_n32currentDataNo = 0;
        }

        if (l_n32PackageNo == m_u32ExpectedPackageNo && m_u32PreFrameNo == l_u32FrameNo) {
            if (data[82] == 0x00) //Dist
            {
                for (int j = 0; j < l_n32PointNum; j++) {
                    scandata[m_n32currentDataNo] = (((unsigned char) data[85 + j * 2]) << 8) +
                                                   ((unsigned char) data[86 + j * 2]);
                    scandata[m_n32currentDataNo] /= 1000.0;
                    scanintensity[m_n32currentDataNo] = 0;
                    if (scandata[m_n32currentDataNo] > scan.range_max ||
                        scandata[m_n32currentDataNo] < scan.range_min || scandata[m_n32currentDataNo] == 0) {
                        scandata[m_n32currentDataNo] = NAN;
                    }
                    m_n32currentDataNo++;
                }
                m_u32ExpectedPackageNo++;
            } else if (data[82] == 0x01 && m_n32currentDataNo >= total_point) //intensities
            {
                for (int j = 0; j < l_n32PointNum; j++) {
                    scanintensity[m_n32currentDataNo - total_point] = (((unsigned char) data[85 + j * 2]) << 8) +
                                                                      ((unsigned char) data[86 + j * 2]);
                    m_n32currentDataNo++;
                }
                m_u32ExpectedPackageNo++;
            }

            if (m_u32ExpectedPackageNo - 1 == l_n32TotalPackage) {

                m_read_write_mtx.lock();
                mp_scan_points->clear();
                for (int i = index_start; i < index_end; i++) {
                    scan.ranges[i - index_start] = scandata[i];
                    //std::cout<<scan.ranges[i - index_start]<<std::endl;
                    update_data(i - index_start);

                    if (scandata[i - index_start] == NAN) {
                        scan.intensities[i - index_start] = 0;
                        continue;
                    } else {
                        scan.intensities[i - index_start] = scanintensity[i];
                    }

                    pcl::PointXYZ point;
                    //判断平面坐标点是否在限制范围
                    double x = scan.x[i - index_start];
                    double y = scan.y[i - index_start];
                    if (Point2D_tool::limit_with_point2D_box(x, y, &m_point2D_box)) {
                        //平面坐标的转换, 可以进行旋转和平移, 不能缩放
                        point.x = x * m_point2D_transform.m00 + y * m_point2D_transform.m01 + m_point2D_transform.m02;
                        point.y = x * m_point2D_transform.m10 + y * m_point2D_transform.m11 + m_point2D_transform.m12;
                        //添加到最终点云
                        mp_scan_points->push_back(point);
                    }
                }
                scan.header.stamp.msecs = std::chrono::duration_cast<std::chrono::milliseconds>(
                        std::chrono::system_clock::now().time_since_epoch()).count();
                m_read_write_mtx.unlock();

//                    ros::Time scan_time = ros::Time::now();
//                    scan.header.stamp = scan_time;
//                    marker_pub.publish(scan);
            }
        }
        return true;
    } else {
        return false;
    }
}

//index点数组下标【0～l_n32PointCount-1】 l_n32PointCount总点数
void wj_716N_lidar_protocol::update_data(int index) {
    if (index >= total_point) {
        return;
    }

//    每个下标对应的弧度
    scan.angle_to_point[index] =
            scan.angle_min + (total_point - 1 - index) * (scan.angle_max - scan.angle_min) / (total_point - 1);
    scan.y[index] = scan.ranges[index] * cos(scan.angle_min +
                                             (total_point - 1 - index) * (scan.angle_max - scan.angle_min) /
                                             (total_point - 1));
    scan.x[index] = scan.ranges[index] * sin(scan.angle_min +
                                             (total_point - 1 - index) * (scan.angle_max - scan.angle_min) /
                                             (total_point - 1));
}

bool wj_716N_lidar_protocol::checkXor(unsigned char *recvbuf, int recvlen) {
    int i = 0;
    unsigned char check = 0;
    unsigned char *p = recvbuf;
    int len;
    if (*p == 0xFF) {
        p = p + 2;
        len = recvlen - 6;
        for (i = 0; i < len; i++) {
            check ^= *p++;
        }
        p++;
        if (check == *p) {
            return true;
        } else
            return false;
    } else {
        return false;
    }
}

std::chrono::system_clock::time_point wj_716N_lidar_protocol::get_scan_time() const {
    return std::chrono::time_point<std::chrono::system_clock, std::chrono::milliseconds>(
            std::chrono::milliseconds(scan.header.stamp.msecs));
}

int wj_716N_lidar_protocol::get_scan_cycle() const {
    return (config_.frequency_scan == 1 ? 67 : 40);
}