Velodyne 32E pcap包GPS时间戳解析

最新推荐文章于 2024-04-19 16:30:00 发布

charyman

最新推荐文章于 2024-04-19 16:30:00 发布

阅读量1.1k

点赞数 1

分类专栏： Lidar 文章标签： linux c++

本文链接：https://blog.csdn.net/charyman/article/details/119820579

版权

Lidar 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

1. pcap解析工具

Libpcap是Packet Capture Libray的英文缩写，即数据包捕获函数库。提供的接口函数实现和封装了与数据包截获有关的过程。

安装方法：sudo apt install libpcap-dev

2. pcap header中的时间戳解析

Libpcap中的packet的header结构如下：

struct timeval
{
  __time_t tv_sec;		/* Seconds.  */
  __suseconds_t tv_usec;	/* Microseconds.  */
};

struct pcap_pkthdr {
	struct timeval ts;	/* time stamp */
	bpf_u_int32 caplen;	/* length of portion present */
	bpf_u_int32 len;	/* length this packet (off wire) */
};

其中，ts即为接收时间戳。利用Libpcap库中的pcap_next_ex()函数可从packet中解析其header数据

int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
  {
    struct pcap_pkthdr *header;
    const u_char *pkt_data;

    while (true)
      {
        int res;
        if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
          {
            // Skip packets not for the correct port and from the
            // selected IP address.
            if (0 == pcap_offline_filter(&pcap_packet_filter_,
                                          header, pkt_data))
              continue;

            // Keep the reader from blowing through the file.
            if (read_fast_ == false)
              packet_rate_.sleep();
            
            memcpy(&pkt->data[0], pkt_data+42, packet_size);
            if (!gps_time_) {
              if (!pcap_time_) {
                pkt->stamp = ros::Time::now(); // time_offset not considered here, as no synchronization required
              } else {
                pkt->stamp = ros::Time(header->ts.tv_sec, header->ts.tv_usec * 1000); // 
              }
            } else {
              // time for each packet is a 4 byte uint located starting at offset 1200 in
              // the data packet
              pkt->stamp = rosTimeFromGpsTimestamp(&(pkt->data[1200]), header);
            }
            empty_ = false;
            return 0;                   // success
          }

        if (empty_)                 // no data in file?
          {
            ROS_WARN("Error %d reading Velodyne packet: %s", 
                     res, pcap_geterr(pcap_));
            return -1;
          }

        if (read_once_)
          {
            ROS_INFO("end of file reached -- done reading.");
            return -1;
          }
        
        if (repeat_delay_ > 0.0)
          {
            ROS_INFO("end of file reached -- delaying %.3f seconds.",
                     repeat_delay_);
            usleep(rint(repeat_delay_ * 1000000.0));
          }

        ROS_DEBUG("replaying Velodyne dump file");

        // I can't figure out how to rewind the file, because it
        // starts with some kind of header.  So, close the file
        // and reopen it with pcap.
        pcap_close(pcap_);
        pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
        empty_ = true;              // maybe the file disappeared?
      } // loop back and try again
  }

3. pcap data中gps时间格式

velodyne的数据包格式中gps timestamp数据存放于[1200～1203]，共4个字节。表示一小时内的毫秒数。

4. gps时间戳合成

第一步从packet的status data中获取gps timestamp；

第二步提取header中的时间戳，并将精度保留到小时；

第三步将第二步结果与gps timestamp结合，得到时间戳；

第四步对齐时间戳。

ros::Time resolveHourAmbiguity(const ros::Time &stamp, const ros::Time &nominal_stamp) {
    const int HALFHOUR_TO_SEC = 1800;
    ros::Time retval = stamp;
    if (nominal_stamp.sec > stamp.sec) {
        if (nominal_stamp.sec - stamp.sec > HALFHOUR_TO_SEC) {
            retval.sec = retval.sec + 2*HALFHOUR_TO_SEC;
        }
    } else if (stamp.sec - nominal_stamp.sec > HALFHOUR_TO_SEC) {
        retval.sec = retval.sec - 2*HALFHOUR_TO_SEC;
    }
    return retval;
}

ros::Time rosTimeFromGpsTimestamp(const uint8_t * const data, const struct pcap_pkthdr *header = NULL) {
    const int HOUR_TO_SEC = 3600;
    // time for each packet is a 4 byte uint
    // It is the number of microseconds from the top of the hour
    uint32_t usecs = (uint32_t) ( ((uint32_t) data[3]) << 24 |
                                  ((uint32_t) data[2] ) << 16 |
                                  ((uint32_t) data[1] ) << 8 |
                                  ((uint32_t) data[0] ));
    ROS_INFO("gpstime: %d", usecs);
    ros::Time time_nom = ros::Time();
    // if header is NULL, assume real time operation
    if (!header) {
        time_nom = ros::Time::now(); // use this to recover the hour
    } else {
        ROS_INFO("header sec: %ld, usec: %ld", header->ts.tv_sec, header->ts.tv_usec);
        time_nom = ros::Time(header->ts.tv_sec, header->ts.tv_usec * 1000);
    }
    uint32_t cur_hour = time_nom.sec / HOUR_TO_SEC;
    ros::Time stamp = ros::Time((cur_hour * HOUR_TO_SEC) + (usecs / 1000000),
                                (usecs % 1000000) * 1000);
    ROS_INFO("before stamp sec: %d, nsec: %d", stamp.sec, stamp.nsec);
    stamp = resolveHourAmbiguity(stamp, time_nom);
    ROS_INFO("after stamp sec: %d, nsec: %d", stamp.sec, stamp.nsec);
    return stamp;
}