Support ZED in SCS 2 log visualizer (#46)

* Add sensor name to ZED announce.

* Start setting up ZED dat file.

* Write timestamp .dat file.

* Remove timebase and add svo filename to dat

* Change to just the svo prefix to save disk space.

* Fix compile error.

* Fix dat file path errors. -Duncan

* Pack timestamp from ZED.

* Add cleanup code.

---------

Co-authored-by: ihmc-rosie <[email protected]>

Author: calvertdw

src/main/java/us/ihmc/robotDataLogger/example/ExampleSVOLogger.java

@@ -46,8 +46,10 @@ public static void main(String[] args)
       /*
          Connect and start logging the SVO
        */
-      String svoFile = System.getProperty("user.home") + "/Desktop/test" + UUID.randomUUID().toString().substring(0, 5) + ".svo2";
-      SVO_LOGGER.start(svoFile, ADDRESS, PORT);
+      String path = System.getProperty("user.home") + "/Desktop/test" + UUID.randomUUID().toString().substring(0, 5);
+      String svoFile = path + ".svo2";
+      String datFile = path + ".dat";
+      SVO_LOGGER.start(svoFile, datFile, System::nanoTime, ADDRESS, PORT);
 
       /*
          Do nothing forever, everything else runs in other threads

src/main/java/us/ihmc/robotDataLogger/logger/MagewellDemuxer.java

@@ -3,6 +3,7 @@
 import org.bytedeco.javacv.FFmpegFrameGrabber;
 import org.bytedeco.javacv.Frame;
 import org.bytedeco.javacv.FrameGrabber;
+import us.ihmc.log.LogTools;
 
 import java.io.File;
 
@@ -80,4 +81,16 @@ public double getFrameRate()
     {
         return grabber.getVideoFrameRate();
     }
+
+    public void stop()
+    {
+       try
+       {
+           grabber.stop();
+       }
+       catch (FFmpegFrameGrabber.Exception e)
+       {
+           LogTools.error(e.getMessage());
+       }
+    }
 }

src/main/java/us/ihmc/robotDataLogger/logger/YoVariableLogger.java

@@ -85,7 +85,7 @@ else if (!Files.isDirectory(logDirectory))
          throw e;
       }
 
-      zedSVOLoggerManager = new ZEDSVOLoggerManager(tempDirectory, finalDirectory);
+      zedSVOLoggerManager = new ZEDSVOLoggerManager(tempDirectory, finalDirectory, logger::getLastReceivedTimestamp);
    }
 
    public void destroy()

src/main/java/us/ihmc/robotDataLogger/logger/YoVariableLoggerListener.java

@@ -673,4 +673,9 @@ private static String toString(Announcement announcement)
 
       return builder.toString();
    }
+
+   public long getLastReceivedTimestamp()
+   {
+      return lastReceivedTimestamp;
+   }
 }

src/main/java/us/ihmc/robotDataLogger/logger/ZEDSVOLogger.java

@@ -1,13 +1,19 @@
 package us.ihmc.robotDataLogger.logger;
 
 import us.ihmc.commons.Conversions;
+import us.ihmc.commons.exception.DefaultExceptionHandler;
+import us.ihmc.commons.exception.ExceptionTools;
 import us.ihmc.commons.thread.RepeatingTaskThread;
 import us.ihmc.commons.thread.ThreadTools;
 import us.ihmc.log.LogTools;
 import us.ihmc.zed.SL_InitParameters;
 import us.ihmc.zed.SL_RuntimeParameters;
 import us.ihmc.zed.global.zed;
 
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.function.LongSupplier;
+
 import static us.ihmc.zed.global.zed.*;
 
 /**
@@ -29,16 +35,26 @@ public class ZEDSVOLogger
    private final RepeatingTaskThread grabThread = new RepeatingTaskThread(getClass().getName() + "GrabThread", this::grab);
    private final RepeatingTaskThread connectionWatchdogThread = new RepeatingTaskThread(getClass().getName() + "ConnectionWatchdog", this::connectionCheck);
 
+   private String svoPrefix;
+   private LongSupplier timestampSupplier;
+   private FileWriter timestampWriter;
+
    private volatile double lastGrabTime;
    private volatile boolean stopRequested;
    private volatile boolean completelyStopped;
    private volatile boolean failedBeyondRecovery;
 
-   public void start(String svoFile, String address, int port)
+   public void start(String svoFile, String datFile, LongSupplier timestampSupplier, String address, int port)
    {
+      this.timestampSupplier = timestampSupplier;
+
       if (stopRequested)
          throw new IllegalStateException("Cannot restart ZEDSVOLogger once stopped");
 
+      String[] parts = svoFile.split("[/\\\\]");
+      svoPrefix = parts[parts.length - 1].substring(0, "yyyyMMdd_HHmmss".length());
+      timestampWriter = ExceptionTools.handle(() -> new FileWriter(datFile, true), DefaultExceptionHandler.RUNTIME_EXCEPTION);
+
       initParameters = new SL_InitParameters();
       initParameters.input_type(zed.SL_INPUT_TYPE_STREAM);
       initParameters.async_grab_camera_recovery(true);
@@ -94,6 +110,8 @@ public void stop()
          // Can't use LogTools here, we might be shutting down...
          System.out.println("Closing ZED SDK stream");
 
+         ExceptionTools.handle(timestampWriter::close, DefaultExceptionHandler.PRINT_MESSAGE);
+
          completelyStopped = true;
       }
    }
@@ -107,6 +125,15 @@ public void grab()
 
       lastGrabTime = System.currentTimeMillis() / 1000D;
 
+      try
+      {
+         timestampWriter.write("%d %d %s%n".formatted(timestampSupplier.getAsLong(), sl_get_current_timestamp(cameraID), svoPrefix));
+      }
+      catch (IOException e)
+      {
+         LogTools.error(e.getMessage());
+      }
+
       if (returnCode == SL_ERROR_CODE_FAILURE || returnCode == SL_ERROR_CODE_CAMERA_NOT_DETECTED)
          failedBeyondRecovery = true;
 

src/main/java/us/ihmc/robotDataLogger/logger/ZEDSVOLoggerManager.java

@@ -13,6 +13,7 @@
 import java.util.Date;
 import java.util.Map;
 import java.util.concurrent.ConcurrentHashMap;
+import java.util.function.LongSupplier;
 
 /**
  * Manages n number of ZED SDK connections for logging SVO files.
@@ -30,13 +31,15 @@ private record ZEDSDKAnnounceHash(String address, int port)
    }
 
    private final File tempDirectory;
+   private final LongSupplier timestampSupplier;
 
    private final ROS2Node ros2Node;
    private final Map<ZEDSDKAnnounceHash, ZEDSVOLogger> zedLoggers = new ConcurrentHashMap<>();
 
-   public ZEDSVOLoggerManager(File tempDirectory, File finalDirectory)
+   public ZEDSVOLoggerManager(File tempDirectory, File finalDirectory, LongSupplier timestampSupplier)
    {
       this.tempDirectory = tempDirectory;
+      this.timestampSupplier = timestampSupplier;
 
       ros2Node = new ROS2NodeBuilder().build(ROS2TopicNameTools.toROSTopicFormat(finalDirectory.getName() + "_zed_svo_logger_node"));
 
@@ -63,11 +66,13 @@ private void onZEDSDKAnnounceMessage(ZEDSDKAnnounce message)
       {
          File perceptionDir = new File(tempDirectory, "perception");
          perceptionDir.mkdirs();
-         String svoFile = perceptionDir.getAbsolutePath() + File.separator + generateSVOFileName();
+         String svoFile = perceptionDir.getAbsolutePath() + File.separator + generateSVOFileName(message);
+         String datFile = perceptionDir.getAbsolutePath() + File.separator + 
+                 "%s%s".formatted(message.getSensorNameAsString(), VideoDataLoggerInterface.timestampDataPostfix);
 
          ZEDSVOLogger zedSVOLogger = new ZEDSVOLogger();
 
-         zedSVOLogger.start(svoFile, message.getAddressAsString(), message.getPort());
+         zedSVOLogger.start(svoFile, datFile, timestampSupplier, message.getAddressAsString(), message.getPort());
 
          zedLoggers.put(announceHash, zedSVOLogger);
       }
@@ -80,9 +85,9 @@ public void destroy()
       ros2Node.destroy();
    }
 
-   private static String generateSVOFileName()
+   private static String generateSVOFileName(ZEDSDKAnnounce message)
    {
       SimpleDateFormat dateFormat = new SimpleDateFormat("yyyyMMdd_HHmmss");
-      return dateFormat.format(new Date()) + "_" + "ZEDRecording.svo2";
+      return "%s_%s.svo2".formatted(dateFormat.format(new Date()), message.getSensorNameAsString());
    }
 }