64 lines
1.9 KiB
Python
64 lines
1.9 KiB
Python
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) |