Regarding your first question, you want to set the setMaxIntervalDuration
. As far as I understand from a quick look at the sources, a higher value for setAgePenalty
makes the algorithm more sloppy for older messages (to get them out of the queue faster) and setInterMessageLowerBound
allows to set a minimum delay between messages of a single input of your filter (messages arriving faster than that will be dropped).
As for the second question, you can just feed a SimpleFilter as an input to your Synchronizer:
#include <ros/ros.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/pass_through.h>
#include <message_filters/simple_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
void callback(const sensor_msgs::Image::ConstPtr& imagemessage1,
const sensor_msgs::Image::ConstPtr& imagemessage2) {
}
int main(int argc, char ** argv) {
ros::init(argc, argv, "test_synchronizer");
ros::NodeHandle nodehandle;
image_transport::ImageTransport imagetransport{nodehandle};
image_transport::SubscriberFilter imagesubscriber1;
imagesubscriber1.subscribe(imagetransport, "/image1", 32);
image_transport::SubscriberFilter imagesubscriber2;
imagesubscriber2.subscribe(imagetransport, "/image2", 32);
message_filters::PassThrough<sensor_msgs::Image> passthrough(imagesubscriber2);
message_filters::SimpleFilter<sensor_msgs::Image> & simplefilter = passthrough;
using MyPolicy = message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image, sensor_msgs::Image>;
MyPolicy mypolicy(32);
mypolicy.setMaxIntervalDuration(ros::Duration(1.e-3));
message_filters::Synchronizer<MyPolicy> synchronizer{
static_cast<const MyPolicy &>(mypolicy), imagesubscriber1, passthrough};
synchronizer.registerCallback(boost::bind(&callback, _1, _2));
ros::spin();
return EXIT_SUCCESS;
}
Official doc about message_filters ApproximateTime: https://wiki.ros.org/message_filters/...