forked from Dan4ick/Lidar_Muxa
67 lines
2.1 KiB
Bash
67 lines
2.1 KiB
Bash
#!/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
|