2
$\begingroup$

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?

$\endgroup$
3
$\begingroup$

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.

$\endgroup$
5
  • $\begingroup$ Thanks for your clear suggestions. If my sensor readings comes at very different frequency such as 7 Hz, 100 Hz and 500 Hz, is there any recommendation you may have minds? $\endgroup$ – Ali Jan 31 '20 at 21:15
  • $\begingroup$ Unfortunately I think it totally depends on your application. If the 500 Hz sensor is far more important to your control loop, then maybe you should run at the higher rate. OTOH, if you can get the performance you need at 1 hz, then why run faster? Experiment a little and see what works. $\endgroup$ – doggie_breath Jan 31 '20 at 21:22
  • $\begingroup$ @doggie_breath do you have any suggestion about how to deal with stale values coming from tf2 transform listener? $\endgroup$ – Franky Feb 6 '20 at 3:39
  • $\begingroup$ @Franky What exactly is the problem you're working on? Might need to ask a new question in order to get enough detail. $\endgroup$ – doggie_breath Feb 6 '20 at 16:20
  • $\begingroup$ @doggie_breath I am controlling the CoM of a robot, however, I have to transform it from body frame to world frame. But tf2 Transformlistener throws stale values. I have actually asked the question separately, here is the link robotics.stackexchange.com/questions/20147/… $\endgroup$ – Franky Feb 7 '20 at 9:52

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.