Subscriber topics with differents samplig time

asked 2016-01-10 04:04:24 -0500

Agachon gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Are you sure you are not confusing header.seq with header.stamp? The former is just a sequence number, the latter is the actual time stamp of the message.

Note also that both are / have to be set by the originating node, they are not handled by the middleware itself.

gvdhoorn gravatar imagegvdhoorn ( 2016-01-10 07:10:25 -0500 )edit

Thanks @gvdhoorn. I agree with you however the stamp is given by ROS_INFO() command and it shows the same problem.

"Note also that both are/have to be set by the originating node,they are not handled by the middleware itself." I don't understand properly your sentence. Can you explain me please?

Agachon gravatar imageAgachon ( 2016-01-11 03:17:05 -0500 )edit

however the stamp is given by ROS_INFO() command [..]

no it is not. That is the time the log message was constructed, and has nothing to do with the message timestamp.

re: last sentence: I'm just pointing out that you should be aware that timestamps are assigned by nodes, not the middleware.

gvdhoorn gravatar imagegvdhoorn ( 2016-01-11 03:34:23 -0500 )edit

Ok I think I understand. I put header.stamp in the ROS_INFO and the display seems to be good but I still have question. Is it possible to print time stamp of the message chronologically ? image1 at t=1 image2 at t=1 IMU at t=2 IMU at t=3 ... image1 at t=10 image2 at t=10 IMU at t=11 IMU at t=12 ...

Agachon gravatar imageAgachon ( 2016-01-11 11:49:40 -0500 )edit