ROS自学实践(10)：ROS节点同时订阅多个话题并进行消息时间同步_python实现ros消息同步

一、前言

在进行SLAM建图或自动驾驶系统设计的过程中，往往涉及到多种传感器进行环境感知和信息采集，这就不仅需要一个节点接收多个传感器数据，还要求传感器数据的时间戳同步，这样才能实现环境数据的实时感知和处理。本文基于ROS操作系统，从C++和python两个角度进行试验，方便后续的开发工作。

二、创建ROS包

mkdir -p multi_receive/src
cd multi_receive/src
catkin_create_pkg multi_receive roscpp rospy sensor_msgs geometry_msgs
cd ..
catkin_make
1
2
3
4
5

三、创建multi_receive.cpp文件(C++版)

#include <ros/ros.h>
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <boost/thread/thread.hpp>
#include <iostream>

void multi_callback(const sensor_msgs::LaserScanConstPtr& scan, const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose)
{
    std::cout << "同步完成！" << std::endl;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "multi_receiver");
    ros::NodeHandle n;

    message_filters::Subscriber<sensor_msgs::LaserScan> subscriber_laser(n,"scan",1000,ros::TransportHints().tcpNoDelay());
    message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped> subscriber_pose(n,"car_pose",1000,ros::TransportHints().tcpNoDelay());
    
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::LaserScan, geometry_msgs::PoseWithCovarianceStamped> syncPolicy;
    //message_filters::TimeSynchronizer<sensor_msgs::LaserScan,geometry_msgs::PoseWithCovarianceStamped> sync(subscriber_laser, subscriber_pose, 10);
    message_filters::Synchronizer<syncPolicy> sync(syncPolicy(10), subscriber_laser, subscriber_pose);  
    sync.registerCallback(boost::bind(&multi_callback, _1, _2));
    
    std::cout << "hahah" << std::endl;

    ros::spin();
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

编译之前别忘了添加

add_executable(multi_receive src/multi_receive.cpp)
target_link_libraries(multi_receive ${catkin_LIBRARIES})
1
2

四、创建multi_receive.py文件(python版)

在/multi_receive文件夹下，添加scripts文件夹，创建multi_receive.py文件并添加如下内容：

#!/usr/bin/env python
# coding=UTF-8
import rospy,math,random
import numpy as np
import message_filters
from std_msgs.msg import String
from sensor_msgs.msg import LaserScan
from laser_geometry import LaserProjection
from geometry_msgs.msg import PoseWithCovarianceStamped

def multi_callback(Subcriber_laser,Subcriber_pose):
    print "同步完成！"

if __name__ == '__main__':
    rospy.init_node('multi_receive',anonymous=True)

    subcriber_laser = message_filters.Subscriber('scan', LaserScan, queue_size=1)
    subcriber_pose  = message_filters.Subscriber('car_pose', PoseWithCovarianceStamped, queue_size=1)
    
    sync = message_filters.ApproximateTimeSynchronizer([subcriber_laser, subcriber_pose],10,0.1,allow_headerless=True)

    sync.registerCallback(multi_callback)

    rospy.spin()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

参考文献：
1.https://blog.csdn.net/orange_littlegirl/article/details/97425696
2.https://blog.csdn.net/weixin_33895516/article/details/93522384
3.https://www.cnblogs.com/shepherd2015/p/11995769.html