(not trivial) ROS how to subscribe to multiple topics and publish to another topic within one loop

Question

My question is related to this one (here) but not exactly. Assume now I have 4 topics: topic_A,topic_B,topic_C,topic_D. Inside my control loop, I need to read the sensor values from the first three topics (all of them are publishing at 1000 Hz for example), calculate my control input and publish the control input to topic_D for each iteration. My question is related to how to write the control loop properly. My current pseudo code is summarized as below:

1) I used the rospy.wait_for_message because it guarantees that my control_input is being calculated from the most recent sensor values (a few ms before I calculate my control_input). Now I want to replace them because it seems like the subscriber callback function is faster, which is very important for my application.

2) In terms of code structure, I didn't use subscriber because I didn't know how to incorporate three callback functions properly to publish my control gain (I need to the three sensor values to update even though I'm running the while loop in the main_control_loop function to get my control).

Based on the discussion from here. It looks like I can have several subscribers running together for rospy. Now assume I have three subscribers running as listed below, will the sensor_X_value be updated in each iteration of my main_control_loop function?

Answer 1

What you have there should be okay.

Basically, each time you construct a subscriber for a new topic, rospy.Subscriber(...), a thread will be started to poll for messages received on that topic. So in your code above, the callback functions will asynchronously handle processing the latest message received and updating the value accordingly. Maybe use a queue size of 1, to ensure it's only the latest value you're polling from your sensors (See rospy docs on publishers/subscribers.

So in your control loop, you shouldn't have to worry about it. Just make sure that your control loop rate is appropriate for the publishing rate of your control inputs.

def main_control_loop(self,scan,pose):

    # This is up to you
    r = rospy.Rate(1000)  # 1000 Hz

    while not rospy.is_shutdown():

        # compute control input
        # These will be the most up to date values
        control_input = some_function(self.topicA_value,
                                      self.topicB_value,
                                      self.topicC_value,
                                      **other args)

        self.pub_topicsD.publish(control_input)

        # sleep
        rate.sleep()

There are a few caveats here - one of which being that depending on your publisher queues, latency, etc... there's a strong likelihood that your control loop will update with "stale" values (ie, use the same value twice), but that all depends on your control loop rate and your publishers.

Also, I don't know what the application here is, but if you really need to run at 1000 Hz then you might consider writing your application in roscpp to have more control over what threads are doing what, and when.