这是速腾最早的产品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 <typename T_Point>
inline const LidarConstantParameter DecoderFactory<T_Point>::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<double(const uint8_t*)> 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 <typename T_Point>
inline double DecoderRS16<T_Point>::getLidarTime(const uint8_t* pkt)
{
  return this->template calculateTimeYMD<RS16MsopPkt>(pkt);
}

decoder_base.hpp----->

template <typename T_Point>
template <typename T_Msop>
inline double DecoderBase<T_Point>::calculateTimeYMD(const uint8_t* pkt)
{
#ifdef _MSC_VER
  long timezone = 0;
  _get_timezone(&timezone);
#endif
  const T_Msop* mpkt_ptr = reinterpret_cast<const T_Msop*>(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<double>(RS_SWAP_SHORT(mpkt_ptr->header.timestamp.ms)) / 1000.0 +
         static_cast<double>(RS_SWAP_SHORT(mpkt_ptr->header.timestamp.us)) / 1000000.0 - static_cast<double>(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<std::chrono::duration<double>>(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<float>(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<float>(blocks_per_round)));

decoder_RS16.hpp----->

template <typename T_Point>
inline RSDecoderResult DecoderRS16<T_Point>::decodeMsopPkt(const uint8_t* pkt, std::vector<T_Point>& 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<float>((RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 1].azimuth) - cur_azi) %
                                    RS_ONE_ROUND);
    }
    else
    {
      azi_diff = static_cast<float>((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报文的主要流程：

1. 获取当前Package的时间戳；
2. 遍历该Package中的所有block,每个block中的时间都会根据基准时间戳上累加time_duration_between_blocks_；
3. 遍历每个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 <typename T_Point>
inline bool LidarDriverImpl<T_Point>::decodeMsopScan(const ScanMsg& scan_msg, PointCloudMsg<T_Point>& point_cloud_msg)
{
 ......
  std::vector<std::vector<T_Point>> pointcloud_one_frame;
  int height = 1;
  pointcloud_one_frame.resize(scan_msg.packets.size());
  for (int i = 0; i < static_cast<int>(scan_msg.packets.size()); i++)
  {
    std::vector<T_Point> 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 <typename T_Point>
inline void LidarDriverImpl<T_Point>::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<T_Point> 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<T_Point>::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 <typename T_Point>
inline void LidarDriverImpl<T_Point>::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。