1、velodyne VLP-16线激光测试参考：

源码地址：

https://github.com/ros-drivers/velodyne

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

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

2、测试目的：

1、测试velodyneROS驱动程序的执行过程，数据流向；

2、确认pub的数据格式，时间戳问题；

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

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

5、测试GPS和激光时间同步方式。

6、测试激光传感器相位锁功能：GPS-PPS信号控制激光内部电机相位；

3、测试结果：

1、velodyne的驱动程序使用ROS的nodelet方式实现，可以实现多节点之间数据pub的时候零拷贝，减少数据拷贝造成的时间浪费。

2、velodyne_driver每次pub一场数据的时候，每场数据的起始角度都不一样（见图中start angle:）。

3、每一场数据超过360度，在363度左右(见图中spin angle:)。

4、velodyne_driver驱动程序，每一场数据包含76个UDP数据包（udp数据包1248字节，刨除42字节，剩余velodynePacket.msg每个数据包1206字节数据），相邻数据包之间激光的角度为：4.75-4.81度。每个包间隔1.327ms左右,合起来刚好100ms一场数据。每次读取76个完整数据包后pub一次数据（velodyneSacn.msg），

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

/** @brief Get one velodyne packet. */
int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
{double time1 = ros::Time::now().toSec();UDP方式读取一包数据double time2 = ros::Time::now().toSec();pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset);  //时间戳
}

6、velodyne_pointcloud包作用，订阅velodyneSacn.msg，并根据标定文件将原始数据转为sensor_msgs::PointCloud2数据发布出去。

1、定义点云输出格式：
output_ =node.advertise<sensor_msgs::PointCloud2>("velodyne_points", 10);
2、转换点云消息为sensor_msgs::PointCloud2，并发布点云消息：/** @brief Callback for raw scan messages. */void Convert::processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg){if (output_.getNumSubscribers() == 0)         // no one listening?return;                                     // avoid much work// allocate a point cloud with same time and frame ID as raw datavelodyne_rawdata::VPointCloud::PtroutMsg(new velodyne_rawdata::VPointCloud());// outMsg's header is a pcl::PCLHeader, convert it before stamp assignmentoutMsg->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp;outMsg->header.frame_id = scanMsg->header.frame_id;outMsg->height = 1;// process each packet provided by the driverfor (size_t i = 0; i < scanMsg->packets.size(); ++i){data_->unpack(scanMsg->packets[i], *outMsg);}// publish the accumulated cloud messageROS_DEBUG_STREAM("Publishing " << outMsg->height * outMsg->width<< " Velodyne points, time: " << outMsg->header.stamp);output_.publish(outMsg);}} // namespace velodyne_pointcloud

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

7、相位锁功能测试：打开浏览器输入激光IP地址：192.168.1.201,通过web界面配置、查看激光参数。

配置使能相位锁：

底部状态栏观看同步结果：如果此时有有效的PPS信号：同步正常

去掉PPS信号：PPS：Absent 相位不再受控

8、时间同步功能：

默认发送更改的时钟后，11秒之后velodyne才会使用新的同步时种作为自己数据的时钟。

默认时间可以配置Delay:(最终时间是Delay+6秒左右)

测试数据见：

时钟同步结论：

不是每次收到有效的UTC时钟都会发生时钟同步。
收到有效的同步时钟后，会等待一定时间后才发生同步，同步当时的角度和相位控制的角度一致。
PPS可以控制内部相位，但内部时钟、时间戳并不和相位控制一起起作用。相位控制优先级更高。时钟同步有延时。

4、编写的测试代码

velodyne的默认包，不知道如何配置的无法使用cout、printf等标准输出函数打印调试信息，测试发现只能使用ROS_ERROR打印。

增加包数据的解析函数：

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);ROS_ERROR_STREAM("T:" << stm_->tm_year <<"-"<< stm_->tm_mon<<"-"<<stm_->tm_mday<<" "<< stm_->tm_hour<<":"<<stm_->tm_min<<":"<<stm_->tm_sec);}void print_hex(uint8_t *buf,int len)
{uint8_t bufs[300] = {0};for(size_t i = 0; i < len; i++){sprintf((char *)bufs+i*3,"%02X ",buf[i]); }// 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);ROS_ERROR_STREAM("hex:" << bufs);
}/*
data 包的前50字节
data2包的倒数6个字节  4字节时间戳、2字节Factory Byte
*/
int stamp_test(uint8_t *data,uint8_t *data2)
{int flag = 0;struct tm stm_;if ((data[0] == 0xFF) && (data[1] == 0xEE) ) //&& (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];uint32_t time_stamp = 0;time_stamp = 255 * 255 * (256 * data2[3] + data2[2]) + (256 * data2[1] + data2[0]);static uint32_t time_stamp_lost = time_stamp;//ros::Time::now().toSec();uint32_t stamp_ms = (time_stamp % 1000000) / 1000; // mktime(&stm_) + uint32_t stamp_us = time_stamp % 1000;  uint32_t stamp_s = ((time_stamp/1000) / 1000)%60; uint32_t stamp_m = (((time_stamp/1000) / 1000) / 60)%60; uint32_t stamp_h = ((( ((time_stamp/1000) / 1000) / 60)%60) / 60); static uint32_t stamp_ms_lost = stamp_ms;//static uint32_t seconds = ros::Time(stamp_double).sec;  //上一次 收到的时间 int azimuth = 256 * data[3] + data[2];//if (ros::Time(stamp_double).sec - seconds > 0) //秒 发生变化 if(stamp_ms_lost > stamp_ms)//if((azimuth>8900)&&(azimuth<9100)){                //seconds = ros::Time(stamp_double).sec;//std::cout << std::endl;//print_time(&stm_);ROS_ERROR_STREAM("time_stamp:"<< time_stamp << "\tMS:"<< stamp_ms << "\tus:" << stamp_us << "\tdiff:"<< (time_stamp-time_stamp_lost)/1000.0 <<"ms" );ROS_ERROR_STREAM("T:"<< stamp_h << ":"<< stamp_m << ":" << stamp_s << " " << stamp_ms%1000);//std::cout << std::setprecision(20) << "stamp_double:" << stamp_double << std::endl;    //std::cout << std::setprecision(6);//std::cout <<"time_stamp:" << time_stamp << "\tseconds: " << seconds ;static int angle_lost = azimuth;ROS_ERROR_STREAM("angle:" << azimuth /100.0 << "\tdiff:"<< (azimuth - angle_lost)/100.0);// 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;stamp_ms_lost = stamp_ms ;}return flag;
}

测试代码位置：


/** poll the device**  @returns true unless end of file reached*/
bool VelodyneDriver::poll(void)
{// Allocate a new shared pointer for zero-copy sharing with other nodelets.velodyne_msgs::VelodyneScanPtr scan(new velodyne_msgs::VelodyneScan);//OS_ERROR_STREAM("cut_angle " << config_.cut_angle);if( config_.cut_angle >= 0) //Cut at specific angle feature enabled{scan->packets.reserve(config_.npackets);velodyne_msgs::VelodynePacket tmp_packet;while(true){while(true){int rc = input_->getPacket(&tmp_packet, config_.time_offset);if (rc == 0) break;       // got a full packet?if (rc < 0) return false; // end of file reached?}scan->packets.push_back(tmp_packet);static int last_azimuth = -1;// Extract base rotation of first block in packetstd::size_t azimuth_data_pos = 100*0+2;int azimuth = *( (u_int16_t*) (&tmp_packet.data[azimuth_data_pos]));// Handle overflow 35999->0if(azimuth<last_azimuth)last_azimuth-=36000;// Check if currently passing cut angleif(   last_azimuth != -1&& last_azimuth < config_.cut_angle&& azimuth >= config_.cut_angle ){last_azimuth = azimuth;break; // Cut angle passed, one full revolution collected}last_azimuth = azimuth;}}else // standard behaviour{// Since the velodyne delivers data at a very high rate, keep// reading and publishing scans as fast as possible.scan->packets.resize(config_.npackets);for (int i = 0; i < config_.npackets; ++i){while (true){          // keep reading until full packet receivedint rc = input_->getPacket(&scan->packets[i], config_.time_offset);uint8_t buf[50];for (size_t ii = 0; ii < 10; ii++){buf[ii] = scan->packets[i].data[ii];}//print_hex(buf,30);uint8_t buf2[50];for (size_t ii = 1200; ii < 1206; ii++){buf2[ii-1200] = scan->packets[i].data[ii];}// std::cout << " test "<< std::endl;if(stamp_test(buf,buf2)==1){ROS_ERROR_STREAM(" packet: " << i << "\n");}// if(i == 0)// {//     int azimuth = 256 * buf[3] + buf[2];//     static int angle_lost = azimuth;//     ROS_ERROR_STREAM("start angle:" << azimuth /100.0 << "\t spin angle:"<< (azimuth+36000 - angle_lost)/100.0 );//     // 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;// }//uint8_t buf[50];// memset(buf,0,50);// for (int ii = 1190; ii < 1206; ii++)// {//     buf[ii-1190] = scan->packets[i].data[ii];// }// print_hex(buf,16);// ROS_ERROR_STREAM(" data : "<< std::hex << scan->packets[i].data[1200] << " "//                             << scan->packets[i].data[1201] << " "//                             << scan->packets[i].data[1202] << " "//                             << scan->packets[i].data[1203] << " "//                             << scan->packets[i].data[1204] << " "//                             << scan->packets[i].data[1205]);// ROS_ERROR_STREAM(" data size: " << scan->packets[i].data.size() );if (rc == 0) break;       // got a full packet?if (rc < 0) return false; // end of file reached?}}}//  std::cout << "test" <<std::endl;// ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,ros::console::levels::Debug);// ROS_ERROR_STREAM("time_offset " << config_.time_offset);// ROS_ERROR_STREAM("test " << config_.frame_id);//ROS_ERROR_STREAM("npackets " << config_.npackets);// publish message using time of last packet readROS_DEBUG("Publishing a full Velodyne scan.");scan->header.stamp = scan->packets.back().stamp;scan->header.frame_id = config_.frame_id;output_.publish(scan);// notify diagnostics that a message has been published, updating// its statusdiag_topic_->tick(scan->header.stamp);diagnostics_.update();return true;
}

【激光雷达】velodyne VLP-16线激光雷达驱动程序、相位锁、时钟同步测试总结相关推荐

3999美元掀价格战，Velodyne宣布16线激光雷达降价50%
3D视觉系统和先进的自动驾驶安全解决方案的全球领导者Velodyne LiDAR今天宣布,其无人驾驶用16线激光雷达现在可以面向全球客户降价50%. 新智元从Velodyne亚洲区总监翁炜处获悉,现在 ...
SLAM实操入门（六）：连接Velodyne的16线激光雷达并可视化
文章目录前言 1 Velodyne激光雷达 2 配置网络 3 创建ROS工程 4 启动并可视化 5 录包与播放总结前言好久没更新这部分了,最近在搞中期答辩的东西,简单补充一部分多线激光雷达建图 ...
gazebo中机器人搭载16线激光雷达建图导航
gazebo中机器人搭载16线激光雷达建图导航如果用过古月的教程大家就会知道,古月教程中的机器人采用的是单线激光雷达,但有的时候仿真需要多线激光雷达,这里就教一下怎么搭载16线激光雷达建图导航. 首 ...
gazebo中给机器人添加16线激光雷达跑LIO-SAM
目录: 前言 1.下载雷达仿真包 2.添加雷达支架描述文件 3.添加雷达描述文件 4.启动仿真 5.添加IMU模块 6.添加RGB-D相机 7.LIO-SAM仿真安装依赖安装GTSAM 编译LIO ...
3、速腾16线激光雷达RS-16 ----- 3D建图算法LeGO-LOAM的使用（Ubuntu18.04 + ROS Melodic）
Ubuntu18.04使用速腾16线激光雷达RS-16测试3D建图算法 LeGO-LOAM 的使用 1. 安装gtsam因子图库 2. 新建ROS工程目录文件夹,下载源码进行编译 3. 下载相关数据集 ...
速腾聚创16线激光雷达rslidar-16的ros驱动安装与rviz点云显示
原链接: https://community.bwbot.org/topic/520 运行测试平台:小强ROS机器人速腾聚创16线激光雷达rslidar-16的供电是直流12v 3A,通信接口是rs ...
robosense速腾16线激光雷达配置过程
环境: 1.速腾16线激光雷达rslidar: 2.工控机: 3.Ubuntu 18.04 melocic版本; 1.1 安装驱动源码 $ mkdir –p ~/catkin_rslid ...
采用16线激光雷达调用cartographer室内定位
采用16线激光雷达调用cartographer室内定位之前进行了16线激光雷达调用cartographer包进行室内建图,这方面很多朋友都知道,cartographer是google的一个开源SLA ...
robosense 16线激光雷达标定
一.准备步骤: 1.安装 RSView (1)网址:http://www.robosense.ai/web/resource/cn,我这边是16线激光雷达,按照自己的版本下载就行了,如下图所示: wi ...
采用16线激光雷达调用cartographer室内建图
采用16线激光雷达调用cartographer室内建图要用cartographer建图,我们需要的前期准备工作有: 1 安装cartographer功能包这是一个谷歌编写的开源功能包,还是非常好用 ...

【激光雷达】velodyne VLP-16线激光雷达驱动程序、相位锁、时钟同步测试总结

1、velodyne VLP-16线激光测试参考：

2、测试目的：

3、测试结果：

4、编写的测试代码

【激光雷达】velodyne VLP-16线激光雷达驱动程序、相位锁、时钟同步测试总结相关推荐

最新文章

热门文章