Using your data

#!/usr/bin/env python3

from io import BytesIO

import av
import rclpy
from rclpy.node import Node
from rclpy.qos import (
    DurabilityPolicy,
    HistoryPolicy,
    QoSProfile,
    ReliabilityPolicy,
)
from sensor_msgs.msg import CompressedImage
from sentinel_msgs.msg import EncodedImage


CODECS = {
    "h264": "h264",
    "h265": "hevc",
    "vp8": "vp8",
    "vp9": "vp9",
    "av1": "av1",
}


class EncodedImageDecoder(Node):
    def __init__(self) -> None:
        super().__init__("encoded_image_decoder")

        self.declare_parameter(
            "input_topic", "/sentinel/vision/output/right_wrist/encoded"
        )
        self.declare_parameter(
            "output_topic", "/sentinel/vision/output/right_wrist/decoded/compressed"
        )

        input_topic = self.get_parameter("input_topic").value
        output_topic = self.get_parameter("output_topic").value

        video_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            history=HistoryPolicy.KEEP_LAST,
            depth=5,
        )

        self.publisher = self.create_publisher(
            CompressedImage, output_topic, video_qos
        )
        self.subscription = self.create_subscription(
            EncodedImage, input_topic, self.decode, video_qos
        )

        self.decoder = None
        self.stream_key = None
        self.frames_decoded = 0

        self.get_logger().info(f"Decoding {input_topic}")
        self.get_logger().info(f"Publishing {output_topic}")

    def decode(self, msg: EncodedImage) -> None:
        codec_name = CODECS.get(msg.encoding.lower())
        if codec_name is None:
            self.get_logger().error(f"Unsupported encoding: {msg.encoding}")
            return

        stream_key = (codec_name, msg.width, msg.height)
        if stream_key != self.stream_key:
            self.decoder = av.CodecContext.create(codec_name, "r")
            self.stream_key = stream_key
            self.get_logger().info(
                f"Initialized {codec_name} decoder for {msg.width}x{msg.height}"
            )

        packet = av.Packet(bytes(msg.data))
        packet.pts = msg.pts_ns
        packet.dts = msg.dts_ns if msg.dts_ns else msg.pts_ns

        try:
            frames = self.decoder.decode(packet)
        except Exception as error:
            # Playback may begin before an IDR frame. Decoding recovers at the
            # next keyframe.
            self.get_logger().debug(f"Waiting for a decodable frame: {error}")
            return

        for frame in frames:
            jpeg = BytesIO()
            frame.to_image().save(jpeg, format="JPEG", quality=90)

            output = CompressedImage()
            output.header = msg.header
            output.format = "jpeg"
            output.data = jpeg.getvalue()
            self.publisher.publish(output)

            self.frames_decoded += 1
            if self.frames_decoded == 1:
                self.get_logger().info("Published the first decoded frame")


def main() -> None:
    rclpy.init()
    node = EncodedImageDecoder()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()