publish after processing data from 2 synced subscribers (rospy)

asked 2021-04-04 02:46:29 -0600

kidpaul
38 ●11 ●17 ●20

updated 2021-04-04 02:51:44 -0600

I want to make one node to subscribe 2 topics synchronously, process those data, and publish it. I know that publishing and subscribing data with single node is possible, but have never seen any example with time synchronization in rospy. This is what I wrote so far (std_msgs in the code will be replaced with sensor_msgs later on to subscribe RGB image and Point cloud):

#!/usr/bin/env python
import rospy
import message_filters
import cv2
from cv_bridge import CvBridge
from std_msgs.msg import String

def callback(data1, data2):
    rospy.loginfo(rospy.get_caller_id())

def listener():
    sub_1 = message_filters.Subscriber("chatter", String)
    sub_2 = message_filters.Subscriber("sub_chatter", String)
    ts = message_filters.ApproximateTimeSynchronizer([sub_1, sub_2], 10, 0.1, allow_headerless=True)
    ts.registerCallback(callback)

def talker():
    pub = rospy.Publisher('Publisher', String, queue_size=10)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        pub.publish()
        rate.sleep()

if __name__ == '__main__':
    rospy.init_node('SubPub', anonymous=True)
    listener()
    talker()
    rospy.spin()

I'm currently getting stuck on how to pass sub_1 and sub_2 to the publisher. I think that I need to define class and put everything into that class and use flag to publish data whenever callback is called. I'm kind of unsure how to integrate all these features or my idea is even valid. Does anyone have idea or example?

edit retag flag offensive close merge delete

add a comment

1

answered 2021-04-04 07:16:37 -0600

abhishek47
228 ●6 ●30 ●18

The code has a few bugs.

In talker(), you've got an infinite while() but you don't really do anything there (pub.publish() isn't publishing any meaningful content).
Why do you have both while not rospy.is_shutdown() and rospy.spin()? Also, rospy.spin() is unreachable in your code.

Based on message_filters Python example, I re-wrote your code (code is untested):

def callback(data1, data2):
    rospy.loginfo(rospy.get_caller_id())
    ## do stuff with data1 and data2 ##
    pub.publish("some_string")


if __name__ == '__main__':
    rospy.init_node('SubPub', anonymous=True)
    pub = rospy.Publisher('Publisher', String, queue_size=10)
    sub_1 = message_filters.Subscriber("chatter", String)
    sub_2 = message_filters.Subscriber("sub_chatter", String)
    ts = message_filters.ApproximateTimeSynchronizer([sub_1, sub_2], 10, 0.1, allow_headerless=True)
    ts.registerCallback(callback)
    rospy.spin()

edit flag offensive delete link

Comments

I was just unsure how to pass data1 and data2 to pub.publish(). And yeah, I misunderstood what rospy.spin does. Thank you for pointing it out.

kidpaul ( 2021-04-04 13:15:40 -0600 )edit

2

And just to clarify: nothing will be synced here, or at least not properly. I understand this is only an example/test, but String does not have a std_msgs/Header, which means, with allow_headerless=True, the time at which msgs were received will be used to synchronise them.

This is obviously not what you'd really want in most cases.