message_filters::sync::ApproximateTime with variable number of topics
Hi,
I am still new to ROS. I am trying to approximately synchronize n
cameras where n
is given at run-time. I saw some examples showing how to do this with n
cameras when n
is given at build time. For example, given that I have a ROS node with two cameras publishing on ROS topic camera/image/cam0
and camera/image/cam1
, the code below would do what I want.
void Callback(const sensor_msgs::ImageConstPtr& left_msg, const sensor_msgs::ImageConstPtr& right_msg)
{
ROS_INFO("Callback");
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "Sync_publisher_node");
ros::NodeHandle nh;
message_filters::Subscriber<sensor_msgs::Image> left_image(nh, "camera/image/cam0", 1);
message_filters::Subscriber<sensor_msgs::Image> right_image(nh, "camera/image/cam1", 1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> SyncPolicy;
message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), left_image, right_image);
ROS_INFO("Created Synchronizer");
sync.registerCallback(boost::bind(Callback, _1, _2));
ROS_INFO("Registered callback");
ros::spin();
return 0;
}
What I would like to do is something like this:
void syncCallback(const sensor_msgs::ImageConstPtr& left_msg, ...)
{
ROS_INFO("Callback");
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "Sync_publisher_node");
ros::NodeHandle nh;
uint8_t n_cam = atoi(argv[1]);
std::vector<message_filters::Subscriber<sensor_msgs::Image>> img_filter;
for (size_t i=0; i < n_cam; i++)
{
img_filter.push_back(message_filters::Subscriber<sensor_msgs::Image>(nh, "camera/image/cam" + std::to_string(i), 1));
}
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, <...>> SyncPolicy;
message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), left_image, <...>);
ROS_INFO("Created Synchronizer");
sync.registerCallback(boost::bind(&syncCallback, <...>));
ROS_INFO("Registered callback");
ros::spin();
return 0;
}
So I am wondering is there a way to dynamically construct the message_filters synchronizer? I saw the chain filter to dynamically add multiple filters together, but I am not sure how to use it with the synchronizer.