[Solved] C++: ApproximateTime with more than two topics?
Hi @all,
short version:
Could you please provide me with a full version for synchronizing up to eight image topics with ROS?
long version:
I am currently trying to get my Synchronizer to work. Thanks to this forum I was able to get a Synchronizer to work with two topics (for interested users, see: http://wiki.ros.org/message_filters?d... ).
Now, all I have found so far deals with two topics. In my case, there are more than two topics (namely, four, six or eight image topics). I need to synchronize them too. And using ApproximateTime for that looks like a good option to me. However, I am getting errors doing so.
Essential code for two topics (Working):
#include <sensor_msgs/Image.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
class My_Synchronizer {
private:
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
typedef image_transport::SubscriberFilter ImageSubscriber;
typedef ros::Publisher ImagePublisher;
ImageSubscriber img1_sub_;
ImageSubscriber img2_sub_;
ImagePublisher img1_pub_;
ImagePublisher img2_pub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> ImageSyncPolicy_1; //two topics
message_filters::Synchronizer< ImageSyncPolicy_1 > sync_1;
//Constructor for two topics:
My_Synchronizer(
std::string t1,
std::string t2
) :
it_(nh_),
img1_sub_( it_, t1, 100),
img2_sub_( it_, t2, 100),
sync_1( ImageSyncPolicy_1( 10 ), img1_sub_, img2_sub_)
{
img1_pub_ = nh_.advertise<sensor_msgs::Image>("/synchronized/" + t1, 1000);
img2_pub_ = nh_.advertise<sensor_msgs::Image>("/synchronized/" + t2, 1000);
std::cout << "Image Synchronizer on two topics started" << std::endl;
sync_1.registerCallback( boost::bind( &My_Synchronizer::Sync1_Callback, this, _1, _2) );
};
void Sync1_Callback(
/*
Callback with two synchronized topics:
*/
const sensor_msgs::ImageConstPtr& image1,
const sensor_msgs::ImageConstPtr& image2
);
};
//Implementation of Sync1_Callback in separate file - not important for this question
Changes in Code for eight topics (Not Working!):
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image> ImageSyncPolicy_4; //eight topics
message_filters::Synchronizer< ImageSyncPolicy_4 > sync_4; //Does not work, but [...] sync_4(); works - still problems below
//Constructor for eight topics:
My_Synchronizer(
std::string t1,
std::string t2,
[...]
) :
it_(nh_),
img1_sub_( it_, t1, 100),
img2_sub_( it_, t2, 100),
[...]
sync_4( ImageSyncPolicy_1( 10 ), img1_sub_, img2_sub_, img3_sub_, img4_sub_, img5_sub_, img6_sub_, img7_sub_, img8_sub_)
{
img1_pub_ = nh_.advertise<sensor_msgs::Image>("/synchronized/" + t1, 1000);
img2_pub_ = nh_.advertise<sensor_msgs::Image>("/synchronized/" + t2, 1000);
[...]
std::cout << "Image Synchronizer on eight topics started" << std::endl;
sync_4.registerCallback( boost::bind( &My_Synchronizer::Sync4_Callback, this, _1, _2, _3, _4, _5, _6, _7, _8) );
};
void Sync4_Callback(
/*
Callback with eight synchronized topics:
*/
const sensor_msgs::ImageConstPtr& image1,
const sensor_msgs::ImageConstPtr& image2,
const sensor_msgs::ImageConstPtr& image3,
const sensor_msgs::ImageConstPtr& image4,
const sensor_msgs::ImageConstPtr& image5,
const sensor_msgs::ImageConstPtr& image6,
const sensor_msgs::ImageConstPtr& image7,
const sensor_msgs::ImageConstPtr& image8
);
The above code throws many boost related errors, about missing arguments or wrong function calls. I cannot really find out where the necessary change is to be made. Trying to use a SyncPolicy for four topics already throws this error for example:
error: no matching function for call to ‘message_filters::sync_policies::ApproximateTime<sensor_msgs::Image_<std::allocator<void> >, sensor_msgs::Image_<std::allocator<void> >, sensor_msgs::Image_<std::allocator<void> >, sensor_msgs::Image_<std::allocator<void> > >::ApproximateTime()’
Could you please tell me ...