码迷,mamicode.com
首页 > 其他好文 > 详细

<ROS> message_filters 对齐多种传感器数据的时间戳

时间:2019-01-19 22:57:37      阅读:1897      评论:0      收藏:0      [点我收藏+]

标签:stream   cal   方式   sync   代码   ack   std   http   数据   

联合标定三维雷达和IMU,第一步要先对齐两种传感信息的时间戳。

ros官网提供了message_filters用于对齐多种传感信息的时间戳。

http://wiki.ros.org/message_filters#Time_Synchronizer

需要提示一点,对齐时间戳有两种方式,一种是时间戳完全对齐 ExactTime Policy ,另一种是时间戳相近 

ApproximateTime Policy ,前者更为严格。

本人选用时间戳相近的对齐方法,代码如下:

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <ros/ros.h>
#include <iostream>

using namespace std;
using namespace sensor_msgs;
using namespace message_filters;
  PointCloud2 syn_pointcloud;
  Imu syn_imu;
void callback(const PointCloud2ConstPtr& ori_pointcloud, const ImuConstPtr& ori_imu)
{
  // Solve all of perception here...
  syn_pointcloud = *ori_pointcloud;
  syn_imu = *ori_imu;
  cout << "syn velodyne points‘ timestamp : " << syn_pointcloud.header.stamp << endl;
  cout << "syn Imu‘s timestamp : " << syn_imu.header.stamp << endl;
  ROS_INFO("pointcloud stamp value is: %d", syn_pointcloud.header.stamp);
  ROS_INFO("imu stamp value is: %d", syn_imu.header.stamp);
}


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

  message_filters::Subscriber<Imu> imu_sub(nh, "/imu/data", 1);
  message_filters::Subscriber<PointCloud2> velodyne_sub(nh, "/velodyne_points", 1);
  typedef sync_policies::ApproximateTime<PointCloud2, Imu> MySyncPolicy;    
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), velodyne_sub, imu_sub); //queue size=10
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();
  return 0;
}

callback函数订阅的所有/topic必须有数据存在,否则调用失败。

 

<ROS> message_filters 对齐多种传感器数据的时间戳

标签:stream   cal   方式   sync   代码   ack   std   http   数据   

原文地址:https://www.cnblogs.com/gdut-gordon/p/10293446.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!