【激光雷达】速腾聚创16线激光雷达驱动程序测试总结

目录

 

1、速腾16线激光测试参考:

2、测试目的:

3、测试结果:

4、测试代码:


1、速腾16线激光测试参考:

源码地址:

https://github.com/RoboSense-LiDAR/ros_rslidar

多线激光使用、建图总结见:

https://blog.csdn.net/xingdou520/article/details/85098314

 

2、测试目的:

1、测试速腾聚创ROS驱动程序的执行过程,数据流向;

2、确认pub的数据格式,时间戳问题;

3、每一场数据包含的激光测量的具体角度问题确认。

4、点云格式确认,是否可以自定义点云格式,从而减小数据拷贝造成的时间、资源浪费。

3、测试结果:

1、rslidar_driver每次pub一场数据的时候,每场数据的起始角度都不一样。

2、每一场数据超过360度,在361-364度之间。

3、rslidar_driver驱动程序,每一场数据包含84个UDP数据包(rslidarPacket.msg每个数据包1248字节数据),每个包间隔1.1-1.4ms,合起来刚好100ms一场数据。每次读取84个完整数据包后pub一次数据(rslidarSacn.msg),

4、驱动程序发布数据的时间戳是激光传感器传上来的最后一包的数据的时候,ROS给数据添加的时间戳。

double time1 = ros::Time::now().toSec();

UDP方式读取一包数据

// Average the times at which we begin and end reading.  Use that to
// estimate when the scan occurred. Add the time offset.
double time2 = ros::Time::now().toSec();
pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset);  //时间戳

5、rslidar_pointcloud包作用,订阅rslidarSacn.msg,并根据标定文件将原始数据转为sensor_msgs::PointCloud2数据发布出去。


  /** @brief Callback for raw scan messages. 
      原始激光数据的回调函数
  */
  void Convert::processScan( const rslidar_msgs::rslidarScan::ConstPtr &scanMsg)
  {
  	pcl::PointCloud<pcl::PointXYZI>::Ptr outPoints(new pcl::PointCloud<pcl::PointXYZI>);  //定义要发布的点云数据格式
  	outPoints->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp;              //使用原始数据的时间戳
  	outPoints->header.frame_id = scanMsg->header.frame_id;                                //使用源数据的frame_id
    // process each packet provided by the driver
    bool finish_packets_parse = false;
    for (size_t i = 0; i < scanMsg->packets.size(); ++i)                        //连续解析所有的数据包,速腾16线激光 每场数据84个数据包
    {
        if(i == (scanMsg->packets.size()-1))
       {
            //ROS_INFO_STREAM("Packets per scan: "<< scanMsg->packets.size());
            finish_packets_parse = true;
       }
        data_->unpack(scanMsg->packets[i], outPoints,finish_packets_parse);     //解析数据包,将原始数据转换为点云格式数据
    }

    sensor_msgs::PointCloud2 outMsg;      //定义ROS标准的点云消息
    pcl::toROSMsg(*outPoints, outMsg);    //将自定义的点云数据转换为ROS点云消息格式

    //if(outPoints->size()==0){
    //    ROS_INFO_STREAM("Height1: "<<outPoints->height<<" Width1: "<<outPoints->width);
    //}
    output_.publish(outMsg);              //发布点云消息
  }
}

6、可以用protobuf的方式代替sensor_msgs::PointCloud2,通过内存共享的方式实现多节点之间数据共享。

4、测试代码:



void print_time(struct tm *stm_)
{
    printf("T:%d-%02d-%02d %02d:%02d:%02d\n",stm_->tm_year ,stm_->tm_mon,stm_->tm_mday,stm_->tm_hour,stm_->tm_min,stm_->tm_sec);
}

int stamp_test(uint8_t *data)
{
        int flag = 0;
        struct tm stm_;
        if ((data[0] == 0x55) && (data[1] == 0xaa) && (data[2] == 0x05) && (data[3] == 0x0a))
        {
            memset(&stm_, 0, sizeof(stm_));
            stm_.tm_year = (int)data[20] + 100;
            stm_.tm_mon  = (int)data[21] - 1;
            stm_.tm_mday = (int)data[22];
            stm_.tm_hour = (int)data[23];
            stm_.tm_min  = (int)data[24];
            stm_.tm_sec  = (int)data[25];

            double time_stamp = 0;
            time_stamp = 0.001 * (256 * data[26] + data[27]) 
                    + 0.000001 * (256 * data[28] + data[29]);

            static double time_stamp_lost = time_stamp;


            double stamp_double = mktime(&stm_) + time_stamp;

            static uint32_t seconds = ros::Time(stamp_double).sec;  //上一次 收到的时间 
            
            //if (ros::Time(stamp_double).sec - seconds > 0) //秒 发生变化 
            if(time_stamp_lost > time_stamp)
            {                
                seconds = ros::Time(stamp_double).sec;

                std::cout << std::endl;
                print_time(&stm_);

                std::cout << std::setprecision(20) << "stamp_double:" << stamp_double << std::endl;    
                
                std::cout << std::setprecision(6);
                std::cout <<"time_stamp:" << time_stamp << "\tseconds: " << seconds ;

                int azimuth = 256 * data[44] + data[45];
                static int angle_lost = azimuth;


                // std::cout << " angle:\t" << azimuth /100.0 << "\tdiff:"<< (azimuth - angle_lost)/100.0 << std::endl;
                std::cout << " angle:\t" << azimuth /100.0 << "\terror:"<< (azimuth - 9000)/100.0 << std::endl;

                angle_lost = azimuth;
                flag= 1;
            }
            time_stamp_lost = time_stamp;
        }
        return flag;
}




/** poll the device
 *
 *  @returns true unless end of file reached
 */
    bool rslidarDriver::poll(void) {
        // Allocate a new shared pointer for zero-copy sharing with other nodelets.
        rslidar_msgs::rslidarScanPtr scan(new rslidar_msgs::rslidarScan);
        scan->packets.resize(config_.npackets);
        // Since the rslidar delivers data at a very high rate, keep
        // reading and publishing scans as fast as possible.

        std::cout << " poll: " << std::endl;
        for (int i = 0; i < config_.npackets; i++) {  //84packets
            while (true) {
                // keep reading until full packet received
                int rc = input_->getPacket(&scan->packets[i], config_.time_offset);

                if (rc == 0) 
                {
                    uint8_t buf[50];
                    for (size_t ii = 0; ii < 50; ii++)
                    {
                        buf[ii] = scan->packets[i].data[ii];
                    }

                    // std::cout << " test "<< std::endl;
                   if(stamp_test(buf)==1)
                   {
                        std::cout << " packet: " << i << std::endl;
                   }

                    if(i == 0)
                    {
                        uint8_t data[50];
                        data[44] = scan->packets[i].data[44];
                        data[45] = scan->packets[i].data[45];

                        int azimuth = 256 * data[44] + data[45];
                   
                        // std::cout << " angle:\t" << azimuth /100.0 << "\tdiff:"<< (azimuth - angle_lost)/100.0 << std::endl;
                        //std::cout << "packet num:" << i << " angle:\t" << azimuth /100.0 << std::endl;

                        static int _angle_lost = azimuth;

                        // std::cout << " angle:\t" << azimuth /100.0 << "\tdiff:"<< (azimuth - angle_lost)/100.0 << std::endl;
                        std::cout << "packet num:" << i << " First Packet angle:\t" << azimuth /100.0 << "\tpub data anlge:"<< (azimuth+36000-_angle_lost)/100.0 << std::endl;
                        _angle_lost = azimuth;
                    }

                    if(i == 83)
                    {
                        uint8_t data[50];
                        data[44] = scan->packets[i].data[44];
                        data[45] = scan->packets[i].data[45];

                        int azimuth = 256 * data[44] + data[45];
                   
                        // std::cout << " angle:\t" << azimuth /100.0 << "\tdiff:"<< (azimuth - angle_lost)/100.0 << std::endl;
                        std::cout << "packet num:" << i << " Last Packet angle:\t" << azimuth /100.0 << std::endl;
                    }


                    break;       // got a full packet?
                }
                if (rc < 0) return false; // end of file reached?
            }
        }

        // publish message using time of last packet read
        ROS_DEBUG("Publishing a full rslidar scan.");
        scan->header.stamp = scan->packets[config_.npackets - 1].stamp;
        scan->header.frame_id = config_.frame_id;
        output_.publish(scan);

        // notify diagnostics that a message has been published, updating its status
        diag_topic_->tick(scan->header.stamp);
        diagnostics_.update();


        // printf("npackets:%d\n",config_.npackets);

        return true;
    }