Issue with message_filters and PointCloud2 in python

asked 2023-04-19 05:34:53 -0500

Stanny gravatar image
import rosbag
import rospy
from message_filters import TimeSynchronizer, Subscriber
import message_filters
from sensor_msgs.msg import Image, PointCloud2

# the first parameter is nodename and 'annonymous' is used to avoid when same nodename occur
rospy.init_node('synchronized_camera',  anonymous=True)


def callback(camera_front_center, camera_front_left, camera_front_right, camera_rear_center, camera_rear_left, camera_rear_right, radar_front_center, radar_front_left, radar_front_right, radar_rear_center, radar_rear_left, radar_rear_right):
    # the first parameter is topicname
    print("into callback")
    pub_camera_front_center = rospy.Publisher(
        'sync/camera/front_center', Image, queue_size=10)
    pub_camera_front_left = rospy.Publisher(
        'sync/camera/front_left', Image, queue_size=10)
    pub_camera_front_right = rospy.Publisher(
        'sync/camera/front_right', Image, queue_size=10)
    pub_camera_rear_center = rospy.Publisher(
        'sync/camera/rear_center', Image, queue_size=10)
    pub_camera_rear_left = rospy.Publisher(
        'sync/camera/rear_left', Image, queue_size=10)
    pub_camera_rear_right = rospy.Publisher(
        'sync/camera/rear_right', Image, queue_size=10)

    pub_radar_front_center = rospy.Publisher(
        'sync/radar/front_center', PointCloud2, queue_size=10)
    pub_radar_front_left = rospy.Publisher(
        'sync/radar/front_left', PointCloud2, queue_size=10)
    pub_radar_front_right = rospy.Publisher(
        'sync/radar/front_right', PointCloud2, queue_size=10)
    pub_radar_rear_center = rospy.Publisher(
        'sync/radar/rear_center', PointCloud2, queue_size=10)
    pub_radar_rear_left = rospy.Publisher(
        'sync/radar/rear_left', PointCloud2, queue_size=10)
    pub_radar_rear_right = rospy.Publisher(
        'sync/radar/rear_right', PointCloud2, queue_size=10)

    # the first parameter is topicname
    pub_camera_front_center.publish(camera_front_center)
    pub_camera_front_left.publish(camera_front_left)
    pub_camera_front_right.publish(camera_front_right)
    pub_camera_rear_center.publish(camera_rear_center)
    pub_camera_rear_left.publish(camera_rear_left)
    pub_camera_rear_right.publish(camera_rear_right)

    pub_radar_front_center.publish(radar_front_center)
    pub_radar_front_left.publish(radar_front_left)
    pub_radar_front_right.publish(radar_front_right)
    pub_radar_rear_center.publish(radar_rear_center)
    pub_radar_rear_left.publish(radar_rear_left)
    pub_radar_rear_right.publish(radar_rear_right)


bag = rosbag.Bag('/home/stannyho/Documents/ros/bags/decomp_pc_0322_1.bag', 'r')
# outbag = rosbag.Bag('/home/stannyho/Documents/ros/bags/sync.bag', 'w')

camera_front_center = message_filters.Subscriber(
    '/rgb/front_center/original_image', Image)
camera_front_left = message_filters.Subscriber(
    '/rgb/front_left/original_image', Image)
camera_front_right = message_filters.Subscriber(
    '/rgb/front_right/original_image', Image)
camera_rear_center = message_filters.Subscriber(
    '/rgb/rear_center/original_image', Image)
camera_rear_left = message_filters.Subscriber(
    '/rgb/rear_left/original_image', Image)
camera_rear_right = message_filters.Subscriber(
    '/rgb/rear_right/original_image', Image)

radar_front_center = message_filters.Subscriber(
    '/radar/front_center/decoded_messages', PointCloud2)
radar_front_left = message_filters.Subscriber(
    '/radar/front_left/decoded_messages', PointCloud2)
radar_front_right = message_filters.Subscriber(
    '/radar/front_right/decoded_messages', PointCloud2)
radar_rear_center = message_filters.Subscriber(
    '/radar/rear_center/decoded_messages', PointCloud2)
radar_rear_left = message_filters.Subscriber(
    '/radar/rear_left/decoded_messages', PointCloud2)
radar_rear_right = message_filters.Subscriber(
    '/radar/rear_right/decoded_messages', PointCloud2)

for topic, msg, t in bag.read_messages(topics='/radar/front_center/decoded_messages'):
    print("timestamp = ", msg.header.stamp)
print("222222")

ats = TimeSynchronizer([camera_front_center,
                        camera_front_left,
                        camera_front_right,
                        camera_rear_center,
                        camera_rear_left,
                        camera_rear_right,
                        radar_front_center,
                        radar_front_left,
                        radar_front_right,
                        radar_rear_center,
                        radar_rear_left,
                        radar_rear_right], queue_size=10)

ats.registerCallback(callback)
rospy.spin()

I want to subscribe to pointcloud data and image data and sync them and then publish them again. I have written a short and simple script for this, but I can't get into the callback function, I can confirm "synchronized_camera" has subscribed all topics that I had played, why is this happening?

edit retag flag offensive close merge delete