ROS2 bag dropping syncronized messages

asked 2023-04-19 03:50:15 -0600

jerek gravatar image

updated 2023-04-19 03:51:23 -0600

Ubuntu 20.04 os. ros2 foxy zed 4.0.0 sdk velodyne ros2 driver

I have following code that syncronizes various messages from two lidars and zed2 camera. For some reason ros2 bag drops messages. What may cause this problem. I record my back with the following command: ros2 bag record /synced_pc_lidar_1 /synced_pc_lidar_2 /synced_odom_cam /synced_cam_pose -o ros_bags/Sync_test_6_4_v15. I also use the Foxy Future branch. If I also add image message, more messages are dropped.

class ExampleNode(Node):
    def __init__(self):
        super().__init__("ExampleNode")


        #self.image_sub = Subscriber(self, Image, "/zed2/zed_node/left/image_rect_color", qos_profile=qos_profile_sensor_data)

        # Subscribers for velodyne pointcloud
        self.plc_sub_1 = Subscriber(self, PointCloud2, "/lidar_right/velodyne_points", qos_profile=qos_profile_sensor_data)
        self.plc_sub_2 = Subscriber(self, PointCloud2, "/lidar_left/velodyne_points", qos_profile=qos_profile_sensor_data)


        # Subscribers for camera odom data
        self.odom_sub_cam = Subscriber(self, Odometry, "/zed2/zed_node/odom", qos_profile=qos_profile_sensor_data)
        self.cam_info_sub = Subscriber(self, CameraInfo, "/zed2/zed_node/left/camera_info", qos_profile=qos_profile_sensor_data)
        self.pose_sub_cam = Subscriber(self, PoseStamped, "/zed2/zed_node/pose", qos_profile=qos_profile_sensor_data)


        #self.image_pub = self.create_publisher(Image, 'synced_img', 10)
        self.pcl_pub_1 = self.create_publisher(PointCloud2, 'synced_pc_lidar_1', 10)
        self.pcl_pub_2 = self.create_publisher(PointCloud2, 'synced_pc_lidar_2', 10)

        self.odom_pub_cam = self.create_publisher(Odometry, 'synced_odom_cam', 10)
        self.cam_info_pub = self.create_publisher(CameraInfo, 'synced_cam_info', 10)
        self.pose_pub = self.create_publisher(PoseStamped, 'synced_cam_pose', 10)
        #self.sync_publisher_ = self.create_publisher(Sync, 'synced_messages', 10)  # CHANGE

        queue_size = 100
        # you can use ApproximateTimeSynchronizer if msgs dont have exactly the same timestamp
        self.ts = ApproximateTimeSynchronizer(
            [self.plc_sub_1, self.plc_sub_2, self.odom_sub_cam, self.pose_sub_cam],
            queue_size,
            0.05,  # defines the delay (in seconds) with which messages can be synchronized
        )
        self.ts.registerCallback(self.callback)

    def callback(self, pcl_msg_1, pcl_msg_2, odom_msgs_cam, pose_msgs):        
        #self.image_pub.publish(image_msg)
        # Publish syncronized messages for rosbag
        self.pcl_pub_1.publish(pcl_msg_1)
        self.pcl_pub_2.publish(pcl_msg_2)
        self.odom_pub_cam.publish(odom_msgs_cam)
        self.pose_pub.publish(pose_msgs)


        print("Callback seems to be working")


def main():
    print('Hi from message_sync_python.')


    rclpy.init()
    syncronizeri = ExampleNode()
    rclpy.spin(syncronizeri)


if __name__ == '__main__':
    main()
edit retag flag offensive close merge delete