多激光时间同步

最新推荐文章于 2024-10-18 15:19:28 发布

浦东新村轱天乐

最新推荐文章于 2024-10-18 15:19:28 发布

阅读量1.2k

点赞数 1

分类专栏：深度学习文章标签： c++ 自动驾驶 Lidar 时间同步

本文链接：https://blog.csdn.net/u012686154/article/details/123495094

版权

深度学习专栏收录该内容

9 篇文章 1 订阅

订阅专栏

最近在倒腾多激光时间同步的问题，场景如下：

一辆车上装有3个激光，需要开发一个把3个激光msg都收齐后合并为一帧发送出去的节点。

正常情况下，3个激光均以10HZ频率发送，每一帧3个激光的时间戳几乎相同（称为一组message），而同一个激光两帧之间差100ms。

msg header时间：该激光曝光时的时间（假定曝光是一瞬间完成的），另外多个激光设置成几乎是同时触发曝光，这样他们所携带的时间戳几乎是相同的。

msg 接收时间： callback实际接收到激光的时间（一般会有延时，如激光曝光时间戳是第10000ms，但由于激光内部点云处理也会耗时，所以下游实际接收时间会晚个几毫秒）

示意图如下：

在这里插入图片描述

我使用的是主激光方案，整体思想如下

选定一个激光作为主激光，当收到主激光msg时，判断一组message是否已经收齐，收齐了则pub。

如果没有收齐，则等待50ms；在等待过程中，如果一组message收齐了，则publish并退出等待，如果等待50ms后仍然没有收齐，则只发送已收到的message。

尝试了如下3种实现方案，前2种都有不可解释的问题，放弃了，最后采用了第3种方法：

sleep查询法

主lidar线程每隔5ms查询一次所有msg是否到齐，共循环10次。如果到齐则publish并退出循环。

void lidar_i_callback(const string& topic)
{
    receive_msg(); //处理接收到的msg
    
    if (topic != main_lidar) { // 只有main_lidar才pub
        return;
    }
    
	for (int i = 0; i < 10; i++) {
		receive_one_group_msg = judge_received_one_group_msg();
        if (receive_one_group_msg) {
        	pub(); // pub拼接后的点云
        	return;
        } else {
            usleep(5000); //sleep 50 ms
        }
    }
}

问题：在usleep过程中，所有激光的接受线程仿佛都被sleep住了。

现象是随着时间，非主激光接收得越来越晚。换言之，sleep并没有释放main_lidar线程，main_lidar_callback在sleep期间，其他lidar_callback也没有接收。

**以前听别人说过，有可能是所有callback实际只有一个CPU核，所以一个callback sleep了，所有的callback都会延时，但是我还查了下代码，代码中有明确指定CPU核数量就是Lidar个数。**存疑。

(不过查询法就算sleep没问题，也显得有点low)

条件变量法

主lidar线程使用条件变量wait_for(lock, time, pred)等待，其他激光到齐后，notify主激光线程publish。如果50ms内仍然没有到齐，则主激光强制发送已收到的msg，没收到的则舍弃。

std::mutex _lidar_topics_timestamp_mtx;
std::condition_variable _lidar_topics_timestamp_cv; // 条件变量

void lidar_i_callback(const string& topic)
{
    receive_msg(); //处理接收到的msg
    
    
    if (topic != main_lidar) { // 只有main_lidar才pub
    	receive_one_group_msg = judge_received_one_group_msg();
        if (receive_one_group_msg) { // 一旦接收完全就notify发布，最小延时
        	_lidar_topics_timestamp_cv.notify_all(); 
        }
    } else {
		std::unique_lock<std::mutex> lck(_lidar_topics_timestamp_mtx);
        _lidar_topics_timestamp_cv.wait_for(lck, std::chrono::milliseconds(50), judge_received_one_group_msg);
        pub(); // pub拼接后的点云
    }
}

问题：

wait_for中的judge_received_one_group_msg()判断一直为true，根本没有等待，直接发送了。
如果让judge_received_one_group_msg()直接返回false，则wait_for()会wait，但无法收到notify信息，每次都是等待50ms后发出，相当于做了50ms的延时。

std::this_thread::sleep_for()法

不知为何stl的stleep_for就可以正常sleep。

设置_main_lidar_last_pub_time一个保存每次pub时的main lidar时间戳。
设置publish()函数动作。publish()默认已经接收到一组msg。首先判断当前组msg的main lidar时间戳是否等于_main_lidar_last_pub_time_，如果相等说明已经发过了，直接return。如果不相等，则证明没有发过，则先更新last_pub_main_timestamp_，再pub。
不论接收到哪个lidar，均查询是否一组msg已经到来，如果到来则直接调用publish()。如果没有到来，如果是主线程，就sleep_for(50)，再调用publish()。

这样如果主激光到来的时候，msg已经收齐了就直接发，没有收齐就sleep_for，其他msg在收齐后会立即pub，不会有延时。

std::atomic_uint64_t _main_lidar_last_pub_time{0}; // 保存每次pub时的main lidar时间戳。

void lidar_i_callback(const string& topic)
{
    receive_msg();
    
	receive_one_group_msg = judge_received_one_group_msg();
    if (receive_one_group_msg) { //无论哪个lidar_callback，只要收齐了就pub
        publish();
    } else {
        // 如果没收齐，且是主lidar线程，则等待50ms发送。
        //等待时间到后，publish()会判断这一组msg是否已经被其他lidar_callback发送过了，如果发送过了会直接return，这样不会重复发送。
        //如果这一组msg没有发送，则只发送已收到的msg。
        if (topic == main_lidar) { 
        	std::this_thread::sleep_for(std::chrono::milliseconds(50));
        	publish();
        }
    }
}

void publish()
{
  if (cur_group_msg_main_lidar_timestamp == last_pub_main_timestamp_) { // 判断是否已经pub过当前组消息
  	return;
  }
  last_pub_main_timestamp_ = cur_group_msg_main_lidar_timestamp; // 更新已经发布的时间戳，  必须放在pub()之前！
  pub(); // pub拼接后的点云
}

注意，publish()中，这两个的先后顺序不能颠倒，不然会出现重复发送的情况，因为pub()也需要时间。

假设main_lidar等待50ms，而pub()耗时60ms。则其他lidar在pub的过程中，main_lidar等待时间也到了，main_lidar也会调用publish()，而此时last_pub_main_timestamp_还没被更新，所以main_lidar认为此组消息还没被发过，就会重复发一次。