这是速腾最早的产品RS-16，目前的主流产品应该是M平台的固态激光雷达以及R平台的多线机械雷达，不过点云的解析的基本原理都是类似的。
雷达点云的时间戳对多传感器同步、畸变矫正等有很大的帮助。下文以速腾RS-16为例，分析其雷达驱动(
https://github.com/RoboSense-LiDAR/rslidar_sdk)中关于点云时间戳的细节。
1. pps时间同步
RS-16支持PPS+GPRMC的时间同步方式，
当利用PPS信号同步时间后，会在MSOP Package中的Header中将时间戳同步，
帧头 Header 共 42byte，21~30byte存储时间戳，分辨率为1us。
2. 雷达驱动中的时间戳
如图2所示，点云是原始数据以MSOP协议的格式发送，一个Package包含了一个Header，其中有一个时间戳，还有12个数据Block(BLOCKS_PER_PKT)，每个Block中有个水平旋转角度和32组channel的数据(CHANNELS_PER_BLOCK)，对于RS-16来说，刚好是两组激光束(16x2)的数据(LASER_NUM)。
RS-16的一些参数，
template
inline const LidarConstantParameter DecoderFactory::getRS16ConstantParam()
{
LidarConstantParameter ret_param;
ret_param.MSOP_ID = 0xA050A55A0A05AA55;
ret_param.DIFOP_ID = 0x555511115A00FFA5;
ret_param.BLOCK_ID = 0xEEFF;
ret_param.PKT_RATE = 750;
ret_param.BLOCKS_PER_PKT = 12;
ret_param.CHANNELS_PER_BLOCK = 32;
ret_param.LASER_NUM = 16;
ret_param.DSR_TOFFSET = 2.8;
ret_param.FIRING_FREQUENCY = 0.009;
ret_param.RX = 0.03825;
ret_param.RY = -0.01088;
ret_param.RZ = 0;
return ret_param;
} 在驱动中支持两种点的类型，XYZI和XYZIRT，除了反射率还有RING和时间戳的信息，本文默认使用XYZIRT格式。
#=======================================
# Custom Point Type (XYZI, XYZIRT)
#=======================================
set(POINT_TYPE XYZIRT)PKT_RATE=750表示每秒发送750个Package，对于10HZ的输出，一帧完整点云所需要的时间为0.1s，在0.1秒内发出了0.1*750=7575个Package，同时每个Package中包含12个Block，则一帧完整点云共75*12=900个Block。
2.1 get_point_time_func_函数
decoder_base.hpp----->
std::function get_point_time_func_;
......
/* Point time function*/
if (RS_HAS_MEMBER(T_Point, timestamp)) ///< return the timestamp of the first block in one packet
{
if (this->param_.use_lidar_clock)
{
get_point_time_func_ = [this](const uint8_t* pkt) { return getLidarTime(pkt); };
}
else
{
get_point_time_func_ = [this](const uint8_t* pkt) {
double ret_time =
getTime() - (this->lidar_const_param_.BLOCKS_PER_PKT - 1) * this->time_duration_between_blocks_;
return ret_time;
};
}
}
else
{
get_point_time_func_ = [this](const uint8_t* pkt) { return 0; };
} 在驱动的配置文件中，每个雷达的driver都需要配置use_lidar_clock: false，如果设置为true，则表示使用lidar的时钟，经过PPS+GPRMC同步后的信号该选项要设置为true，否则会使用驱动运行所在的控制器中的系统时间。
假设使用PPS同步后的时间，
decoder_RS16.hpp----->
template
inline double DecoderRS16::getLidarTime(const uint8_t* pkt)
{
return this->template calculateTimeYMD(pkt);
}
decoder_base.hpp----->
template
template
inline double DecoderBase::calculateTimeYMD(const uint8_t* pkt)
{
#ifdef _MSC_VER
long timezone = 0;
_get_timezone(&timezone);
#endif
const T_Msop* mpkt_ptr = reinterpret_cast(pkt);
std::tm stm;
memset(&stm, 0, sizeof(stm));
stm.tm_year = mpkt_ptr->header.timestamp.year + 100;
stm.tm_mon = mpkt_ptr->header.timestamp.month - 1;
stm.tm_mday = mpkt_ptr->header.timestamp.day;
stm.tm_hour = mpkt_ptr->header.timestamp.hour;
stm.tm_min = mpkt_ptr->header.timestamp.minute;
stm.tm_sec = mpkt_ptr->header.timestamp.second;
return std::mktime(&stm) + static_cast(RS_SWAP_SHORT(mpkt_ptr->header.timestamp.ms)) / 1000.0 +
static_cast(RS_SWAP_SHORT(mpkt_ptr->header.timestamp.us)) / 1000000.0 - static_cast(timezone);
}
上述代码直接取MSOP中的header中的时间戳，安装数据结构填充，并转换。
假设使用控制器系统时间，
获取系统时间(system_clock)，这里使用的是system_clock感觉有些问题，控制器联网时间同步后时间会跳变吧？
utility/time.h----->
inline double getTime(void)
{
const auto t = std::chrono::system_clock::now();
const auto t_sec = std::chrono::duration_cast>(t.time_since_epoch());
return (double)t_sec.count();
}
获取系统当前的时间，同时减去当前Package中Block所用的时间，相当于估计一个该Package初始的发射时间。
get_point_time_func_ = [this](const uint8_t* pkt) {
double ret_time =
getTime() - (this->lidar_const_param_.BLOCKS_PER_PKT - 1) * this->time_duration_between_blocks_;
return ret_time;
2.2 点的时间戳赋值
time_duration_between_blocks_表示两个block之间的时间，
this->time_duration_between_blocks_ =
(60 / static_cast(this->rpm_)) /
((this->lidar_const_param_.PKT_RATE * 60 / this->rpm_) * this->lidar_const_param_.BLOCKS_PER_PKT);
int fov_start_angle = RS_SWAP_SHORT(dpkt_ptr->fov.start_angle);
int fov_end_angle = RS_SWAP_SHORT(dpkt_ptr->fov.end_angle);
int fov_range = (fov_start_angle < fov_end_angle) ? (fov_end_angle - fov_start_angle) :
(RS_ONE_ROUND - fov_start_angle + fov_end_angle);
int blocks_per_round =
(this->lidar_const_param_.PKT_RATE / (this->rpm_ / 60)) * this->lidar_const_param_.BLOCKS_PER_PKT;
this->fov_time_jump_diff_ =
this->time_duration_between_blocks_ * (fov_range / (RS_ONE_ROUND / static_cast(blocks_per_round)));
decoder_RS16.hpp----->
template
inline RSDecoderResult DecoderRS16::decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height,
int& azimuth)
{
//1.获取当前Package的时间戳
double block_timestamp = this->get_point_time_func_(pkt);
......
for (size_t blk_idx = 0; blk_idx < this->lidar_const_param_.BLOCKS_PER_PKT; blk_idx++)
{
if (blk_idx == 0)
{
azi_diff = static_cast((RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 1].azimuth) - cur_azi) %
RS_ONE_ROUND);
}
else
{
azi_diff = static_cast((RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 1].azimuth)) %
RS_ONE_ROUND);
// 2.遍历该Package中的所有block,每个block中的时间都会根据基准时间戳上累加time_duration_between_blocks_
block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) :
(block_timestamp + this->time_duration_between_blocks_);
}
......
for (size_t channel_idx = 0; channel_idx < this->lidar_const_param_.CHANNELS_PER_BLOCK; channel_idx++)
{
......
// 3.遍历每个block中的channel，即32个channel的数据，对应RS-16两组数据，设置反射率，RING，点的时间戳
T_Point point;
setX(point, x);
setY(point, y);
setZ(point, z);
setIntensity(point, intensity);
setRing(point, this->beam_ring_table_[channel_idx % 16]);
if (this->echo_mode_ != ECHO_DUAL && channel_idx > 15)
{
setTimestamp(point, block_timestamp + this->time_duration_between_blocks_ / 2);
}
else
{
setTimestamp(point, block_timestamp);
}
vec.emplace_back(std::move(point));
}
}
}
上述解析MSOP报文的主要流程：
- 获取当前Package的时间戳；
- 遍历该Package中的所有block,每个block中的时间都会根据基准时间戳上累加time_duration_between_blocks_；
- 遍历每个block中的channel，即32个channel的数据，对应RS-16两组数据，设置反射率，RING，点的时间戳。
从目前的驱动代码中可以看到，该Block中所有点的时间戳是相同的，还没有细化到每个点的时间戳。因为雷达转一圈，获取一帧的数据的时间是0.1秒(10HZ，600RPM)，共有900个Block，同一Block中的点的时间戳是相同的，时间的分辨率大概为0.1/900=0.111e-3,比ms级别精度略高一些。
2.3 ROS中Header时间的赋值
下面的代码处理完一帧点云所有的MSOP数据后，将一帧点云的时间戳设置为scan_msg.timestamp，而scan_msg.timestamp在驱动处理时，会被设置为系统时间(system_clock)或者是Scan中的最后一个Package中的时间戳(pps同步)。
lidar_driver_impl.hpp----->
template
inline bool LidarDriverImpl::decodeMsopScan(const ScanMsg& scan_msg, PointCloudMsg& point_cloud_msg)
{
......
std::vector> pointcloud_one_frame;
int height = 1;
pointcloud_one_frame.resize(scan_msg.packets.size());
for (int i = 0; i < static_cast(scan_msg.packets.size()); i++)
{
std::vector pointcloud_one_packet;
RSDecoderResult ret =
lidar_decoder_ptr_->processMsopPkt(scan_msg.packets[i].packet.data(), pointcloud_one_packet, height);
switch (ret)
{
case RSDecoderResult::DECODE_OK:
case RSDecoderResult::FRAME_SPLIT:
pointcloud_one_frame[i] = std::move(pointcloud_one_packet);
break;
case RSDecoderResult::WRONG_PKT_HEADER:
reportError(Error(ERRCODE_WRONGPKTHEADER));
break;
case RSDecoderResult::PKT_NULL:
reportError(Error(ERRCODE_PKTNULL));
break;
default:
break;
}
}
for (auto iter : pointcloud_one_frame)
{
output_point_cloud_ptr->insert(output_point_cloud_ptr->end(), iter.begin(), iter.end());
}
point_cloud_msg.point_cloud_ptr = point_cloud_transform_func_(output_point_cloud_ptr, height);
point_cloud_msg.height = height;
point_cloud_msg.width = point_cloud_msg.point_cloud_ptr->size() / point_cloud_msg.height;
setPointCloudMsgHeader(point_cloud_msg);
//将一帧点云的时间戳设置为scan_msg.timestamp
point_cloud_msg.timestamp = scan_msg.timestamp;
if (point_cloud_msg.point_cloud_ptr->size() == 0)
{
reportError(Error(ERRCODE_ZEROPOINTS));
return false;
}
return true;
}
template
inline void LidarDriverImpl::processMsop()
{
while (msop_pkt_queue_.size() > 0)
{
PacketMsg pkt = msop_pkt_queue_.popFront();
int height = 1;
int ret = lidar_decoder_ptr_->processMsopPkt(pkt.packet.data(), *point_cloud_ptr_, height);
scan_ptr_->packets.emplace_back(std::move(pkt));
if ((ret == DECODE_OK || ret == FRAME_SPLIT))
{
if (ret == FRAME_SPLIT)
{
PointCloudMsg msg(point_cloud_transform_func_(point_cloud_ptr_, height));
msg.height = height;
msg.width = point_cloud_ptr_->size() / msg.height;
setPointCloudMsgHeader(msg);
if (driver_param_.decoder_param.use_lidar_clock == true)
{
msg.timestamp = lidar_decoder_ptr_->getLidarTime(pkt.packet.data());
}
else
{
msg.timestamp = getTime();
}
if (msg.point_cloud_ptr->size() == 0)
{
reportError(Error(ERRCODE_ZEROPOINTS));
}
else
{
runCallBack(msg);
}
//设置ScanMsg的时间戳
setScanMsgHeader(*scan_ptr_);
runCallBack(*scan_ptr_);
point_cloud_ptr_.reset(new typename PointCloudMsg::PointCloud);
scan_ptr_.reset(new ScanMsg);
}
}
else
{
reportError(Error(ERRCODE_WRONGPKTHEADER));
msop_pkt_queue_.clear();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}
msop_pkt_queue_.is_task_finished_.store(true);
}
lidar_driver_impl.hpp----->
template
inline void LidarDriverImpl::setScanMsgHeader(ScanMsg& msg)
{
//设置为系统时间，system_clock或者是Scan中的最后一个Package中的时间戳(pps同步)
msg.timestamp = getTime();
if (driver_param_.decoder_param.use_lidar_clock == true)
{
msg.timestamp = lidar_decoder_ptr_->getLidarTime(msg.packets.back().packet.data());
}
msg.seq = scan_seq_++;
msg.frame_id = driver_param_.frame_id;
}
ROS发送点云，
inline void PointCloudRosAdapter::sendPointCloud(const LidarPointCloudMsg& msg)
{
point_cloud_pub_.publish(toRosMsg(msg));
}
将rs_msg.timestamp的时间戳赋值给sensor_msgs::PointCloud2数据的header。
inline sensor_msgs::PointCloud2 toRosMsg(const LidarPointCloudMsg& rs_msg)
{
sensor_msgs::PointCloud2 ros_msg;
pcl::toROSMsg(*rs_msg.point_cloud_ptr, ros_msg);
ros_msg.header.stamp = ros_msg.header.stamp.fromSec(rs_msg.timestamp);
ros_msg.header.frame_id = rs_msg.frame_id;
ros_msg.header.seq = rs_msg.seq;
return std::move(ros_msg);
}
3.更精确的点的时间的计算
目前速腾驱动中的点的时间精细化到每个Block的时间，接下来会升级为细化到每个点的时间，这样后续做点云的畸变矫正就会非常方便。
在每个 MSOP Packet 中，有 12 个 Block，每个 Block 有 2 组完整的 16 线激光数据，因此一个 Packet 中有 24 组完整的激光数据。16 通道激光完成一轮发射和充能需要 55.5us。两线激光之间发射的时间间隔是 2.8us，16 线激光完成一轮发射时间为 2.816=44.8us，并有 10.7us 的额外间隔时间，所以一轮发射和充电的时间周期是 2.816+10.7=55.5us。
假设激光的序号 data_index 是 1-16，激光发射轮数序列为 sequence_index 是 1-24。每 个 MSOP Packet 的时间戳是第 1 个激光点的时间，为了计算每个激光点的时间，需要将每个激光点的时间偏移量加到时间戳上。
时间偏移量 Time_offset:
_=55.5?(_?1)+2.8?(_?1)
精确的每个激光点的时间 Exact_point_time：
__=+_
下图为RS-16MSOP中每个激光点的时间偏移量，单位为us。