Usage of Time Synchronizer with rosbag
Hi there, I am writing a ros node which subscribe to 3 lasers range finders (Sick LMS), and do some processes on them. I am runing on ros Fuerte.
So I use a message_filter::TimeSynchronizer to synchronize them, and to centralize all the datas in an unique callback.
class LaserSubscriber
{
typedef message_filters::Subscriber<sensor_msgs::LaserScan> tLaserSub;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::LaserScan, sensor_msgs::LaserScan,
sensor_msgs::LaserScan> tApproxTimeLaser;
typedef message_filters::Synchronizer<tApproxTimeLaser> tSynchronizer;
public:
LaserSubscriber();
void OnNewScans(LaserListener::tLaser &_scanVert1, LaserListener::tLaser &_scanVert2,
LaserListener::tLaser &_scanHoriz);
//...
};
So as you can see, I am using the ApproximateTime sync policy.
When I play with rosbag, I have a strange behaviour. If I launch rosbag play with the "loop" and "clock" options, the first loop will works well, but not the second one. In the second loop, the call back is never called.
I think it's because the synchronizer is waiting for a data which will never be published. When /clock is "x" (the end of my rosbag", and my rosbag restart, /clock is "y" and y < x, so the synchrnonizer seems to wait for a data stamped with something greater than x (that is what we want when we use a synchroniser).
TF have the same problem, but TF is very smart, it detect a jump in time and reset the cache.
My reasoning is it good ? Does the synchronizer can do the same ? Is it possible to reset manualy a synchronizer ?
Regards,
Asked by abarral on 2013-12-02 04:41:01 UTC
Answers
It looks like calling setReset(true)
on the Synchronizer is what you want:
https://github.com/ros/ros_comm/blob/noetic-devel/utilities/message_filters/include/message_filters/sync_policies/approximate_time.h#L309-L313
void setReset(const bool reset)
{
// Set this true to reset queue on ROS time jumped back
enable_reset_ = reset;
}
if (ros::Time::isSimTime() && enable_reset_)
{
if (now < last_stamps_[i])
{
++num_reset_deques_;
if (num_reset_deques_ == 1)
{
ROS_WARN("Detected jump back in time. Clearing message filter queues");
}
clearDeque<i>();
if (num_reset_deques_ >= RealTypeCount::value)
{
num_reset_deques_ = 0;
}
}
}
Asked by lucasw on 2022-10-07 12:08:49 UTC
Comments
Have you found an answer to this? I'm experiencing the very same problem right now and would appreciate an answer. Thanks!
Asked by Hendrik Wiese on 2017-11-13 11:36:56 UTC
I am trying to solve this problem too...
Asked by davidQ on 2019-04-05 05:45:23 UTC