Issue with message_filters and PointCloud2 in python
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?
Asked by Stanny on 2023-04-19 05:34:53 UTC
Comments