Feature/capture magewell

scs2-session-logger/build.gradle.kts

@@ -14,7 +14,7 @@ mainDependencies {
    api("us.ihmc:scs2-session:source")
    api("us.ihmc:scs2-simulation:source") // TODO Need to fix this, it needs the Robot.
 
-   api("us.ihmc:ihmc-robot-data-logger:0.29.3")
+   api("us.ihmc:ihmc-robot-data-logger:0.29.4")
    api("com.github.luben:zstd-jni:1.5.5-10")
    api("org.antlr:antlr4-runtime:4.13.1")
    //api("org.lz4:lz4-java:1.8.0")

scs2-session-visualizer-jfx/src/main/java/us/ihmc/scs2/sessionVisualizer/jfx/session/log/BlackMagicVideoDataReader.java

@@ -0,0 +1,144 @@
+package us.ihmc.scs2.sessionVisualizer.jfx.session.log;
+
+import us.ihmc.codecs.demuxer.MP4VideoDemuxer;
+import us.ihmc.codecs.generated.YUVPicture;
+import us.ihmc.concurrent.ConcurrentCopier;
+import us.ihmc.robotDataLogger.Camera;
+import us.ihmc.scs2.session.log.ProgressConsumer;
+
+import java.io.File;
+import java.io.IOException;
+import java.io.PrintWriter;
+
+public class BlackMagicVideoDataReader implements VideoDataReader
+{
+   private final TimestampScrubber timestampScrubber;
+   private final String name;
+
+   private final MP4VideoDemuxer demuxer;
+   private final JavaFXPictureConverter converter = new JavaFXPictureConverter();
+
+   private final File videoFile;
+   private final Camera camera;
+   private final ConcurrentCopier<FrameData> imageBuffer = new ConcurrentCopier<>(FrameData::new);
+
+   public BlackMagicVideoDataReader(Camera camera, File dataDirectory, boolean hasTimeBase) throws IOException
+   {
+      this.camera = camera;
+      name = camera.getNameAsString();
+      boolean interlaced = camera.getInterlaced();
+
+      if (!hasTimeBase)
+      {
+         System.err.println("Video data is using timestamps instead of frame numbers. Falling back to seeking based on timestamp.");
+      }
+
+      videoFile = new File(dataDirectory, camera.getVideoFileAsString());
+
+      if (!videoFile.exists())
+      {
+         throw new IOException("Cannot find video: " + videoFile);
+      }
+
+      demuxer = new MP4VideoDemuxer(videoFile);
+
+      File timestampFile = new File(dataDirectory, camera.getTimestampFileAsString());
+      this.timestampScrubber = new TimestampScrubber(timestampFile, hasTimeBase, interlaced);
+   }
+
+   @Override
+   public int getImageHeight()
+   {
+      return 0;
+   }
+
+   @Override
+   public int getImageWidth()
+   {
+      return 0;
+   }
+
+   public void readVideoFrame(long queryRobotTimestamp)
+   {
+      long videoTimestamp = timestampScrubber.getVideoTimestampFromRobotTimestamp(queryRobotTimestamp);
+      long currentRobotTimestamp = timestampScrubber.getCurrentRobotTimestamp();
+
+      try
+      {
+         demuxer.seekToPTS(videoTimestamp);
+         FrameData copyForWriting = imageBuffer.getCopyForWriting();
+         copyForWriting.queryRobotTimestamp = queryRobotTimestamp;
+         copyForWriting.currentRobotTimestamp = currentRobotTimestamp;
+         copyForWriting.currentVideoTimestamp = videoTimestamp;
+         copyForWriting.currentDemuxerTimestamp = demuxer.getCurrentPTS();
+         YUVPicture nextFrame = demuxer.getNextFrame(); // Increment frame index after getting frame.
+         copyForWriting.frame = converter.toFXImage(nextFrame, copyForWriting.frame);
+
+         imageBuffer.commit();
+      }
+      catch (IOException e)
+      {
+         e.printStackTrace();
+      }
+   }
+
+   public void cropVideo(File outputFile, File timestampFile, long startTimestamp, long endTimestamp, ProgressConsumer monitor) throws IOException
+   {
+      long startVideoTimestamp = timestampScrubber.getVideoTimestampFromRobotTimestamp(startTimestamp);
+      long endVideoTimestamp = timestampScrubber.getVideoTimestampFromRobotTimestamp(endTimestamp);
+
+      int framerate = VideoConverter.cropBlackMagicVideo(videoFile, outputFile, startVideoTimestamp, endVideoTimestamp, monitor);
+
+      PrintWriter timestampWriter = new PrintWriter(timestampFile);
+      timestampWriter.println(1);
+      timestampWriter.println(framerate);
+
+      long pts = 0;
+      /*
+       * PTS gets reordered to be monotonically increasing starting from 0
+       */
+      for (int i = 0; i < timestampScrubber.getRobotTimestampsLength(); i++)
+      {
+         long robotTimestamp = timestampScrubber.getRobotTimestampAtIndex(i);
+
+         if (robotTimestamp >= startTimestamp && robotTimestamp <= endTimestamp)
+         {
+            timestampWriter.print(robotTimestamp);
+            timestampWriter.print(" ");
+            timestampWriter.println(pts);
+            pts++;
+         }
+         else if (robotTimestamp > endTimestamp)
+         {
+            break;
+         }
+      }
+
+      timestampWriter.close();
+   }
+
+   public String getName()
+   {
+      return name;
+   }
+
+   public Camera getCamera()
+   {
+      return camera;
+   }
+
+   public FrameData pollCurrentFrame()
+   {
+      return imageBuffer.getCopyForReading();
+   }
+
+   public int getCurrentIndex()
+   {
+      return timestampScrubber.getCurrentIndex();
+   }
+
+   public boolean replacedRobotTimestampsContainsIndex(int index)
+   {
+      return timestampScrubber.getReplacedRobotTimestampIndex(index);
+   }
+}

scs2-session-visualizer-jfx/src/main/java/us/ihmc/scs2/sessionVisualizer/jfx/session/log/FrameData.java

@@ -0,0 +1,16 @@
+package us.ihmc.scs2.sessionVisualizer.jfx.session.log;
+
+import javafx.scene.image.WritableImage;
+
+/**
+ * This class is used for debugging timestamp delays in the videos when viewing them in SCS2. In order to see the timestamp debugging information change the
+ * boolean in {@link VideoViewer#LOGGER_VIDEO_DEBUG} to true. This can also be set as an environmental variable.
+ */
+public class FrameData
+{
+   public WritableImage frame;
+   public long queryRobotTimestamp;
+   public long currentRobotTimestamp;
+   public long currentVideoTimestamp;
+   public long currentDemuxerTimestamp;
+}

scs2-session-visualizer-jfx/src/main/java/us/ihmc/scs2/sessionVisualizer/jfx/session/log/MagewellVideoDataReader.java

@@ -0,0 +1,205 @@
+package us.ihmc.scs2.sessionVisualizer.jfx.session.log;
+
+import javafx.scene.image.Image;
+import javafx.scene.image.PixelReader;
+import javafx.scene.image.PixelWriter;
+import javafx.scene.image.WritableImage;
+import org.bytedeco.javacv.Frame;
+import org.bytedeco.javacv.JavaFXFrameConverter;
+import us.ihmc.robotDataLogger.Camera;
+import us.ihmc.robotDataLogger.logger.MagewellDemuxer;
+import us.ihmc.robotDataLogger.logger.MagewellMuxer;
+import us.ihmc.scs2.session.log.ProgressConsumer;
+
+import java.io.File;
+import java.io.IOException;
+import java.io.PrintWriter;
+
+public class MagewellVideoDataReader implements VideoDataReader
+{
+   private final TimestampScrubber timestampScrubber;
+   private final String name;
+
+   private final MagewellDemuxer magewellDemuxer;
+
+   private final Camera camera;
+   private final FrameData frameData = new FrameData();
+
+   public MagewellVideoDataReader(Camera camera, File dataDirectory, boolean hasTimeBase) throws IOException
+   {
+      this.camera = camera;
+      name = camera.getNameAsString();
+      boolean interlaced = camera.getInterlaced();
+
+      if (!hasTimeBase)
+      {
+         System.err.println("Video data is using timestamps instead of frame numbers. Falling back to seeking based on timestamp.");
+      }
+
+      File videoFile = new File(dataDirectory, camera.getVideoFileAsString());
+
+      if (!videoFile.exists())
+      {
+         throw new IOException("Cannot find video: " + videoFile);
+      }
+
+      magewellDemuxer = new MagewellDemuxer(videoFile);
+
+      File timestampFile = new File(dataDirectory, camera.getTimestampFileAsString());
+      this.timestampScrubber = new TimestampScrubber(timestampFile, hasTimeBase, interlaced);
+   }
+
+   public int getImageHeight()
+   {
+      return magewellDemuxer.getImageHeight();
+   }
+
+   public int getImageWidth()
+   {
+      return magewellDemuxer.getImageWidth();
+   }
+
+   public void readVideoFrame(long queryRobotTimestamp)
+   {
+      long currentVideoTimestamps = timestampScrubber.getVideoTimestampFromRobotTimestamp(queryRobotTimestamp);
+      long currentRobotTimestamp = timestampScrubber.getCurrentRobotTimestamp();
+
+      magewellDemuxer.seekToPTS(currentVideoTimestamps);
+
+      // This is a copy that can be shown in the video view to debug timestamp issues
+      {
+         FrameData copyForWriting = frameData;
+         copyForWriting.queryRobotTimestamp = queryRobotTimestamp;
+         copyForWriting.currentRobotTimestamp = currentRobotTimestamp;
+         copyForWriting.currentVideoTimestamp = currentVideoTimestamps;
+         copyForWriting.currentDemuxerTimestamp = magewellDemuxer.getCurrentPTS();
+      }
+
+      Frame nextFrame = magewellDemuxer.getNextFrame();
+      frameData.frame = convertFrameToWritableImage(nextFrame);
+   }
+
+   /**
+    * This class converts a {@link Frame} to a {@link WritableImage} in order to be displayed correctly in JavaFX.
+    *
+    * @param frameToConvert is the next frame we want to visualize so we convert it to be compatible with JavaFX
+    * @return {@link WritableImage}
+    */
+   public WritableImage convertFrameToWritableImage(Frame frameToConvert)
+   {
+      Image currentImage;
+
+      if (frameToConvert == null)
+      {
+         return null;
+      }
+
+      try (JavaFXFrameConverter frameConverter = new JavaFXFrameConverter())
+      {
+         currentImage = frameConverter.convert(frameToConvert);
+      }
+      WritableImage writableImage = new WritableImage((int) currentImage.getWidth(), (int) currentImage.getHeight());
+      PixelReader pixelReader = currentImage.getPixelReader();
+      PixelWriter pixelWriter = writableImage.getPixelWriter();
+
+      for (int y = 0; y < currentImage.getHeight(); y++)
+      {
+         for (int x = 0; x < currentImage.getWidth(); x++)
+         {
+            pixelWriter.setArgb(x, y, pixelReader.getArgb(x, y));
+         }
+      }
+
+      return writableImage;
+   }
+
+   public Frame getFrame()
+   {
+      return magewellDemuxer.getNextFrame();
+   }
+
+   public void cropVideo(File outputFile, File timestampFile, long startTimestamp, long endTimestamp, ProgressConsumer progressConsumer) throws IOException
+   {
+      long startVideoTimestamp = timestampScrubber.getVideoTimestampFromRobotTimestamp(startTimestamp);
+      long endVideoTimestamp = timestampScrubber.getVideoTimestampFromRobotTimestamp(endTimestamp);
+
+      long[] robotTimestampsForCroppedLog = timestampScrubber.getCroppedRobotTimestamps(startTimestamp, endTimestamp);
+      long[] videoTimestampsForCroppedLog = new long[robotTimestampsForCroppedLog.length];
+      int i = 0;
+
+      // This stuff is used to print to SCS2 so the user knows how the cropped log is going, progress wise
+      long startFrame = getFrameAtTimestamp(startVideoTimestamp, magewellDemuxer); // This also moves the stream to the startFrame
+      long endFrame = getFrameAtTimestamp(endVideoTimestamp, magewellDemuxer);
+      long numberOfFrames = endFrame - startFrame;
+      int frameRate = (int) magewellDemuxer.getFrameRate();
+
+      magewellDemuxer.seekToPTS(startVideoTimestamp);
+
+      PrintWriter timestampWriter = new PrintWriter(timestampFile);
+      timestampWriter.println(1 + "\n" + frameRate);
+
+      long startTime = System.currentTimeMillis();
+
+      MagewellMuxer magewellMuxer = new MagewellMuxer(outputFile, magewellDemuxer.getImageWidth(), magewellDemuxer.getImageHeight());
+      magewellMuxer.start();
+
+      Frame frame;
+      while ((frame = magewellDemuxer.getNextFrame()) != null && magewellDemuxer.getFrameNumber() <= endFrame)
+      {
+         // We want to write all the frames at once to get equal timestamps between frames. When recording from the camera we have a fixed rate at which we
+         // receive frames, so we don't need to worry about it, here however, we don't have that so we cna grab the next frame as fast as possible. However if the
+         // timestamps between frames aren't large enough, things won't work. (maybe :))
+         long videoTimestamp = 1000 * (System.currentTimeMillis() - startTime);
+         magewellMuxer.recordFrame(frame, videoTimestamp);
+         videoTimestampsForCroppedLog[i] = magewellMuxer.getTimeStamp();
+         i++;
+
+         if (progressConsumer != null)
+         {
+            progressConsumer.info("frame %d/%d".formatted(magewellDemuxer.getFrameNumber() - startFrame, numberOfFrames));
+            progressConsumer.progress((double) (magewellDemuxer.getFrameNumber() - startFrame) / (double) numberOfFrames);
+         }
+      }
+
+      for (i = 0; i < videoTimestampsForCroppedLog.length; i++)
+      {
+         timestampWriter.print(robotTimestampsForCroppedLog[i]);
+         timestampWriter.print(" ");
+         timestampWriter.println(videoTimestampsForCroppedLog[i]);
+      }
+
+      magewellMuxer.close();
+      timestampWriter.close();
+   }
+
+   private static long getFrameAtTimestamp(long endCameraTimestamp, MagewellDemuxer magewellDemuxer)
+   {
+      magewellDemuxer.seekToPTS(endCameraTimestamp);
+      return magewellDemuxer.getFrameNumber();
+   }
+
+   public String getName()
+   {
+      return name;
+   }
+
+   public Camera getCamera()
+   {
+      return camera;
+   }
+
+   public FrameData pollCurrentFrame()
+   {
+      return frameData;
+   }
+
+   public int getCurrentIndex()
+   {
+      return timestampScrubber.getCurrentIndex();
+   }
+
+   public boolean replacedRobotTimestampsContainsIndex(int index)
+   {
+      return timestampScrubber.getReplacedRobotTimestampIndex(index);
+   }
+}