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 packetpkt->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 houruint32_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 operationif (!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; }

總結

以上是生活随笔為你收集整理的Velodyne 32E pcap包GPS时间戳解析的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。