ROS之多个订阅数据同步

做传感器数据融合时,常常会需要用到多个数据,即需要同时订阅多个话题。那么,如何同步这些传感器数据的时间辍,并将它们放入一个回调函数中进行处理呢?

参考文档:

  1. http://wiki.ros.org/message_filters
  2. ROS自带多传感器时间同步机制Time Synchronizer测试
  3. ros消息时间同步与回调

有些消息类型会带有一个头部数据结构,如下所示。信息中带有时间辍数据,可以通过这个数据进行时间同步。

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
  • 以下是一种同步的方式:Time Synchronizer

The TimeSynchronizer filter synchronizes incoming channels by the timestamps contained in their headers, and outputs them in the form of a single callback that takes the same number of channels. The C++ implementation can synchronize up to 9 channels.

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;

  message_filters::Subscriber<Image> image_sub(nh, "image", 1);
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);
  TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}
  • 另外一种是基于策略的同步方式,也是通过消息头部数据的时间辍进行同步。

Policy-Based Synchronizer [ROS 1.1+]

The Synchronizer filter synchronizes incoming channels by the timestamps contained in their headers, and outputs them in the form of a single callback that takes the same number of channels. The C++ implementation can synchronize up to 9 channels.

The Synchronizer filter is templated on a policy that determines how to synchronize the channels. There are currently two policies: ExactTime and ApproximateTime.

  1. 当需要同步的所有消息都带有时间辍的头部数据:ExactTime

    The message_filters::sync_policies::ExactTime policy requires messages to have exactly the same timestamp in order to match. Your callback is only called if a message has been received on all specified channels with the same exact timestamp. The timestamp is read from the header field of all messages (which is required for this policy).

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;
  message_filters::Subscriber<Image> image_sub(nh, "image", 1);
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);

  typedef sync_policies::ExactTime<Image, CameraInfo> MySyncPolicy;
  // ExactTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, info_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

由于该同步策略是当所有需同步的话题的时间辍严格相等时,才会触发回调函数。这就会导致以下一些问题:

  • 回调函数的触发频率必然小于等于这些话题中最小的发布频率;
  • 回调函数的触发并不十分稳定,有时候甚至会出现长时间不被触发的情况。如下图所示,某一次的间隔甚至长达10s左右。

在这里插入图片描述

  1. ROS提供了另外一种方法来实现数据的同步:ApproximateTime。与需要时间辍完全相同的ExactTime不同,该方法允许话题之间的时间辍存在一定的偏差。

    The message_filters::sync_policies::ApproximateTime policy uses an adaptive algorithm to match messages based on their timestamp.

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image1, const ImageConstPtr& image2)
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;
  message_filters::Subscriber<Image> image1_sub(nh, "image1", 1);
  message_filters::Subscriber<Image> image2_sub(nh, "image2", 1);

  typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;
  // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

从下图可以看出,虽然该方法允许时间之间存在偏差,但实际上偏差并不大。而且比起上一种方法,这个方法的回调函数的触发频率快多了。

在这里插入图片描述
关于ApproximateTime,我还有一个不解的地方,这里做一下记录:

If not all messages have a header field from which the timestamp could be determined, see below for a workaround.
If some messages are of a type that doesn’t contain the header field, ApproximateTimeSynchronizer refuses by default adding such messages.

以上这两句话,似乎自相矛盾。不知道是不是我理解的问题。。。从时间同步的角度看,话题消息内容中应该必须要带上时间辍信息才能进行同步,但第一句话却说可以允许一些消息不带时间辍?

[补充于2021.2.11: 今天在使用ApproximateTime时同步了一个自定义的消息类型,发生了如下图所示的错误。后来查阅资料才发现是没有加header的原因,即没有时间辍,程序就无法根据时间进行同步。换句话说,该方法也是必须需要时间辍信息的。加上header后错误就没有了。]

在这里插入图片描述

另外需要注意的是,使用message_filters时,需要在CMakeLists.txtpackage.xml中添加相关依赖:

# CMakeLists.txt
find_package( catkin REQUIRED COMPONENTS
	 ...
	message_filters
)
# package.xml
find_package( catkin REQUIRED COMPONENTS
	<build_depend>message_filters</build_depend>
	<build_export_depend>message_filters</build_export_depend>
	<exec_depend>message_filters</exec_depend>
)
  • 7
    点赞
  • 54
    收藏
    觉得还不错? 一键收藏
  • 8
    评论
当在ROS节点中需要同时订阅多个话题时,可以使用两种方法来保持话题数据同步并一起处理。方法一是利用全局变量TimeSynchronizer,方法二是利用类成员message_filters::Synchronizer。 对于方法一,可以通过使用全局变量TimeSynchronizer来实现话题数据同步。可以按照以下步骤进行操作: 1. 首先,在ROS包的CMakeLists.txt文件和packages.xml文件中添加所需的依赖项。 2. 然后,在代码中创建一个TimeSynchronizer对象,将需要订阅的话题作为参数传入。例如,如果你需要同时订阅名为topic1和topic2的话题,可以创建一个TimeSynchronizer对象,将topic1和topic2作为参数传递给它。 3. 接下来,定义一个回调函数,用于处理接收到的话题数据。在回调函数中,你可以同时处理接收到的topic1和topic2的数据,并将其作为参数传递给另一个函数进行进一步处理。 4. 最后,在主程序中,将回调函数与TimeSynchronizer对象的回调函数绑定,并使用ros::spin()函数来使节点保持运行。 对于方法二,可以使用类成员message_filters::Synchronizer来实现话题数据同步。可以按照以下步骤进行操作: 1. 同样需要在ROS包的CMakeLists.txt文件和packages.xml文件中添加所需的依赖项。 2. 在代码中创建一个message_filters::Synchronizer对象,并使用其模板参数来定义需要同步的消息类型。例如,如果你需要同时订阅类型为Image和CameraInfo的消息,并进行时间同步,可以创建一个message_filters::Synchronizer对象,并将类型为Image和CameraInfo的消息作为模板参数传递给它。 3. 接下来,定义一个回调函数,用于处理接收到的话题数据。在回调函数中,你可以同时处理接收到的Image和CameraInfo的数据,并将其作为参数传递给另一个函数进行进一步处理。 4. 最后,在主程序中,将回调函数与message_filters::Synchronizer对象的回调函数绑定,并使用ros::spin()函数来使节点保持运行。 综上所述,以上是两种常用的方法来同时订阅多个话题并保持话题数据同步。你可以根据具体的需求选择适合你的方法来实现。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 8
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值