Ask Your Question
0

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

asked 2016-07-20 08:33:33 -0600

AlexandreB gravatar image

updated 2016-07-24 10:04:11 -0600

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.

edit retag flag offensive close merge delete

Comments

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

Chrissi gravatar imageChrissi ( 2016-07-21 10:47:37 -0600 )edit

Hi, I've try publishing directly in the callbacks but I didn't see any change in the published topic rate unfortunately.

AlexandreB gravatar imageAlexandreB ( 2016-07-22 07:35:57 -0600 )edit

Try removing the locks from the callbacks as well, just having the publishers in them.

Chrissi gravatar imageChrissi ( 2016-07-22 08:05:51 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-10-24 14:25:31 -0600

AlexandreB gravatar image

Finally, I found out that the issue was with the package rosbag itself. Playing the bag from the command line or the rqt bag played had two different frequency result. Still can't explain why but the algorithm is not at fault.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-07-20 08:33:33 -0600

Seen: 730 times

Last updated: Oct 24 '16