// This file is used for collecting data fromm imu and camera at the same time.
// One camera and one imu
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sensor_msgs/Imu.h>
#include <rosbag/bag.h>
using namespace std;
using namespace sensor_msgs;
using namespace message_filters;
using namespace geometry_msgs;
// global variable
char command;
rosbag::Bag bag_record;
int img_num=0;
char szName1[200] = {'\0'};
char szName2[200] = {'\0'};
double pose_array[7]={10,11,12,13,14,15,16};
typedef message_filters::sync_policies::ApproximateTime<Image,Imu> sync_policy_classification;
string img_topic = "/camera/image_mono";
string imu_topic = "/mu_data";
void callback(const ImageConstPtr& image, const sensor_msgs::ImuConstPtr& imu_msg)
{
std::cout<<"time gap is : "<<image->header.stamp.now() - imu_msg->header.stamp.now() <<endl;
bag_record.write(img_topic,image->header.stamp.now(), image);
bag_record.write(imu_topic, imu_msg->header.stamp.now(), imu_msg);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "msg_filter_xuqw");
ros::NodeHandle nh;
bag_record.open("test.bag", rosbag::bagmode::Write);
message_filters::Subscriber<Image> info_sub(nh, img_topic, 1);
message_filters::Subscriber<Imu> imu_sub(nh, imu_topic, 1);
message_filters::Synchronizer<sync_policy_classification> sync(sync_policy_classification(10), info_sub, imu_sub);
sync.registerCallback(boost::bind(&callback, _1, _2));
ros::spin();
ros::shutdown();
return 0;
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)