Ask Your Question
1

How to synchronize two topics without message loss ?

asked 2016-11-23 05:08:26 -0600

Inounx gravatar image

updated 2016-11-23 06:02:40 -0600

Hi all,

Context

Synchronizer

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-04-07 05:06:53 -0600

dotcom gravatar image

I do not know how to make this work to be honest, so I usually have a workaround.

I do it with a message_filters::Cache.

  • Cache your /scan, if everything was smooth then a queue of (25/3) would be enough, but I would go for 100 messages at least.
  • In your /localizer_pose callback, you can retrieve the latest message from the /scan cache with getLatestTime(). You can also retrieve the latest message before a given timestamp.
edit flag offensive delete link more

Comments

Back then I worked around this problem by manually saving a laser scan and when receiving an AMCL pose, match with the last received scan. Didn't knew the option with the message filter cache, good to know.

Inounx gravatar imageInounx ( 2018-07-04 06:58:06 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2016-11-23 05:08:26 -0600

Seen: 1,093 times

Last updated: Apr 07 '18