Problems publishing synchronized topics
I want to publish image synchronized topics using following code:
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
using namespace sensor_msgs;
using namespace message_filters;
void callback(const ImageConstPtr& left_image, const CameraInfoConstPtr& left_cam_info, const ImageConstPtr& right_image, const CameraInfoConstPtr& right_cam_info)
{
// Solve all of perception here...
printf("-----------------------\n");
std::cout << "Time stamp (left image): "<< left_image->header.stamp << std::endl;
std::cout << "Time stamp (right image): "<< right_image->header.stamp << std::endl;
std::cout << "Time stamp (left camera info): "<< left_cam_info->header.stamp << std::endl;
std::cout << "Time stamp (right camera info): "<< right_cam_info->header.stamp << std::endl;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "vizzy_sync_node");
ros::NodeHandle nh;
message_filters::Subscriber<Image> left_image_sub(nh, "vizzy/left/image_raw", 1);
message_filters::Subscriber<CameraInfo> left_info_sub(nh, "vizzy/left/camera_info", 1);
message_filters::Subscriber<Image> right_image_sub(nh, "vizzy/right/image_raw", 1);
message_filters::Subscriber<CameraInfo> right_info_sub(nh, "vizzy/right/camera_info", 1);
TimeSynchronizer<Image, CameraInfo, Image, CameraInfo> sync(left_image_sub, left_info_sub, right_image_sub, right_info_sub, 10);
sync.registerCallback(boost::bind(&callback, _1, _2, _3, _4)); // cria 4 callbacks
ros::spin();
return 0;
}
but I see that the Callback function is not fired and don't know why. Anyone can help me and if possible tell me if in order to output the synchronized topics I can just publish them with simple advertise?
Are you sure the time stamps do EXACTLY match? The TimeSynchronizer requires this for firing the callback, no epsilon allowed....