import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import PoseWithCovarianceStamped

PC_TOPIC = '/localization/util/downsample/pointcloud'
POSE_TOPIC = '/localization/pose_twist_fusion_filter/biased_pose_with_covariance'

pc_qos = QoSProfile(
    history=QoSHistoryPolicy.KEEP_LAST,
    depth=5,
    reliability=QoSReliabilityPolicy.BEST_EFFORT,
    durability=QoSDurabilityPolicy.VOLATILE,
)

pose_qos = QoSProfile(
    history=QoSHistoryPolicy.KEEP_LAST,
    depth=10,
    reliability=QoSReliabilityPolicy.RELIABLE,
    durability=QoSDurabilityPolicy.VOLATILE,
)

class OnceStampDiff(Node):
    def __init__(self):
        super().__init__('once_stamp_diff_checker')
        self.pc_msg = None
        self.pose_msg = None

        self.create_subscription(PointCloud2, PC_TOPIC, self.pc_cb, pc_qos)
        self.create_subscription(PoseWithCovarianceStamped, POSE_TOPIC, self.pose_cb, pose_qos)

    def to_sec(self, stamp):
        return stamp.sec + stamp.nanosec * 1e-9

    def pc_cb(self, msg):
        self.pc_msg = msg
        self.try_print()

    def pose_cb(self, msg):
        self.pose_msg = msg
        self.try_print()

    def try_print(self):
        if self.pc_msg is None or self.pose_msg is None:
            return

        pc_t = self.to_sec(self.pc_msg.header.stamp)
        pose_t = self.to_sec(self.pose_msg.header.stamp)
        diff = pose_t - pc_t

        now = self.get_clock().now().nanoseconds * 1e-9

        print(f'pointcloud stamp: {pc_t:.9f}')
        print(f'pose stamp:       {pose_t:.9f}')
        print(f'pose - pointcloud: {diff:.6f} s')
        print(f'pointcloud delay:  {now - pc_t:.6f} s')
        print(f'pose delay:        {now - pose_t:.6f} s')

        rclpy.shutdown()

rclpy.init()
node = OnceStampDiff()
rclpy.spin(node)