随手记录：Livox 时间戳修改为ROS时间戳

news2024/9/21 12:21:43

参考与前言

传感器类型：Livox-Mid70

参考链接：Ubuntu20.04系统安装Livox ROS Driver

官方驱动：https://github.com/Livox-SDK/livox_ros_driver

碎碎念：之所以要改成rostime主要是提取pcd的时候发现这个timestamp 300.xxx 打头没法和其他VLP对上，所以想着直接改成rostime 也方便后续bag tf的邻近时间提取

修改过程

在livox_ros_driver下打开Vscode，

CTRL+P搜索文件Iddc.h 在105行下面的加入一个now_time 如下：

  ros::NodeHandle *cur_node_;
  rosbag::Bag *bag_;
  ros::Time ros_time_now=ros::Time::now(); // new add

CTRL+P搜索文件Iddc.cpp 全局搜索将：

ros::Time(timestamp / 1000000000.0)

替换为

ros_time_now

替换完成后，举个例子是这样的：

	// replace
    if (!published_packet) {
      cloud.header.stamp = ros::Time(timestamp / 1000000000.0);
    }
    // after replace
    if (!published_packet) {
      cloud.header.stamp = ros_time_now;
    }

在最后的DistributeLiDARData函数添加一行时间更新即可：

	void Lddc::DistributeLidarData(void) {
  if (lds_ == nullptr) {
    return;
  }
  lds_->semaphore_.Wait();
  for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
    uint32_t lidar_id = i;
    LidarDevice *lidar = &lds_->lidars_[lidar_id];
    LidarDataQueue *p_queue = &lidar->data;
    if ((kConnectStateSampling != lidar->connect_state) ||
        (p_queue == nullptr)) {
      continue;
    }
    ros_time_now = ros::Time::now(); // new add!
    PollingLidarPointCloudData(lidar_id, lidar);
    PollingLidarImuData(lidar_id, lidar);
  }

  if (lds_->IsRequestExit()) {
    PrepareExit();
  }
}