probe_frame.py

#!/usr/bin/env python3
"""Probe one HbmMsg1080P frame: print fields + save a PNG to verify decoding."""
import sys
sys.path.insert(0, '/opt/tros/humble/lib/python3.10/site-packages')
import numpy as np
import cv2

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
from hbm_img_msgs.msg import HbmMsg1080P

class P(Node):
    def __init__(self):
        super().__init__('probe_one')
        qos = QoSProfile(depth=2, reliability=QoSReliabilityPolicy.BEST_EFFORT,
                         durability=QoSDurabilityPolicy.VOLATILE,
                         history=QoSHistoryPolicy.KEEP_LAST)
        self.create_subscription(HbmMsg1080P, '/hbmem_img', self.cb, qos)
        self.done = False
    def cb(self, m):
        enc = bytes(m.encoding).rstrip(b'\x00').decode('ascii', 'replace')
        print(f"height={m.height} width={m.width} data_size={m.data_size} step={m.step} encoding={enc!r}")
        data = bytes(m.data)
        print(f"len(data)={len(data)}")
        if enc == 'bgr8':
            img = np.frombuffer(data[:m.height*m.width*3], dtype=np.uint8).reshape((m.height, m.width, 3))
        elif enc == 'nv12':
            img = cv2.cvtColor(np.frombuffer(data[:m.height*m.width*3//2], dtype=np.uint8).reshape((m.height*3//2, m.width)), cv2.COLOR_YUV2BGR_NV12)
        else:
            img = None
            print("unknown encoding")
        if img is not None:
            cv2.imwrite('/tmp/mipi_frame.png', img)
            print(f"saved /tmp/mipi_frame.png shape={img.shape} mean={img.mean():.1f}")
        self.done = True

def main():
    rclpy.init()
    n = P()
    import time
    t0 = time.time()
    while not n.done and time.time()-t0 < 8:
        rclpy.spin_once(n, timeout_sec=0.1)
    n.destroy_node()
    rclpy.shutdown()
    print("done" if n.done else "NO FRAME RECEIVED")

main()