How to synchronize two topics without message loss ?

edit

Hi all,

Context

In order to assess the performance of AMCL, I am trying to synchronize two topics :

• The estimated pose from AMCL output

• The scan from the laser range

  /scan topic is published at 25Hz

  /localizer_pose topic is published at a variable frequency depending on the robot's movement (Max is about 3Hz).

The topics are not synchronized, as AMCL introduce a variable delay. As the /localizer_pose topic is published at a low frequency, I do not want to lose any pose message, or at worse very few.

I use a bagfile to replay the scenario to be able to compare different solutions. I also added a simple subscriber on the /localizer_pose topic, to get the total amount of pose received.

ROS Indigo, with Ubuntu Server 14.04.

Test1: Message filters, exact time

First test, add a TimeSynchronizer to see if an exact timestamp match could do the trick.

//Declarations
message_filters::Subscriber<sensor_msgs::LaserScan>* m_scanSubscriber;
message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* m_localizerPoseMsgFilterSubscriber;
message_filters::TimeSynchronizer<geometry_msgs::PoseWithCovarianceStamped, sensor_msgs::LaserScan>* m_scanLocalizerPoseSynchronizer;

//Callbacks
void MyClass::localizerPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose)
{
    m_localizerPoseCount++;
}

void MyClass::localizerPoseWithScanCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose, const sensor_msgs::LaserScan::ConstPtr& laserScan)
{
    m_localizerPoseWithScanCount++;
}

//Initialization
m_localizerPoseMsgFilterSubscriber = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(m_nodeHandle, m_localizerPoseInputTopic, 5);
m_scanSubscriber = new message_filters::Subscriber<sensor_msgs::LaserScan>(m_nodeHandle, m_laserScanTopic, 50);

m_scanLocalizerPoseSynchronizer = new message_filters::TimeSynchronizer<geometry_msgs::PoseWithCovarianceStamped, sensor_msgs::LaserScan>(*m_localizerPoseMsgFilterSubscriber, *m_scanSubscriber, 10);
m_scanLocalizerPoseSynchronizer->registerCallback(boost::bind(&MyClass::localizerPoseWithScanCallback, this, _1, _2));

Results: I get 95 pairs for 203 poses, pretty bad, as expected.

Test2: Message filters, approximate time

That's the best candidate for now.

//Declarations
message_filters::Subscriber<sensor_msgs::LaserScan>* m_scanSubscriber;
message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* m_localizerPoseMsgFilterSubscriber;
typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::PoseWithCovarianceStamped, sensor_msgs::LaserScan> SyncScanWithLocalizerPose;
message_filters::Synchronizer<SyncScanWithLocalizerPose>* m_scanLocalizerPoseSynchronizer;

//Callbacks
void MyClass::localizerPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose)
{
    m_localizerPoseCount++;
}

void MyClass::localizerPoseWithScanCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose, const sensor_msgs::LaserScan::ConstPtr& laserScan)
{
    m_localizerPoseWithScanCount++;
}

//Initialization
m_localizerPoseMsgFilterSubscriber = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(m_nodeHandle, m_localizerPoseInputTopic, 5);
m_scanSubscriber = new message_filters::Subscriber<sensor_msgs::LaserScan>(m_nodeHandle, m_laserScanTopic, 50);

m_scanLocalizerPoseSynchronizer = new message_filters::Synchronizer<SyncScanWithLocalizerPose>(SyncScanWithLocalizerPose(10), *m_localizerPoseMsgFilterSubscriber, *m_scanSubscriber);
m_scanLocalizerPoseSynchronizer->registerCallback(&MyClass::localizerPoseWithScanCallback, this);

Results: Unfortunately, the results are not better: 95 pairs for 204 poses. Another run gave me 84 pairs for 194 total poses.

Test2: Tuning

I tried with AMCL tf_tolerance at 0 and at 0.1 (this is the default value), no difference.

I also tried to set the inter-message lower bound (parameter of ApproximateTime policy) to 0.02 (Half a period of laser scan) and to set the maximum interval duration (parameter of ApproximateTime policy) to 0.15 (maximum allowable time difference between pose and scan ?), but no difference.

So far, the approximate time didn't gave me good results, so I am wondering what I am doing wrong ? According to the documentation I would expect to not loose any pose message, and without doing too much parameter tuning.

Any thoughts ?

Thanks