Subcriber callback's rate is lower then publisher's publish rate
Hi,
I've been working on the implementation of a EKF and have been running into a strange issue. As some may be familiar the Kalman filter algorithm can be viewed as running two process. The Update phase done asynchronously each time a new measurement is received and the Prediction phase done at a fix rate (much higher then the fastest measurement rate).
In my implementation, each the sensors publish messages to which the EKF is subscribed and the prediction phase in executed in the main loop.
Here is the basic structure
Sensor1Callback(&msg)
{
{
boost::mutex::scoped_lock lock(sensor1)
Sensor1msg = *msg;
new_Sensor1 = true;
}
}
Sensor2Callback(&msg)
{
{
boost::mutex::scoped_lock lock(sensor2)
Sensor2msg = *msg;
new_Sensor2 = true;
}
}
main
{
// ...Some INIT stuff... //
ros::Subscriber Sensor1_sub = nh.subscribe("Sensor1", 1, Sensor1Callback);
ros::Subscriber Sensor2_sub = nh.subscribe("Sensor2", 1, Sensor2Callback);
ros::AsyncSpinner spinner(0);
spinner.start();
while(ros::ok())
{
ros::rate(250.0).sleep();
if (new_Sensor1 )
{
{
boost::mutex::scoped_lock lock(sensor1)
// ...Update processing... //
Sensor1_sanitycheck_publisher.publish(Sensor1msg);
new_Sensor1 = false;
}
}
if (new_Sensor2 )
{
{
boost::mutex::scoped_lock lock(sensor2)
// ...Update processing... //
Sensor2_sanitycheck_publisher.publish(Sensor2msg);
new_Sensor2 = false;
}
}
// ...Prediction processing... //
EKF_publisher.publish(EKF_Output);
}
}
Unfortunately, what I observe is :
Sensor 1 incoming rate: 100 hz
Sensor 2 incoming rate: 100 hz
Sensor1 sanitycheck publish rate : 60hz
Sensor2 sanitycheck publish rate : 60hz
EKF publish rate: 250 hz
I would expect the Sensor 1 & 2 sanitycheck rate to be 100 Hz as I would expect the callback to be executed at 100 Hz.
However if I remove the Sensor 2 subscriber the Sensor1 sanitycheck goes up to ~100 Hz.
If also tried without any luck:
removing all the processing part of the node.
a single thread spinOnce implementation.
moving all the update processing into the callbacks
increasing the main loop rate.
This situation seems to indicate that my understanding of the subscriber, publisher, callback mechanisms is flawed.
I would really appreciate if someone could help me better understand the mechanisms behind my observations.
Thanks
EDIT :
I'm now under the impression that the callback rates are correct and it's the displayed Hz of the original topic with are incorrect. i.e. Sensor1 and Sensor2 displayed rate is 100 Hz but are really published at 60 Hz.
Can you try to put the sanity check publishers into the subscriber callbacks? That should hopefully have them publish at the same rate as the data comes in. Since you only accept new sensor readings when the old one is processed the mix of different rates could cause that you loose messages
Hi, I've try publishing directly in the callbacks but I didn't see any change in the published topic rate unfortunately.
Try removing the locks from the callbacks as well, just having the publishers in them.