(Reverted driver) Fix H264 stream, inject sps and pps data on keyframe with config opt

sensors/video/sensorhub-driver-ffmpeg/README.md

@@ -24,6 +24,7 @@ When added to an OpenSensorHub node, both of the drivers listed above have the f
   - **Connection String:** A string that indicates the source of the "live" MPEG TS stream. As currently implemented, this is an ffmpeg connection string, and can take a number of forms. Common examples are URLs, e.g. `https://myserver.local/path/to/stream.m2ts`, or TCP endpoints, e.g. `tcp://192.168.1.200:1234`. See the [ffmpeg documentation for more details of the format of this string](https://www.ffmpeg.org/ffmpeg-protocols.html). Only one of **File Path** and **Connection String** should be set. (Though if both are set, **File Path** is used.)
   - **FPS:** When **File Path** is set, this indicates how fast the data is streamed. A value of `0` means "stream as quickly as possible", and can be used to load historical data into the OSH historical database quickly.
   - **Loop:** When checked and **File Path** is set, indicates that the data should be looped to create a continuous stream of data.
+  - **Inject Extradata:** Only for H264. When checked, injects Annex B extradata into the video stream before every keyframe. This is necessary for late-join decoders (those that receive live data from the driver after the driver has started).
 
 - **Outputs:** Each of the following causes a specific output (data stream) to be separately emitted by the sensor.
   - **Airframe Position:** If enabled, the sensor will have a separate output that provides lat, lon, alt, heading, pitch, and roll of the airframe.

sensors/video/sensorhub-driver-ffmpeg/src/main/java/org/sensorhub/impl/sensor/ffmpeg/FFMPEGSensorBase.java

@@ -196,6 +196,9 @@ protected void openStream() throws SensorHubException {
 	        } else {
 	        	throw new SensorHubException("Either the input file path or the connection string must be set");
 	        }
+            if (mpegTsProcessor != null) {
+                mpegTsProcessor.injectExtradata(config.connection.injectExtradata);
+            }
 
 	        if (mpegTsProcessor.openStream()) {
 	        	logger.info("Stream opened for {}", getUniqueIdentifier());

sensors/video/sensorhub-driver-ffmpeg/src/main/java/org/sensorhub/impl/sensor/ffmpeg/config/Connection.java

@@ -42,6 +42,9 @@ public class Connection {
     @DisplayInfo(label = "MJPEG", desc = "Select if video codec format is MJPEG. Otherwise driver will use H264.")
     public boolean isMJPEG = false;
 
+    @DisplayInfo(label = "Inject Extradata for Streaming", desc = "Injects extradata into the video stream. Set true if this driver is being used to output a live video stream for late-join decoders.")
+    public boolean injectExtradata = true;
+
     @DisplayInfo(label = "Ignore Data Timestamps", desc = "This ignores any data timestamps and defaults to current system time. This is necessary if video stream does not contain any timestamps")
     public boolean ignoreDataTimestamps = true;
 }

sensors/video/sensorhub-driver-ffmpeg/src/main/java/org/sensorhub/mpegts/MpegTsProcessor.java

@@ -22,10 +22,12 @@
 import org.bytedeco.ffmpeg.global.avcodec;
 import org.bytedeco.ffmpeg.global.avformat;
 import org.bytedeco.ffmpeg.global.avutil;
+import org.bytedeco.javacpp.BytePointer;
 import org.bytedeco.javacpp.PointerPointer;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 
+import java.io.ByteArrayOutputStream;
 import java.util.concurrent.atomic.AtomicBoolean;
 
 import static org.bytedeco.ffmpeg.global.avformat.av_read_frame;
@@ -177,6 +179,9 @@ public class MpegTsProcessor extends Thread {
      * If true, play the video file continuously in a loop
      */
     volatile boolean loop;
+
+    byte[] spsPpsHeader;
+    boolean doInjectExtradata = false;
 
 
     /**
@@ -208,6 +213,10 @@ public MpegTsProcessor(String source, int fps, boolean loop) {
         this.loop = loop;
     }
 
+    public void injectExtradata(boolean doInject) {
+        this.doInjectExtradata = doInject;
+    }
+
     /**
      * Attempts to open the stream given the {@link MpegTsProcessor#streamSource}
      * Opening the stream entails establishing appropriate connection and ability to
@@ -271,6 +280,7 @@ public void queryEmbeddedStreams() {
         for (int streamId = 0; streamId < avFormatContext.nb_streams(); ++streamId) {
 
             int codecType = avFormatContext.streams(streamId).codecpar().codec_type();
+            int codecId = avFormatContext.streams(streamId).codecpar().codec_id();
 
             AVRational timeBase = avFormatContext.streams(streamId).time_base();
 
@@ -288,6 +298,8 @@ public void queryEmbeddedStreams() {
                 videoStreamTimeBase = timeBaseUnits;
             }
 
+
+
 //            if (INVALID_STREAM_ID == dataStreamId && avutil.AVMEDIA_TYPE_DATA == codecType) {
 //
 //                logger.debug("Data stream present with id: {}", streamId);
@@ -297,6 +309,12 @@ public void queryEmbeddedStreams() {
 //                dataStreamTimeBase = timeBaseUnits;
 //            }
         }
+        int extraSize = avFormatContext.streams(videoStreamId).codecpar().extradata_size();
+        // Get extradata if the data exists, the option was set in the config, and the codec is H264.
+        if (extraSize > 0 && doInjectExtradata && avFormatContext.streams(videoStreamId).codecpar().codec_id() == avcodec.AV_CODEC_ID_H264) {
+            //spsPpsHeader = new byte[extraSize];
+            spsPpsHeader = getAnnexBExtradata(avFormatContext.streams(videoStreamId).codecpar().extradata(), extraSize);
+        }
     }
 
     /**
@@ -493,6 +511,8 @@ private void processStreamPackets() {
                 // If it is a video or data frame and there is a listener registered
                 if ((avPacket.stream_index() == videoStreamId) && (null != videoDataBufferListener)) {
 
+                    boolean isKeyFrame = (avPacket.flags() & avcodec.AV_PKT_FLAG_KEY) != 0;
+
                     // Process video packet
                     byte[] dataBuffer = new byte[avPacket.size()];
                     avPacket.data().get(dataBuffer);
@@ -511,8 +531,13 @@ private void processStreamPackets() {
                         }
                     }
 
-                    // Pass data buffer to interested listener
+
                     frameCount++;
+                    // Inject available extradata before keyframe
+                    if (isKeyFrame && spsPpsHeader != null) {
+                        videoDataBufferListener.onDataBuffer(new DataBufferRecord(avPacket.pts() * videoStreamTimeBase, spsPpsHeader));
+                    }
+                    // Pass data buffer to interested listener
                     videoDataBufferListener.onDataBuffer(new DataBufferRecord(avPacket.pts() * videoStreamTimeBase, dataBuffer));
                 }
 //                 else if ((avPacket.stream_index() == dataStreamId) && (null != metadataDataBufferListener)) {
@@ -628,6 +653,47 @@ private void openCodecContext() throws IllegalStateException {
         }
     }
 
+    private byte[] getAnnexBExtradata(BytePointer extradata, int size) {
+        if (extradata == null || size < 7) return null;
+
+        byte[] data = new byte[size];
+        extradata.get(data);
+
+        // Check for Annex B start code, either 0x000001 or 0x00000001
+        if (data[0] == 0x00 && data[1] == 0x00 && (data[2] == 0x01 || (data[2] == 0x00 && data[3] == 0x01))) {
+            // Already in Annex B format, use directly
+            return data;
+        }
+
+        // Otherwise, we need to convert AVCC to Annex B format
+        ByteArrayOutputStream out = new ByteArrayOutputStream();
+        int pos = 5;
+        int numSps = data[pos++] & 0x1F;
+        try {
+            for (int i = 0; i < numSps; i++) {
+                if (pos + 2 > data.length) return null;
+                int spsLen = ((data[pos++] & 0xFF) << 8) | (data[pos++] & 0xFF);
+                if (pos + spsLen > data.length) return null;
+                out.write(new byte[]{0x00, 0x00, 0x00, 0x01});
+                out.write(data, pos, spsLen);
+                pos += spsLen;
+            }
+
+            int numPps = data[pos++] & 0xFF;
+            for (int i = 0; i < numPps; i++) {
+                if (pos + 2 > data.length) return null;
+                int ppsLen = ((data[pos++] & 0xFF) << 8) | (data[pos++] & 0xFF);
+                if (pos + ppsLen > data.length) return null;
+                out.write(new byte[]{0x00, 0x00, 0x00, 0x01});
+                out.write(data, pos, ppsLen);
+                pos += ppsLen;
+            }
+        } catch (Exception e) {
+            logger.error("Error extracting SPS and PPS from AVCC extradata", e);
+        }
+        return out.toByteArray();
+    }
+
 //    public String getCodecFormat() {
 ////        String codecFormat = avCodecContext.codec().long_name().getString(StandardCharsets.UTF_8);
 //        String codecFormat = "temporary";