#!/usr/bin/env bash
# С какой частотой `ros2 bag play` реально отдаёт кадры — без нашей ноды.
# Нужно, чтобы отличить потери в нашем коде от предела самого транспорта.
set -u
BAG="${1:?укажите бэг}"
TOPIC="${2:-/sensing/lidar/hesai128/pointcloud}"

python3 - "$TOPIC" > /tmp/rate.txt 2>/dev/null <<'PY' &
import sys, time
import rclpy
from rclpy.node import Node
from rclpy.qos import (QoSProfile, QoSHistoryPolicy, QoSReliabilityPolicy,
                       QoSDurabilityPolicy)
from sensor_msgs.msg import PointCloud2

topic = sys.argv[1]
qos = QoSProfile(history=QoSHistoryPolicy.KEEP_LAST, depth=10,
                 reliability=QoSReliabilityPolicy.RELIABLE,
                 durability=QoSDurabilityPolicy.VOLATILE)


class Sink(Node):
    def __init__(self):
        super().__init__("sink")
        self.n = 0
        self.first = self.last = None
        self.bytes = 0
        self.create_subscription(PointCloud2, topic, self.cb, qos, raw=True)

    def cb(self, raw):
        now = time.perf_counter()
        if self.first is None:
            self.first = now
        self.last = now
        self.n += 1
        self.bytes += len(raw)


rclpy.init()
s = Sink()
try:
    rclpy.spin(s)
except KeyboardInterrupt:
    pass
finally:
    span = (s.last - s.first) if (s.first and s.last and s.last > s.first) else 0
    print(f"принято сообщений: {s.n}")
    if span > 0:
        print(f"за {span:.1f} с → {s.n / span:.2f} Гц")
        print(f"поток: {s.bytes / span / 1e6:.0f} МБ/с, размер кадра "
              f"{s.bytes / max(s.n, 1) / 1e6:.1f} МБ")
PY
SINK=$!
sleep 3

echo "=== проигрывание (приёмник только считает, ничего не обрабатывает) ==="
T0=$(date +%s.%N)
ros2 bag play "$BAG" >/dev/null 2>&1
T1=$(date +%s.%N)
awk -v a="$T0" -v b="$T1" 'BEGIN{printf "бэг проигран за %.1f с
", b-a}'

sleep 2
kill -INT $SINK 2>/dev/null || true
wait $SINK 2>/dev/null || true
echo
cat /tmp/rate.txt