Subcriber callback's rate is lower then publisher's publish rate

edit

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.