ApproximateTime Policy of message filter doesn't filter right [closed]

edit

Hi all,

I would like to synchronize some of my sensordata (kinect and a webcam) in the mater of time. My webcam seems to put everything in a internal memory before transmitting it. Because of that I have a time difference of about 1 sec to the actual recorded scene. I tried to adjust the time-stamp in the header of webcam message before publishing and then use the message filter package to synchronize them as good as possible to my kinect images (openni_launch).

Here is my function which grabs an image from the webcam, fills in the header and publishes the data. (its the source code of the usb_cam with some minor tweaks)

bool take_and_send_image(){

         usb_cam_camera_grab_image(camera_image_);
         fillImage(img_, "rgb8", camera_image_->height, camera_image_->width, 3 *camera_image_->width, camera_image_->image);


         img_.header.stamp = ros::Time::now();
         ROS_INFO("Time now = %i.%i",img_.header.stamp.sec, img_.header.stamp.nsec );

         ros::Duration lag(5.0);
         ros::Time header_time = ros::Time::now()-lag;
         img_.header.stamp = header_time;
         info_.header.stamp = img_.header.stamp;

         ROS_INFO("Time lag = %i.%i",img_.header.stamp.sec, img_.header.stamp.nsec );


         image_pub_.publish(img_, info_);

            return true;


}

the output of this code indicates that the minus operation does the right thing.

[ INFO] [1357672295.986516662]: Time now = 1357672295.986394591
[ INFO] [1357672295.986791322]: Time lag = 1357672290.986699769

This is the subscriber which is pretty much the same as in the tutorial for the message filter. subscribing and calling the callback everytime two messages have the approximate same time.

void sync_callback(const ImageConstPtr&  webcam_image,const ImageConstPtr&  rgb_image){

     // save both images to disk



     ROS_INFO("syncro callback");
     ROS_INFO("Kinect Timestamp= %i.%i",rgb_image->header.stamp.sec,rgb_image->header.stamp.nsec);
     ROS_INFO("Webcam Timestamp= %i.%i",webcam_image->header.stamp.sec,webcam_image->header.stamp.nsec);

}



int main(int argc, char **argv){

      ros::init(argc, argv, "listener");
      ros::NodeHandle nh;
      image_transport::ImageTransport it(nh);

      image_transport::SubscriberFilter webcam_sub (it, "/panda_cam/image_raw", 20);
      image_transport::SubscriberFilter rgb_sub (it, "/camera/rgb/image", 20);
      Synchronizer<MySyncPolicy> sync(MySyncPolicy(1000), webcam_sub, rgb_sub);

      ROS_INFO("Ready to recieve messages");
      sync.registerCallback(boost::bind(&sync_callback, _1, _2));

      ros::spin();
      return 0;
}

The output of the time is indeed indicating that the message are 5 seconds behind the actual time.

[ INFO] [1357672295.859623746]: Kinect Timestamp= 1357672290.292021533
[ INFO] [1357672295.859715299]: Webcam Timestamp= 1357672290.301270495

But by observing the images (of a stopwatch) there is now real difference even when I ad 10 or more seconds to it. The Kinect header is still the original and pretty much on the original time and can be seen as my reference.

Would be great if someone could point me to the failure.