转自 https://blog.csdn.net/zyh821351004/article/details/47758433
由于传感器频率的不同,需要将传感器数据进行同步后才能进行融合,下面是ros中同步的方法,
方式一: 全局变量形式 : TimeSynchronizer
步骤:
1. message_filter ::subscriber 分别订阅不同的输入topic
2. TimeSynchronizer 定义时间同步器;
3. sync.registerCallback 同步回调
4. void callback(const ImageConstPtr&image,const CameraInfoConstPtr& cam_info) 带多消息的消息同步自定义回调函数
//wiki参考demo http://wiki.ros.org/message_filters
#include
#include
#include
#include
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_sub(nh, "image", 1); // topic1 输入
message_filters::Subscriber info_sub(nh, "camera_info", 1); // topic2 输入
TimeSynchronizer sync(image_sub, info_sub, 10); // 同步
sync.registerCallback(boost::bind(&callback, _1, _2)); // 回调
ros::spin();
return 0;
}
//
参考链接 http://wiki.ros.org/message_filters
方式二: 类成员的形式 message_filters::Synchronizer
说明: 我用 TimeSynchronizer 改写成类形式中间出现了一点问题.后就改写成message_filters::Synchronizer的形式.
1、头文件
#include
#include
#include
2、定义消息同步机制
typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry,sensor_msgs::Image> slamSyncPolicy;
3、定义类成员变量
message_filters::Subscriber::Odometry>* odom_sub_ ; // topic1 输入
message_filters::Subscriber::Image>* img_sub_; // topic2 输入
message_filters::Synchronizer* sync_;
4、类构造函数中开辟空间new
odom_sub_ = new message_filters::Subscriber::Odometry>(ar_handle, "/odom", 1);
img_sub_ = new message_filters::Subscriber::Image>(ar_handle, "/usb_cam/image_raw", 1);
sync_ = new message_filters::Synchronizer (slamSyncPolicy(10), *odom_sub_, *img_sub_);
sync_->registerCallback(boost::bind(&QrSlam::combineCallback,this, _1, _2));
5、类成员函数回调处理
void QrSlam::combineCallback(const nav_msgs::Odometry::ConstPtr& pOdom, const sensor_msgs::ImageConstPtr& pImg) //回调中包含多个消息
{
//TODO
fStampAll<<pOdom->header.stamp<<" "<<pImg->header.stamp<<endl;
getOdomData(pOdom); //
is_img_update_ = getImgData(pImg); // 像素值
cout << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta
<< " " << robot_odom_.v << " " << robot_odom_.w << std::endl;
fOdom << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta
<< " " << robot_odom_.v << " " << robot_odom_.w << std::endl;
pixDataToMetricData();
static bool FINISH_INIT_ODOM_STATIC = false;
if(FINISH_INIT_ODOM_STATIC)
{
ekfslam(robot_odom_);
}
else if(is_img_update_)
{
if(addInitVectorFull())
{
computerCoordinate();
FINISH_INIT_ODOM_STATIC = true;
}
}
}