diff --git a/monologue/src/generate/java/DataLogger.java.jinja b/monologue/src/generate/java/DataLogger.java.jinja index f26b96c..b9f873c 100644 --- a/monologue/src/generate/java/DataLogger.java.jinja +++ b/monologue/src/generate/java/DataLogger.java.jinja @@ -4,12 +4,15 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.NTSendable; import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; import edu.wpi.first.util.datalog.*; +import monologue.NTIntegerArrayLogEntry; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.DataLogManager; 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(); @@ -34,8 +37,11 @@ class DataLogger extends GenericLogger { if (value == null) {return;} {%endif%} {%if t.TypeName == 'IntegerArray'%} - new {{t.java.EntryName}}LogEntry(log, entryName) + new NTIntegerArrayLogEntry(log, entryName) .append(toLongArray(value)); +{%elif t.TypeName == 'LongArray'%} + new NTIntegerArrayLogEntry(log, entryName) + .append(value); {%else%} new {{t.java.EntryName}}LogEntry(log, entryName).append(value); {%endif%} @@ -45,9 +51,12 @@ class DataLogger extends GenericLogger { public void add{{t.TypeName}}(String entryName, {{t.java.Supplier}}valueSupplier, LogLevel level) - { + { + {%if t.TypeName == 'IntegerArray' or t.TypeName == 'LongArray'%} + var entry = new NTIntegerArrayLogEntry(log, entryName); +{%else %} var entry = new {{t.java.EntryName}}LogEntry(log, entryName); - +{%endif%} LongConsumer consumer; {%if t.TypeName == 'IntegerArray'%} if (this.isLazy()) { @@ -60,7 +69,10 @@ class DataLogger extends GenericLogger { var value = toLongArray(valueSupplier.get()); if (!(Arrays.equals(value, lastValue))) { entry.append(value, timestamp); - lastValue = value; + if (lastValue.length != value.length) { + lastValue = new long[value.length]; + } + System.arraycopy(value, 0, lastValue, 0, value.length); } } }; @@ -92,7 +104,10 @@ class DataLogger extends GenericLogger { if (!(Arrays.equals(value, lastValue))) { {%endif%} entry.append(value, timestamp); - lastValue = value; + if (lastValue.length != value.length) { + lastValue = new {{t.java.ComponentType}}[value.length]; + } + System.arraycopy(value, 0, lastValue, 0, value.length); } } }; @@ -163,15 +178,19 @@ class DataLogger extends GenericLogger { NetworkTableInstance.getDefault().startEntryDataLog(DataLogManager.getLog(), path, path); } + @Override public void addStruct(String entryName, Struct struct, Supplier 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 R lastValue = null; + private ByteBuffer value1 = ByteBuffer.allocate(size); + private ByteBuffer value2 = ByteBuffer.allocate(size); + boolean useValue1 = true; @Override public void accept(long timestamp) { @@ -179,13 +198,21 @@ class DataLogger extends GenericLogger { if (value == null) { return; } - if (!(value.equals(lastValue))) { - entry.append(value, timestamp); - lastValue = value; + 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) { @@ -203,27 +230,50 @@ class DataLogger extends GenericLogger { @Override public void addStructArray(String entryName, Struct struct, Supplier 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/monologue/src/generate/java/NTLogger.java.jinja b/monologue/src/generate/java/NTLogger.java.jinja index 98f3378..575df80 100644 --- a/monologue/src/generate/java/NTLogger.java.jinja +++ b/monologue/src/generate/java/NTLogger.java.jinja @@ -88,29 +88,11 @@ class NTLogger extends GenericLogger { {%endif%} LongConsumer consumer; {%if t.TypeName == 'IntegerArray'%} - if (this.isLazy()) { - consumer = new LongConsumer() { - private long[] lastValue = new long[] {}; - @Override - public void accept(long timestamp) { - var intarr = valueSupplier.get(); - if (intarr == null) { return;} - var value = toLongArray(valueSupplier.get()); - if (!(Arrays.equals(value, lastValue))) { - entry.set(value, timestamp); - lastValue = value; - } - - - } - }; - } else { - consumer = (timestamp) -> { - var value = valueSupplier.get(); - if (value == null) { return;} - entry.set(toLongArray(value), timestamp); - }; - } + consumer = (timestamp) -> { + var value = valueSupplier.get(); + if (value == null) { return;} + entry.set(toLongArray(value), timestamp); + }; addField( entryName, @@ -124,54 +106,12 @@ class NTLogger extends GenericLogger { entry::unpublish ); {%else%} -{%if t.java.IsArray == true%} - if (this.isLazy()) { - consumer = new LongConsumer() { - private {{t.java.ValueType}} lastValue = new {{t.java.ValueType}} {}; - @Override - public void accept(long timestamp) { - var value = valueSupplier.get(); - if (value == null) { return;} - {%if t.TypeName == 'StringArray'%} - if (!(Arrays.deepEquals(value, lastValue))) { - {%else%} - if (!(Arrays.equals(value, lastValue))) { - {%endif%} - entry.set(value, timestamp); - lastValue = value; - } - } - }; - } else { - consumer = (timestamp) -> { - var value = valueSupplier.get(); - if (value != null) { - entry.set(value, timestamp); - } - }; - } -{%else%} - if (this.isLazy()) { - consumer = new LongConsumer() { - private {{t.java.ValueType}} lastValue = {{t.java.EmptyValue}}; - @Override - public void accept(long timestamp) { - - var value = valueSupplier.get(); - if (value == null) { return; } - if (value != lastValue) { - entry.set(value, timestamp); - lastValue = value; - } - } - }; - } else { - consumer = (timestamp) -> { - var value = valueSupplier.get(); - if (value == null) { return; } + consumer = (timestamp) -> { + var value = valueSupplier.get(); + if (value != null) { entry.set(value, timestamp); - }; - } + } + }; {%endif%} addField( @@ -195,28 +135,11 @@ class NTLogger extends GenericLogger { var publisher = topic.publish(); LongConsumer consumer; - - if (this.isLazy()) { - consumer = new LongConsumer() { - private R lastValue = null; - @Override - public void accept(long timestamp) { - - var value = valueSupplier.get(); - if (value == null) { return;} - if (!(value.equals(lastValue))) { - publisher.set(value, timestamp); - lastValue = value; - } - } - }; - } else { - consumer = (timestamp) -> { - var value = valueSupplier.get(); - if (value == null) { return;} - publisher.set(value, timestamp); - }; - } + consumer = (timestamp) -> { + var value = valueSupplier.get(); + if (value == null) { return;} + publisher.set(value, timestamp); + }; addField( entryName, @@ -238,26 +161,11 @@ class NTLogger extends GenericLogger { LongConsumer consumer; - if (this.isLazy()) { - consumer = new LongConsumer() { - private R[] lastValue = null; - @Override - public void accept(long timestamp) { - var value = (R[]) valueSupplier.get(); - if (value == null) {return;} - if (!(Arrays.deepEquals(value, lastValue))) { - publisher.set(value, timestamp); - lastValue = value; - } - } - }; - } else { - consumer = (timestamp) -> { - var value = valueSupplier.get(); - if (value == null) {return;} - publisher.set(value, timestamp); - }; - } + consumer = (timestamp) -> { + var value = valueSupplier.get(); + if (value == null) {return;} + publisher.set(value, timestamp); + }; addField( entryName, diff --git a/monologue/src/generate/types.json b/monologue/src/generate/types.json index 6155601..c706273 100644 --- a/monologue/src/generate/types.json +++ b/monologue/src/generate/types.json @@ -97,7 +97,8 @@ "FunctionTypeSuffix": "", "EntryName": "Raw", "SupplierGet" : "get", - "IsArray" : true + "IsArray" : true, + "ComponentType": "byte" } }, { @@ -111,7 +112,8 @@ "FunctionTypeSuffix": "", "EntryName": "BooleanArray", "SupplierGet" : "get", - "IsArray" : true + "IsArray" : true, + "ComponentType": "boolean" } }, { @@ -125,7 +127,8 @@ "FunctionTypeSuffix": "", "EntryName": "IntegerArray", "SupplierGet" : "get", - "IsArray" : true + "IsArray" : true, + "ComponentType": "int" } }, { @@ -139,7 +142,8 @@ "FunctionTypeSuffix": "", "EntryName": "IntegerArray", "SupplierGet" : "get", - "IsArray" : true + "IsArray" : true, + "ComponentType" : "long" } }, { @@ -153,7 +157,8 @@ "FunctionTypeSuffix": "", "EntryName": "FloatArray", "SupplierGet" : "get", - "IsArray" : true + "IsArray" : true, + "ComponentType": "float" } }, { @@ -167,7 +172,8 @@ "FunctionTypeSuffix": "", "EntryName": "DoubleArray", "SupplierGet" : "get", - "IsArray" : true + "IsArray" : true, + "ComponentType": "double" } }, { @@ -180,7 +186,8 @@ "FunctionTypeSuffix": "", "EntryName": "StringArray", "SupplierGet" : "get", - "IsArray" : true + "IsArray" : true, + "ComponentType": "String" } } ] diff --git a/monologue/src/main/java/monologue/NTIntegerArrayLogEntry.java b/monologue/src/main/java/monologue/NTIntegerArrayLogEntry.java new file mode 100644 index 0000000..b68d11c --- /dev/null +++ b/monologue/src/main/java/monologue/NTIntegerArrayLogEntry.java @@ -0,0 +1,81 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package monologue; + +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DataLogEntry; + +/** Log array of integer values. + * The datatype matches NT instead of DataLog spec + * to avoid type mismatches when NT-logging to this key. + * This gets around https://github.com/wpilibsuite/allwpilib/issues/6203 +*/ +public class NTIntegerArrayLogEntry extends DataLogEntry { + /** The data type for integer array values. */ + public static final String kDataType = "int[]"; + + /** + * Constructs a integer array log entry. + * + * @param log datalog + * @param name name of the entry + * @param metadata metadata + * @param timestamp entry creation timestamp (0=now) + */ + public NTIntegerArrayLogEntry(DataLog log, String name, String metadata, long timestamp) { + super(log, name, kDataType, metadata, timestamp); + } + + /** + * Constructs a integer array log entry. + * + * @param log datalog + * @param name name of the entry + * @param metadata metadata + */ + public NTIntegerArrayLogEntry(DataLog log, String name, String metadata) { + this(log, name, metadata, 0); + } + + /** + * Constructs a integer array log entry. + * + * @param log datalog + * @param name name of the entry + * @param timestamp entry creation timestamp (0=now) + */ + public NTIntegerArrayLogEntry(DataLog log, String name, long timestamp) { + this(log, name, "", timestamp); + } + + /** + * Constructs a integer array log entry. + * + * @param log datalog + * @param name name of the entry + */ + public NTIntegerArrayLogEntry(DataLog log, String name) { + this(log, name, 0); + } + + /** + * Appends a record to the log. + * + * @param value Value to record + * @param timestamp Time stamp (0 to indicate now) + */ + public void append(long[] value, long timestamp) { + m_log.appendIntegerArray(m_entry, value, timestamp); + } + + /** + * Appends a record to the log. + * + * @param value Value to record + */ + public void append(long[] value) { + m_log.appendIntegerArray(m_entry, value, 0); + } +} diff --git a/test-project/simgui.json b/test-project/simgui.json index 59d12e9..76bf9a2 100644 --- a/test-project/simgui.json +++ b/test-project/simgui.json @@ -47,12 +47,33 @@ "Translation2d##v_/Robot/structTest": { "open": true }, + "Translation2d##v_/Robot/structTestDebug": { + "open": true + }, + "double[]##v_/Robot/array": { + "open": true + }, "field": { "double[]##v_/Robot/field/Robot": { "open": true }, "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 index 7e187ec..0ebd865 100644 --- a/test-project/src/main/java/frc/robot/Robot.java +++ b/test-project/src/main/java/frc/robot/Robot.java @@ -13,6 +13,9 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.BooleanEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.IntegerArrayLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.DriverStation.MatchType; @@ -63,7 +66,8 @@ public class Robot extends TimedRobot implements Logged { @Log.NT private Field2d field = new Field2d(); @Log.NT private Mechanism2d mech = new Mechanism2d(1, 1); - @Log.NT private long[] array = {0, 1, 2}; + @Log.NT private int[] array = {0, 1, 2}; + @Log.NT private int number = 0; BooleanEntry fileOnlyEntry = NetworkTableInstance.getDefault().getBooleanTopic("/fileOnly").getEntry(false); @@ -75,6 +79,7 @@ public Robot() { @Override public void robotInit() { fileOnlyEntry.set(false); + System.out.println("before first update"); } @Override @@ -95,6 +100,7 @@ public void robotPeriodic() { (Math.random()+0.55) * translation2d.getX(), (Math.random()+0.55) * translation2d.getY() ); + //array[0] = samples; } @Override @@ -113,7 +119,7 @@ public void autonomousPeriodic() {} @Override public void teleopInit() { - array=null; + } @Override