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

目录

 

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

2、测试目的：

3、测试结果：

4、编写的测试代码

---

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("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 data
    velodyne_rawdata::VPointCloud::Ptr
      outMsg(new velodyne_rawdata::VPointCloud());
    // outMsg's header is a pcl::PCLHeader, convert it before stamp assignment
    outMsg->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 driver
    for (size_t i = 0; i < scanMsg->packets.size(); ++i)
      {
        data_->unpack(scanMsg->packets[i], *outMsg);
      }

    // publish the accumulated cloud message
    ROS_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<<"-"<tm_mday<<" "<< stm_->tm_hour<<":"<tm_min<<":"<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 packet
      std::size_t azimuth_data_pos = 100*0+2;
      int azimuth = *( (u_int16_t*) (&tmp_packet.data[azimuth_data_pos]));

      // Handle overflow 35999->0
      if(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 received
            int 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" <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 status
  diag_topic_->tick(scan->header.stamp);
  diagnostics_.update();

  return true;
}