一、前言
在进行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
三、创建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;
}
编译之前别忘了添加
add_executable(multi_receive src/multi_receive.cpp)
target_link_libraries(multi_receive ${catkin_LIBRARIES})
四、创建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.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