详解 ROS 近似时间戳同步 ApproximateTime

2024-04-29 02:36

本文主要是介绍详解 ROS 近似时间戳同步 ApproximateTime,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

ApproximateTime

功能介绍

message_filters::sync_policies::ApproximateTime 策略使用自适应算法根据时间戳来匹配多个Topic消息。

ApproximateTime根据时间戳来进行匹配,因此Topic的消息类型必须包含Header字段【C++】。

以下补充属性

  1. 以同步两个Topic为例,ApproximateTime至少会保证其中一个Topic中的消息不会被丢失。另一个Topic的消息可能会被丢失。
  2. 匹配完全按照Header字段中的stamp时间戳进行同步,和消息到来的时间顺序无关。
  3. 同步的一组Topic不能有重复

使用案例

示例代码

GitHup : https://github.com/SeekyChan/ApproximateTime_demo.git

创建两个ROS Node(time_01_node time_02_node),发布两个Topic分别为 : "timestamp_02_topic" 和 "timestamp_02_topic",两个Topic间隔 1 s 发送一次 sensor_msgs::PointCloud2 消息,时间戳为当前系统的时间戳。

创建ROS Node(sync_node) 订阅并同步两个topic的消息,同时打印信息。

三个节点的代码如下:

time_01_node

#include "ros/ros.h"
#include "std_msgs/Header.h"
#include <sensor_msgs/PointCloud2.h>
#include <ctime>int main(int argc, char **argv)
{// 初始化ROS节点ros::init(argc, argv, "timestamp_publisher");// 创建节点句柄ros::NodeHandle n;ros::Publisher pub = n.advertise<sensor_msgs::PointCloud2>("timestamp_01_topic", 10);// 设置发布频率,这里设定为每秒1次ros::Rate loop_rate(1);int count = 0;while (ros::ok()){// 获取当前系统时间戳sensor_msgs::PointCloud2 cloud_msg;// 设置消息的元数据cloud_msg.header.frame_id = "time_01_node"; cloud_msg.header.stamp = ros::Time::now(); // 设置时间戳cloud_msg.header.seq = count;// 发布消息pub.publish(cloud_msg);// 输出当前时间戳//ROS_INFO("time_01_node publishing timestamp: %f", cloud_msg.header.stamp.toSec());// 递增count值count++;// 延时以达到指定的发布频率loop_rate.sleep();}return 0;
}

time_02_node

#include "ros/ros.h"
#include "std_msgs/Header.h"
#include <sensor_msgs/PointCloud2.h>
#include <ctime>int main(int argc, char **argv)
{// 初始化ROS节点ros::init(argc, argv, "timestamp_publisher");// 创建节点句柄ros::NodeHandle n;ros::Publisher pub = n.advertise<sensor_msgs::PointCloud2>("timestamp_02_topic", 10);// 设置发布频率,这里设定为每秒1次ros::Rate loop_rate(1);int count = 0;while (ros::ok()){// 获取当前系统时间戳sensor_msgs::PointCloud2 cloud_msg;// 设置消息的元数据cloud_msg.header.frame_id = "time_02_node"; cloud_msg.header.stamp = ros::Time::now(); // 设置时间戳cloud_msg.header.seq = count;// 发布消息pub.publish(cloud_msg);// 输出当前时间戳//ROS_INFO("time_02_node publishing timestamp: %f", cloud_msg.header.stamp.toSec());// 递增count值count++;// 延时以达到指定的发布频率loop_rate.sleep();}return 0;
}

sync_node

#include "ros/ros.h"
#include "std_msgs/Header.h"
#include <sensor_msgs/PointCloud2.h>#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>void callback(const sensor_msgs::PointCloud2::ConstPtr& msg1, const sensor_msgs::PointCloud2::ConstPtr& msg2) {// 打印消息的时间戳ROS_INFO("Received timestamps: topic1: %f, topic2: %f", msg1->header.stamp.toSec(), msg2->header.stamp.toSec());// 计算时间戳的差值(绝对值)double timestamp_diff = fabs(msg1->header.stamp.toSec() - msg2->header.stamp.toSec());ROS_INFO("Timestamp difference: %f seconds ,node_1 seq : %d , node_1 seq : %d", timestamp_diff,msg1->header.seq,msg2->header.seq);
}int main(int argc, char **argv) {// 初始化 ROS 节点ros::init(argc, argv, "timestamp_sync_node");// 创建节点句柄ros::NodeHandle nh;//订阅两个话题message_filters::Subscriber<sensor_msgs::PointCloud2> sub1(nh, "timestamp_01_topic", 5);message_filters::Subscriber<sensor_msgs::PointCloud2> sub2(nh, "timestamp_02_topic", 5);typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,sensor_msgs::PointCloud2> syncPolicy; //近似同步策略message_filters::Synchronizer<syncPolicy> sync(syncPolicy(10), sub1, sub2);// 同步sync.registerCallback(boost::bind(&callback, _1, _2));// 循环等待回调ros::spin();return 0;
}

运行结果

从结果可以看到,两个Topic的消息都一一同步并都打印出来了。因为我们两个节点发布的消息都是每隔一秒发送一个当前时间戳的消息,因此两个Topic的消息的时间戳几乎是完全一致的,因此全部一一匹配也在意料之中。

设置匹配下限

假设我们此时更改 time_02_node 节点,将每隔 1 s 发送 的时间戳改为 当前时间戳 + 5s,其他节点代码保持不变。

cloud_msg.header.frame_id = "time_02_node"; 
cloud_msg.header.stamp = ros::Time::now() + ros::Duration(5); // 设置时间戳

运行结果

此时我们看到,time_01_node 的前5s的消息被丢弃了,从time_01_node的第 6 个消息开始与 time_02_node 的第一个消息进行匹配。

如果我们更改需求,设time_01_node 和 time_02_node 两个节点的消息时间相差时间不超过5s就算作匹配,那么time_01_node的第一个消息与time_02_node的第一个消息时间戳上就是匹配的,不应该被丢弃。此时我们可以使用 setInterMessageLowerBound 来设置匹配允许的误差。

更改sync_node的代码如下:

    sync.setInterMessageLowerBound(ros::Duration(10,0));sync.registerCallback(boost::bind(&callback, _1, _2));

此时观察运行结果

消息再次一一对应了。

消息同步的队列大小

例程中Synchronizer的queue_size 设为 10

message_filters::Synchronizer<syncPolicy> sync(syncPolicy(10), sub1, sub2);// 同步

具体的含义是什么呢?

上图中两条线分别代表两个Topic , queue_size 指的是 每一个Topic最多进来多少个点作为匹配的集合。如果在消息进入的点的数量达到设置定的队列大小数量还没有相应时间戳匹配成功,那么则丢弃当前的点进行下一轮匹配。以上文示例代码的程序为例,如果设为syncPolicy(5),而 time_02_node 节点每隔 1 s 发送 的时间戳为 当前时间戳 + 5s,在不设置匹配下限的情况下,意味着在Topic的消息队列的集合为 5 的范围内则时间戳永不匹配。

这篇关于详解 ROS 近似时间戳同步 ApproximateTime的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/944904

相关文章

Python 字典 (Dictionary)使用详解

《Python字典(Dictionary)使用详解》字典是python中最重要,最常用的数据结构之一,它提供了高效的键值对存储和查找能力,:本文主要介绍Python字典(Dictionary)... 目录字典1.基本特性2.创建字典3.访问元素4.修改字典5.删除元素6.字典遍历7.字典的高级特性默认字典

MySQL 主从复制部署及验证(示例详解)

《MySQL主从复制部署及验证(示例详解)》本文介绍MySQL主从复制部署步骤及学校管理数据库创建脚本,包含表结构设计、示例数据插入和查询语句,用于验证主从同步功能,感兴趣的朋友一起看看吧... 目录mysql 主从复制部署指南部署步骤1.环境准备2. 主服务器配置3. 创建复制用户4. 获取主服务器状态5

一文详解如何使用Java获取PDF页面信息

《一文详解如何使用Java获取PDF页面信息》了解PDF页面属性是我们在处理文档、内容提取、打印设置或页面重组等任务时不可或缺的一环,下面我们就来看看如何使用Java语言获取这些信息吧... 目录引言一、安装和引入PDF处理库引入依赖二、获取 PDF 页数三、获取页面尺寸(宽高)四、获取页面旋转角度五、判断

Spring Boot中的路径变量示例详解

《SpringBoot中的路径变量示例详解》SpringBoot中PathVariable通过@PathVariable注解实现URL参数与方法参数绑定,支持多参数接收、类型转换、可选参数、默认值及... 目录一. 基本用法与参数映射1.路径定义2.参数绑定&nhttp://www.chinasem.cnbs

MySql基本查询之表的增删查改+聚合函数案例详解

《MySql基本查询之表的增删查改+聚合函数案例详解》本文详解SQL的CURD操作INSERT用于数据插入(单行/多行及冲突处理),SELECT实现数据检索(列选择、条件过滤、排序分页),UPDATE... 目录一、Create1.1 单行数据 + 全列插入1.2 多行数据 + 指定列插入1.3 插入否则更

Redis中Stream详解及应用小结

《Redis中Stream详解及应用小结》RedisStreams是Redis5.0引入的新功能,提供了一种类似于传统消息队列的机制,但具有更高的灵活性和可扩展性,本文给大家介绍Redis中Strea... 目录1. Redis Stream 概述2. Redis Stream 的基本操作2.1. XADD

Spring StateMachine实现状态机使用示例详解

《SpringStateMachine实现状态机使用示例详解》本文介绍SpringStateMachine实现状态机的步骤,包括依赖导入、枚举定义、状态转移规则配置、上下文管理及服务调用示例,重点解... 目录什么是状态机使用示例什么是状态机状态机是计算机科学中的​​核心建模工具​​,用于描述对象在其生命

Java JDK1.8 安装和环境配置教程详解

《JavaJDK1.8安装和环境配置教程详解》文章简要介绍了JDK1.8的安装流程,包括官网下载对应系统版本、安装时选择非系统盘路径、配置JAVA_HOME、CLASSPATH和Path环境变量,... 目录1.下载JDK2.安装JDK3.配置环境变量4.检验JDK官网下载地址:Java Downloads

使用Python删除Excel中的行列和单元格示例详解

《使用Python删除Excel中的行列和单元格示例详解》在处理Excel数据时,删除不需要的行、列或单元格是一项常见且必要的操作,本文将使用Python脚本实现对Excel表格的高效自动化处理,感兴... 目录开发环境准备使用 python 删除 Excphpel 表格中的行删除特定行删除空白行删除含指定

MySQL中的LENGTH()函数用法详解与实例分析

《MySQL中的LENGTH()函数用法详解与实例分析》MySQLLENGTH()函数用于计算字符串的字节长度,区别于CHAR_LENGTH()的字符长度,适用于多字节字符集(如UTF-8)的数据验证... 目录1. LENGTH()函数的基本语法2. LENGTH()函数的返回值2.1 示例1:计算字符串