Subscriber topics with differents samplig time

edit

Hi guys,

I am new to ROS Indigo and am trying to work with rosbag file who contains messages from topic with different sampling time (images and IMU data). The rosbag info returns the following information :

path:        data2-03-11-15.bag
version:     2.0
duration:    2:29s (149s)
start:       Nov 03 2015 14:58:15.74 (1446559095.74)
end:         Nov 03 2015 15:00:45.04 (1446559245.04)
size:        4.0 GB
messages:    77540
compression: none [3982/3982 chunks]
types:       geometry_msgs/PoseStamped          [d3812c3cbc69362b77dc0b19b345f8f5]
             sensor_msgs/CameraInfo             [c9a58c1b0b154e0e6da7578cb991d214]
             sensor_msgs/Image                  [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/Imu                    [6a62c6daae103f4ff57a132d6f95cec2]
             visensor_node/visensor_calibration [ed589a2fd70bfbada27d1d4266e0dea2]
             visensor_node/visensor_time_host   [d74fe546f2f8fbb3e0cfaff8b959c212]
topics:      /MT09/pose          17821 msgs    : geometry_msgs/PoseStamped         
             /cam0/calibration    2986 msgs    : visensor_node/visensor_calibration
             /cam0/camera_info    2986 msgs    : sensor_msgs/CameraInfo            
             /cam0/image_raw      2986 msgs    : sensor_msgs/Image                 
             /cam1/calibration    2986 msgs    : visensor_node/visensor_calibration
             /cam1/camera_info    2986 msgs    : sensor_msgs/CameraInfo            
             /cam1/image_raw      2986 msgs    : sensor_msgs/Image                 
             /left/image_rect     2986 msgs    : sensor_msgs/Image                 
             /mpu0               14930 msgs    : sensor_msgs/Imu                   
             /right/image_rect    2986 msgs    : sensor_msgs/Image                 
             /time_host           5972 msgs    : visensor_node/visensor_time_host

The rostopic hz on IMU and image topics are :

rostopic hz /mpu0
subscribed to [/mpu0]
average rate: 105.713
    min: 0.000s max: 0.103s std dev: 0.02714s window: 100

rostopic hz /right/image_rect
subscribed to [/right/image_rect]
average rate: 20.053
    min: 0.041s max: 0.058s std dev: 0.00322s window: 20

rostopic hz /left/image_rect
subscribed to [/left/image_rect]
average rate: 20.114
    min: 0.041s max: 0.054s std dev: 0.00269s window: 19

From my understanding, the IMU message are sent at 100Hz and images (left and right) are sent at 20Hz. I would like to deal with stereo images left and right and in the same time work with the IMU data. I did a program with subscribers, based on ApproximateTime for images (in order to have images at the “same time”) and classical subscriber for the IMU. My program is the following :

void callback_image(const ImageConstPtr& image1, const ImageConstPtr& image2)
{
  ROS_INFO("Image 1 at [%d]", image1->header.seq);
  ROS_INFO("Image 2 at [%d]", image2->header.seq);
}


void callback_gyro(const sensor_msgs::Imu::ConstPtr& imu)
{
  ROS_INFO("Imu gyro Seq: [%d]", imu->header.seq);
}


int main(int argc, char** argv)
{
  ROS_INFO("Start !")
 ;
  ros::init(argc, argv, "vision_node");
  ros::NodeHandle nh;

  //Suscriber for images
  message_filters::Subscriber<Image> image1_sub(nh, "/left/image_rect", 1);
  message_filters::Subscriber<Image> image2_sub(nh, "/right/image_rect", 1);
  typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);
  sync.registerCallback(boost::bind(&callback_image, _1, _2 ));

  //Suscriber for IMU
  ros::Subscriber sub3 = nh.subscribe("/mpu0", 1000, callback_gyro);

  ros::spin();

  return 0;
}

When I start my program, I obtain the following result on my console :

[ INFO] [1452419480.048670407]: Start !
[ INFO] [1452419480.323157479]: Image 1 at [113965]
[ INFO] [1452419480.323209657]: Image 2 at [113710]
[ INFO] [1452419480.328795802]: Imu gyro Seq: [566209]
[ INFO] [1452419480.328853778]: Imu gyro Seq: [566210]
[ INFO] [1452419480.328887739]: Imu gyro Seq: [566211]
[ INFO] [1452419480.328918985]: Imu gyro Seq: [566212]
[ INFO] [1452419480.328954773]: Imu gyro Seq: [566213]
[ INFO] [1452419480.328989697]: Imu gyro Seq: [566214]
[ INFO] [1452419480.329022374]: Imu gyro Seq: [566215 ...