CT_project/测量节点/wanji_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));
//        marker_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 50);
//        ros::Time scan_time = ros::Time::now();
//        scan.header.stamp = scan_time;
    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 = NULL;

    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;
//        scan.ranges.resize(samples);
//        scan.intensities.resize(samples);

    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 = NULL;
	}

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

	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() == NULL )
	{
		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 == NULL)
			{
				// m_wanji_lidar_protocol_status = E_FAULT;
			}
			else
			{
				Binary_buf *tp_binary_buf = NULL;
				bool is_pop = mp_communication_data_queue->wait_and_pop(tp_binary_buf);
				if (is_pop == true && tp_binary_buf != NULL)
				{
                    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;
	return;
}


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
                    {
                        std::cout << "continuous search frame header" << std::endl;
                        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;
        }

//    std::cout<<"该圈的数据总条数："<<total_point_number_per_circle<<std::endl;
//    std::cout<<"数据点对应的数组下标："<<index<<std::endl;
//    每个下标对应的弧度
        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));

//        scan.angle_to_point[total_point-1-index] = scan.angle_min+(total_point-1-index)*(scan.angle_max-scan.angle_min)/(total_point-1);
//        scan.y[total_point-1-index]=scan.ranges[index]*cos(scan.angle_min+(total_point-1-index)*(scan.angle_max-scan.angle_min)/(total_point-1));
//        scan.x[total_point-1-index]=scan.ranges[index]*sin(scan.angle_min+(total_point-1-index)*(scan.angle_max-scan.angle_min)/(total_point-1));

//    if(index!=540)
//       std::cout<<"每个点对应的弧度: "<<scan.angle_to_point[total_point-1-index]<<", 每个点对应的距离： "<<scan.ranges[total_point-1-index]<<", (x,y): ("<<scan.x[total_point-1-index]<<","<<scan.y[total_point-1-index]<<")"<<std::endl;
    }

    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;
        }
    }