Lazy Logging now works with structs and struct arrays

shueja · Jan 12, 2024 · 5f438b0 · 5f438b0
1 parent 975c676
commit 5f438b0
Show file tree

Hide file tree

Showing 3 changed files with 69 additions and 18 deletions.
diff --git a/monologue/src/generate/java/DataLogger.java.jinja b/monologue/src/generate/java/DataLogger.java.jinja
@@ -11,6 +11,8 @@ import java.util.Arrays;
 import java.util.function.Supplier;
 import java.util.function.LongConsumer;
 import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
 
 class DataLogger extends GenericLogger {
     private DataLog log = DataLogManager.getLog();
@@ -176,33 +178,41 @@ class DataLogger extends GenericLogger {
         NetworkTableInstance.getDefault().startEntryDataLog(DataLogManager.getLog(), path, path);
     }
 
+
     @Override
     public <R> void addStruct(String entryName, Struct<R> struct, Supplier<? extends R> valueSupplier, LogLevel level) {
-        var entry = StructLogEntry.create(log, entryName, struct);
-
         LongConsumer consumer;
 
         if (this.isLazy()) {
+            var entryHandle = log.start(entryName, struct.getTypeString(), "", 0);
+            log.addSchema(struct, 0);
+            int size = struct.getSize();
             consumer = new LongConsumer() {
-                private byte[] lastValue = new byte[] {};
+                private ByteBuffer value1 = ByteBuffer.allocate(size);
+                private ByteBuffer value2 = ByteBuffer.allocate(size);
+                boolean useValue1 = true; 
 
                 @Override
                 public void accept(long timestamp) {
                     var value = valueSupplier.get();
                     if (value == null) {
                         return;
                     }
-                    var packed = struct.pack(value);
-                    if (!(Arrays.equals(packed, lastValue))) {
-                        entry.append(value, timestamp);
-                        if (lastValue.length != value.length) {
-                            lastValue = new byte[value.length];
-                        }
-                        lastValue = packed;
+                    ByteBuffer cur = useValue1 ? value1 : value2;
+                    cur.position(0);
+                    struct.pack(cur, value);
+                    cur.position(0);
+                    // checks that the buffer segments written by the struct match.
+                    // ByteBuffer equality looks at the content after the position,
+                    // so both positions need to be 0 at this point.
+                    if (!(value1.equals(value2))) {
+                        log.appendRaw(entryHandle, cur, 0, size, timestamp);
+                        useValue1 = !useValue1;
                     }
                 }
             };
         } else {
+            var entry = StructLogEntry.create(log, entryName, struct);
             consumer = (timestamp) -> {
                 var value = valueSupplier.get();
                 if (value == null) {
@@ -220,27 +230,50 @@ class DataLogger extends GenericLogger {
 
     @Override
     public <R> void addStructArray(String entryName, Struct<R> struct, Supplier<R[]> valueSupplier, LogLevel level) {
-        var entry = StructArrayLogEntry.create(log, entryName, struct);
-
         LongConsumer consumer;
 
         if (this.isLazy()) {
+            var entryHandle = log.start(entryName, struct.getTypeString()+"[]", "", 0);
+            log.addSchema(struct, 0);
+            int size = struct.getSize();
             consumer = new LongConsumer() {
-                private R[] lastValue = null;
-
+                private ByteBuffer value1 = ByteBuffer.allocate(4 * size);
+                private ByteBuffer value2 = ByteBuffer.allocate(4 * size);
+                boolean useValue1 = true; 
+                int lastLength = 0;
                 @Override
                 public void accept(long timestamp) {
                     var value = valueSupplier.get();
                     if (value == null) {
                         return;
                     }
-                    if (!(Arrays.deepEquals(value, lastValue))) {
-                        entry.append(value, timestamp);
-                        lastValue = value;
+                    ByteBuffer cur = useValue1 ? value1 : value2;
+                    cur.position(0);
+                    if ((value.length * size) > cur.capacity()) {
+                      cur =
+                          ByteBuffer.allocateDirect(value.length * size * 2)
+                              .order(ByteOrder.LITTLE_ENDIAN);
+                    }
+                    for (R v : value) {
+                      struct.pack(cur, v);
+                    }
+                    cur.position(0);
+                    cur.limit(value.length * size);
+                    // checks that the buffer segments written by the struct match.
+                    // ByteBuffer equality looks at the content after the position and until the limit,
+                    // so both positions need to be 0 at this point.
+                    if (!(
+                        lastLength == value.length &&
+                        value1.equals(value2)
+                    )) {
+                        log.appendRaw(entryHandle, cur, 0, size, timestamp);
+                        useValue1 = !useValue1;
+                        lastLength = value.length;
                     }
                 }
             };
         } else {
+            var entry = StructArrayLogEntry.create(log, entryName, struct);
             consumer = (timestamp) -> {
                 var value = valueSupplier.get();
                 if (value == null) {

diff --git a/test-project/simgui.json b/test-project/simgui.json
@@ -47,6 +47,9 @@
         "Translation2d##v_/Robot/structTest": {
           "open": true
         },
+        "Translation2d##v_/Robot/structTestDebug": {
+          "open": true
+        },
         "double[]##v_/Robot/array": {
           "open": true
         },
@@ -56,6 +59,21 @@
           },
           "open": true
         },
+        "geometry": {
+          "Rotation2d##v_/Robot/geometry/getRotation2d": {
+            "open": true
+          },
+          "Rotation3d##v_/Robot/geometry/getRotation3d": {
+            "open": true
+          },
+          "Transform2d##v_/Robot/geometry/getTransform2d": {
+            "Translation2d##v_translation": {
+              "open": true
+            },
+            "open": true
+          },
+          "open": true
+        },
         "int[]##v_/Robot/array": {
           "open": true
         },

diff --git a/test-project/src/main/java/frc/robot/Robot.java b/test-project/src/main/java/frc/robot/Robot.java
@@ -73,7 +73,7 @@ public class Robot extends TimedRobot implements Logged {
 
   public Robot() {
     super();
-    Monologue.setupMonologue(this, "/Robot", true, false);
+    Monologue.setupMonologue(this, "/Robot", true, true);
   }
 
   @Override