diff --git a/build.gradle b/build.gradle index 7db24d5..3ec6c86 100644 --- a/build.gradle +++ b/build.gradle @@ -4,17 +4,13 @@ buildscript { mavenCentral() maven { url 'https://jitpack.io' } } - dependencies { - classpath "com.github.ionicbond-9738.gversion-plugin:build:master-SNAPSHOT" - } } - plugins { id "java" id "edu.wpi.first.GradleRIO" version "2025.3.2" id 'com.diffplug.spotless' version '7.2.1' - id 'com.peterabeles.gversion' version 'master-SNAPSHOT' + id "com.peterabeles.gversion" version "1.10" } apply plugin: 'com.peterabeles.gversion' @@ -136,8 +132,6 @@ dependencies { implementation 'com.jcraft:jsch:0.1.55' implementation 'com.diffplug.spotless:spotless-lib:3.3.1' - implementation 'com.github.ionicbond-9738.gversion-plugin:build:master-SNAPSHOT' - def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" diff --git a/settings.gradle b/settings.gradle index 4f49860..223f363 100644 --- a/settings.gradle +++ b/settings.gradle @@ -28,9 +28,6 @@ pluginManagement { resolutionStrategy { eachPlugin { details -> - if (details.requested.id.toString() == 'com.peterabeles.gversion') { - details.useModule("com.github.ionicbond-9738.gversion-plugin:build:master-SNAPSHOT") - } } } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index abdbb38..9960299 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -61,11 +61,8 @@ public Robot() { Logger.recordOutput("BuildConstants/version", BuildConstants.VERSION); Logger.recordOutput("BuildConstants/gitRevision", BuildConstants.GIT_REVISION); Logger.recordOutput("BuildConstants/gitBranch", BuildConstants.GIT_BRANCH); - Logger.recordOutput("BuildConstants/lastCommitAuthor", BuildConstants.LAST_COMMIT_AUTHOR); - Logger.recordOutput("BuildConstants/buildUser", BuildConstants.BUILD_USER); Logger.recordOutput("BuildConstants/buildDate", BuildConstants.BUILD_DATE); - Logger.recordOutput( - "BuildConstants/hasUncommittedChanges", BuildConstants.HAS_UNCOMMITTED_CHANGES); + Logger.recordOutput("BuildConstants/sha", BuildConstants.GIT_SHA); } public static Robot getInstance() { diff --git a/src/main/java/monologue/Annotations.java b/src/main/java/monologue/Annotations.java deleted file mode 100644 index 9605a4d..0000000 --- a/src/main/java/monologue/Annotations.java +++ /dev/null @@ -1,101 +0,0 @@ -package monologue; - -import java.lang.annotation.Annotation; -import java.lang.annotation.Documented; -import java.lang.annotation.ElementType; -import java.lang.annotation.Retention; -import java.lang.annotation.RetentionPolicy; -import java.lang.annotation.Target; - -@SuppressWarnings("unchecked") -public class Annotations { - static final Class[] ALL_ANNOTATIONS = - new Class[] {Log.class, Log.Once.class}; - - /** - * Logs the annotated field/method to NetworkTables if inside a {@link Logged} class. - * - *

Static fields and methods will emit a warning and not be logged. - * - * @param key [optional] the key to log the variable as. If empty, the key will be the name of the - * field/method - * @param sink [optional] the log sink to use - */ - @Documented - @Retention(RetentionPolicy.RUNTIME) - @Target({ElementType.FIELD, ElementType.METHOD}) - public @interface Log { - - /** The relative path to log to. If empty, the path will be the name of the field/method. */ - public String key() default ""; - - /** The {@link LogSink} to use. */ - public LogSink sink() default LogSink.NT; - - /** - * Logs the annotated field/method to NetworkTables if inside a {@link Logged} class. - * - * @param key [optional] the key to log the variable as. If empty, the key will be the name of - * the field/method - */ - @Documented - @Retention(RetentionPolicy.RUNTIME) - @Target({ElementType.FIELD, ElementType.METHOD}) - public @interface Once { - /** The relative path to log to. If empty, the path will be the name of the field/method. */ - public String key() default ""; - - /** The {@link LogSink} to use. */ - public LogSink sink() default LogSink.NT; - } - } - - /** - * Makes the annotated field containing a {@link Logged} class not be recursed into. - * - * @apiNote this will also make fields inside the object in the field not be logged - */ - @Documented - @Retention(RetentionPolicy.RUNTIME) - @Target({ElementType.FIELD}) - public @interface IgnoreLogged {} - - /** - * Allows singletons to be logged only once with a predefined key. - * - *

This also allows static variables to be logged under the singleton's key. - * - * @param key the key to log at, still appends the class name - */ - @Documented - @Retention(RetentionPolicy.RUNTIME) - @Target({ElementType.TYPE}) - public @interface SingletonLogged { - public String key(); - } - - /** - * Will cause the internal fields of the annotated field to be logged as if they were fields of - * the object this field is in. This is useful for flattening complex objects into a single path. - */ - @Documented - @Retention(RetentionPolicy.RUNTIME) - @Target({ElementType.FIELD}) - public @interface FlattenedLogged {} - - /** - * Will make Monologue aware that this field could contain an object that implements {@link - * Logged} but the type of the field itself does not implement {@link Logged}. - */ - @Documented - @Retention(RetentionPolicy.RUNTIME) - @Target({ElementType.FIELD}) - public @interface MaybeLoggedType {} - - @Documented - @Retention(RetentionPolicy.RUNTIME) - @Target({ElementType.FIELD}) - public @interface OptionalLogged { - public Class type(); - } -} diff --git a/src/main/java/monologue/Eval.java b/src/main/java/monologue/Eval.java deleted file mode 100644 index 4305517..0000000 --- a/src/main/java/monologue/Eval.java +++ /dev/null @@ -1,371 +0,0 @@ -package monologue; - -import java.lang.invoke.MethodHandle; -import java.lang.invoke.MethodHandles; -import java.lang.invoke.VarHandle; -import java.lang.reflect.AccessibleObject; -import java.lang.reflect.Field; -import java.lang.reflect.Method; -import java.lang.reflect.Modifier; -import java.util.ArrayList; -import java.util.Collections; -import java.util.List; -import java.util.Optional; -import monologue.Annotations.FlattenedLogged; -import monologue.Annotations.IgnoreLogged; -import monologue.Annotations.Log; -import monologue.Annotations.MaybeLoggedType; -import monologue.Annotations.OptionalLogged; -import monologue.Annotations.SingletonLogged; -import monologue.LoggingTree.*; -import monologue.Primatives.BooleanVarHandle; -import monologue.Primatives.DoubleVarHandle; -import monologue.Primatives.LongVarHandle; - -public class Eval { - /** - * Simplifies the user specified annotations on a field/method to a quanery conditiion expressed - * as an Optional {@link LogSink}. - * - * @param element The field/method to simplify - * @return The simplified condition - */ - static Optional annoEval(AccessibleObject element) { - if (element.isAnnotationPresent(Log.class)) { - return Optional.of(element.getAnnotation(Log.class).sink()); - } else if (element.isAnnotationPresent(Log.Once.class)) { - return Optional.of(element.getAnnotation(Log.Once.class).sink()); - } else { - return Optional.empty(); - } - } - - /** - * Checks if their are multiple logging annotations on one field/method. - * - * @param element The field/method to check - * @return If there are too many logging annotations - */ - static boolean overloadedAnno(AccessibleObject element) { - int count = 0; - for (var anno : Annotations.ALL_ANNOTATIONS) { - if (element.isAnnotationPresent(anno)) { - count++; - } - } - return count > 1; - } - - /** Checks if the class is a singleton and returns its key if it is. */ - static Optional singletonKey(Class clazz) { - if (clazz.isAnnotationPresent(SingletonLogged.class)) { - return Optional.of(clazz.getAnnotation(SingletonLogged.class).key()); - } else { - return Optional.empty(); - } - } - - /** A condensed packaged of what describes a singular logged field/method */ - static class LogMetadata { - public final boolean annotated; - public final LogSink sink; - public final boolean once; - public final String relativePath; - - private LogMetadata(boolean annotated, LogSink sink, boolean once, String path) { - this.annotated = annotated; - this.sink = sink; - this.once = once; - this.relativePath = "/" + path; - } - - /** - * Derives the metadata from an annotated field/method, if there are no logging annotations this - * returns null. - * - * @param element The field/method to derive the metadata from - * @return The metadata - */ - static LogMetadata from(AccessibleObject element) { - String name; - if (element instanceof java.lang.reflect.Field) { - name = ((java.lang.reflect.Field) element).getName(); - } else if (element instanceof java.lang.reflect.Method) { - name = ((java.lang.reflect.Method) element).getName(); - } else { - throw new IllegalArgumentException("Only fields and methods can be logged"); - } - if (element.isAnnotationPresent(Log.class)) { - Log anno = element.getAnnotation(Log.class); - return new LogMetadata(true, anno.sink(), false, anno.key().isEmpty() ? name : anno.key()); - } else if (element.isAnnotationPresent(Log.Once.class)) { - Log.Once anno = element.getAnnotation(Log.Once.class); - return new LogMetadata(true, anno.sink(), true, anno.key().isEmpty() ? name : anno.key()); - } else { - return new LogMetadata(false, LogSink.OP, false, name); - } - } - } - - static List> getLoggedInHierarchy(Class type, Class stop) { - ArrayList> result = new ArrayList>(); - - Class i = type; - while (i != stop && Logged.class.isAssignableFrom(i)) { - result.add(i); - i = i.getSuperclass(); - } - - return result; - } - - static List getAllFields(List> classes) { - ArrayList result = new ArrayList(); - for (Class clazz : classes) { - Collections.addAll(result, clazz.getDeclaredFields()); - } - return result; - } - - static List getAllMethods(List> classes) { - ArrayList result = new ArrayList(); - for (Class clazz : classes) { - Collections.addAll(result, clazz.getDeclaredMethods()); - } - return result; - } - - static List> getLoggedClasses(Class type) { - return getLoggedInHierarchy(type, Object.class); - } - - static boolean isNestedLogged(Field field) { - final boolean optional = Optional.class.isAssignableFrom(field.getType()); - final boolean optionalAnnotated = field.isAnnotationPresent(OptionalLogged.class); - final Class type = - optional && optionalAnnotated - ? field.getAnnotation(OptionalLogged.class).type() - : field.getType(); - final boolean fieldTyLogged = - Logged.class.isAssignableFrom(type) - || (type.isArray() && Logged.class.isAssignableFrom(type.getComponentType())); - final boolean maybeLoggedAnnotation = field.isAnnotationPresent(MaybeLoggedType.class); - final boolean ignoreLoggedAnnotation = field.isAnnotationPresent(IgnoreLogged.class); - if (fieldTyLogged && maybeLoggedAnnotation) { - RuntimeLog.warn( - field.getName() - + " of type " - + field.getType().getSimpleName() - + " is a Logged type, @MaybeLoggedType is redundant"); - } - return (fieldTyLogged || maybeLoggedAnnotation) && !ignoreLoggedAnnotation; - } - - static VarHandle getHandle(Field field, MethodHandles.Lookup lookup) { - try { - var privateLookup = MethodHandles.privateLookupIn(field.getDeclaringClass(), lookup); - return privateLookup.unreflectVarHandle(field); - } catch (IllegalAccessException e) { - RuntimeLog.warn( - "Could not access field " - + field.getName() - + " of type " - + field.getType().getSimpleName() - + " in " - + field.getDeclaringClass().getSimpleName() - + ": " - + e.getMessage()); - return null; - } - } - - static MethodHandle getHandle(Method method, MethodHandles.Lookup lookup) { - try { - var privateLookup = MethodHandles.privateLookupIn(method.getDeclaringClass(), lookup); - return privateLookup.unreflect(method); - } catch (IllegalAccessException e) { - RuntimeLog.warn( - "Could not access field " - + method.getName() - + " in " - + method.getDeclaringClass().getSimpleName() - + ": " - + e.getMessage()); - return null; - } - } - - static LN exploreNodes(List> types, final LN rootNode) { - final List fields = getAllFields(types); - final List methods = getAllMethods(types); - final MethodHandles.Lookup lookup = MethodHandles.lookup(); - final String rootPath = rootNode.getPath(); - - for (final Field field : fields) { - final boolean isNestedLogged = isNestedLogged(field); - final boolean isValidLiteralType = TypeChecker.isValidLiteralType(field.getType()); - final boolean isStatic = Modifier.isStatic(field.getModifiers()); - final boolean isArray = field.getType().isArray(); - final LogMetadata metadata = LogMetadata.from(field); - final boolean optional = Optional.class.isAssignableFrom(field.getType()); - final boolean optionalAnnotated = field.isAnnotationPresent(OptionalLogged.class); - if (!isNestedLogged && !isValidLiteralType) { - continue; - } - if ((optional && !isNestedLogged) || (optional && !optionalAnnotated)) { - continue; - } - final VarHandle handle = getHandle(field, lookup); - if (handle == null) { - continue; - } - - // handle singletons - if (isNestedLogged && isStatic) { - Optional singletonKey = singletonKey(field.getType()); - if (singletonKey.isPresent() && !Logged.singletonAlreadyAdded(field.getType())) { - try { - Monologue.logTree((Logged) field.get(null), singletonKey.get()); - Logged.addSingleton( - field.getType(), new SingletonNode(singletonKey.get(), field.getType(), handle)); - } catch (IllegalAccessException e) { - RuntimeLog.warn("Issue with singleton " + field.getType().getSimpleName()); - } - } - continue; - } else if (metadata.annotated && isStatic) { - RuntimeLog.warn( - "Static field " - + field.getName() - + " of type " - + field.getType().getSimpleName() - + " in " - + rootPath - + " is will not be logged"); - continue; - } - - final Class type; - if (optional && optionalAnnotated) { - type = field.getAnnotation(OptionalLogged.class).type(); - } else { - type = field.getType(); - } - - if (isArray && isNestedLogged) { - ComposableNode node = new ObjectArrayNode(rootPath + metadata.relativePath, handle::get); - rootNode.addChild(exploreNodes(getLoggedClasses(type.getComponentType()), node)); - } else if (isNestedLogged) { - boolean isFlattened = field.isAnnotationPresent(FlattenedLogged.class); - String relativePath = isFlattened ? "" : metadata.relativePath; - final ObjectNode node; - if (optional) { - node = new OptionalNode(rootPath + relativePath, metadata.sink, handle::get, type); - } else { - node = new ObjectNode(rootPath + relativePath, handle::get, type); - } - rootNode.addChild(exploreNodes(getLoggedClasses(type), node)); - } else if (isValidLiteralType) { - if (!metadata.annotated || overloadedAnno(field)) { - continue; - } - final boolean isFinal = Modifier.isFinal(field.getModifiers()); - final boolean isPrimitive = type.isPrimitive(); - LoggingNode node; - if (type.isArray()) { - node = - new ValueArrayNode( - rootPath + metadata.relativePath, - metadata.sink, - obj -> (Object[]) handle.get(obj), - type); - } else if (type == double.class - || type == Double.TYPE - || type == float.class - || type == Float.TYPE) { - node = - new DoubleValueNode( - rootPath + metadata.relativePath, - metadata.sink, - new DoubleVarHandle(handle)::get); - } else if ((type == long.class - || type == Long.TYPE - || type == int.class - || type == Integer.TYPE)) { - node = - new LongValueNode( - rootPath + metadata.relativePath, metadata.sink, new LongVarHandle(handle)::get); - } else if (type == boolean.class || type == Boolean.TYPE) { - node = - new BooleanValueNode( - rootPath + metadata.relativePath, - metadata.sink, - new BooleanVarHandle(handle)::get); - } else { - node = - new ValueNode( - rootPath + metadata.relativePath, metadata.sink, handle::get, field.getType()); - } - if (isFinal && isPrimitive) { - node = node.asImmutable(); - } else if (!isPrimitive) { - node = node.asNullable(); - } - rootNode.addChild(node); - } - } - - for (final Method method : methods) { - LogMetadata metadata = LogMetadata.from(method); - if (!metadata.annotated - || method.getParameterCount() > 0 - || !TypeChecker.isValidLiteralType(method.getReturnType())) { - continue; - } - MethodHandle handle = getHandle(method, lookup); - if (handle == null) { - continue; - } - - final String err = "Could not invoke method " + method.getName() + " in " + rootPath + ": "; - - boolean isPrimitive = method.getReturnType().isPrimitive(); - LoggingNode node; - if (method.getReturnType().isArray()) { - node = - new ValueArrayNode( - metadata.relativePath, - metadata.sink, - obj -> { - try { - return (Object[]) handle.invoke(obj); - } catch (Throwable e) { - RuntimeLog.warn(err + e.getMessage()); - return null; - } - }, - method.getReturnType()); - } else { - node = - new ValueNode( - rootPath + metadata.relativePath, - metadata.sink, - obj -> { - try { - return handle.invoke(obj); - } catch (Throwable e) { - RuntimeLog.warn(err + e.getMessage()); - return null; - } - }, - method.getReturnType()); - } - if (!isPrimitive) { - node = node.asNullable(); - } - rootNode.addChild(node); - } - - return rootNode; - } -} diff --git a/src/main/java/monologue/GlobalField.java b/src/main/java/monologue/GlobalField.java deleted file mode 100644 index c3c0875..0000000 --- a/src/main/java/monologue/GlobalField.java +++ /dev/null @@ -1,30 +0,0 @@ -package monologue; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import java.util.List; -import java.util.concurrent.atomic.AtomicBoolean; - -public class GlobalField { - private static final AtomicBoolean initialized = new AtomicBoolean(false); - private static final Field2d field = new Field2d(); - - static void publish() { - if (initialized.getAndSet(true)) { - return; - } - Monologue.publishSendable("/Field", field, LogSink.NT); - } - - public static synchronized void setObject(String name, Pose2d pose) { - field.getObject(name).setPose(pose); - } - - public static synchronized void setObject(String name, Pose2d... pose) { - field.getObject(name).setPoses(pose); - } - - public static synchronized void setObject(String name, List pose) { - field.getObject(name).setPoses(pose); - } -} diff --git a/src/main/java/monologue/GlobalLogged.java b/src/main/java/monologue/GlobalLogged.java deleted file mode 100644 index 3394893..0000000 --- a/src/main/java/monologue/GlobalLogged.java +++ /dev/null @@ -1,583 +0,0 @@ -package monologue; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.struct.Struct; -import edu.wpi.first.util.struct.StructSerializable; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import monologue.MonoSendableLayer.NtSendableCompat; - -/** - * The GlobalLogged class is a utility class that provides a simple way to use Monologue's logging - * tooling from any part of your robot code. It provides a set of log methods that allow you to log - * data to the NetworkTables and DataLog. - * - * @see Monologue - * @see LogSink - */ -class GlobalLogged { - static String ROOT_PATH = ""; - - static void setRootPath(String rootPath) { - ROOT_PATH = NetworkTable.normalizeKey(rootPath, true); - } - - /** - * Logs a boolean using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static boolean log(String entryName, boolean value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a boolean using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static boolean log(String entryName, boolean value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.createBoolean(entryName, sink).logBoolean(value); - - return value; - } - - /** - * Logs a int using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static int log(String entryName, int value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a int using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static int log(String entryName, int value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.createLong(entryName, sink).logLong(value); - - return value; - } - - /** - * Logs a long using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static long log(String entryName, long value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a long using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static long log(String entryName, long value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.createLong(entryName, sink).logLong(value); - - return value; - } - - /** - * Logs a float using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static float log(String entryName, float value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a float using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static float log(String entryName, float value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.createDouble(entryName, sink).logDouble(value); - - return value; - } - - /** - * Logs a double using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static double log(String entryName, double value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a double using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static double log(String entryName, double value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.createDouble(entryName, sink).logDouble(value); - - return value; - } - - /** - * Logs a String using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static String log(String entryName, String value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a String using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static String log(String entryName, String value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.create(entryName, String.class, sink).log(value); - - return value; - } - - /** - * Logs a byte[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static byte[] log(String entryName, byte[] value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a byte[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static byte[] log(String entryName, byte[] value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.create(entryName, byte[].class, sink).log(value); - - return value; - } - - /** - * Logs a boolean[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static boolean[] log(String entryName, boolean[] value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a boolean[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static boolean[] log(String entryName, boolean[] value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.create(entryName, boolean[].class, sink).log(value); - - return value; - } - - /** - * Logs a int[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static int[] log(String entryName, int[] value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a int[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static int[] log(String entryName, int[] value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.create(entryName, int[].class, sink).log(value); - - return value; - } - - /** - * Logs a long[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static long[] log(String entryName, long[] value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a long[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static long[] log(String entryName, long[] value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.create(entryName, long[].class, sink).log(value); - - return value; - } - - /** - * Logs a float[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static float[] log(String entryName, float[] value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a float[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static float[] log(String entryName, float[] value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.create(entryName, float[].class, sink).log(value); - - return value; - } - - /** - * Logs a double[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static double[] log(String entryName, double[] value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a double[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static double[] log(String entryName, double[] value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.create(entryName, double[].class, sink).log(value); - - return value; - } - - /** - * Logs a String[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static String[] log(String entryName, String[] value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a String[] using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static String[] log(String entryName, String[] value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.create(entryName, String[].class, sink).log(value); - - return value; - } - - /** - * Logs a Serializable Struct using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static R log(String entryName, R value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs a Serializable Struct using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - @SuppressWarnings("unchecked") - public static R log(String entryName, R value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - Class clazz = (Class) value.getClass(); - Struct struct = - ProceduralStructGenerator.extractClassStruct(clazz) - .orElseThrow( - () -> - new IllegalArgumentException( - "Class " + clazz.getName() + " does not have a struct.")); - MonoEntryLayer.MonologueEntry.create(entryName, struct, sink).log(value); - - return value; - } - - /** - * Logs an array of Serializable Structs using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static R[] log(String entryName, R[] value) { - return log(entryName, value, LogSink.NT); - } - - /** - * Logs an array of Serializable Structs using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - * @param sink The log sink to use. - */ - @SuppressWarnings("unchecked") - public static R[] log(String entryName, R[] value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, value, sink)); - return value; - } - Class clazz = (Class) value.getClass(); - Struct struct = - (Struct) - ProceduralStructGenerator.extractClassStructDynamic(clazz.getComponentType()).get(); - MonoEntryLayer.MonologueEntry.create(entryName, struct, clazz, sink).log(value); - - return value; - } - - /** - * Logs a Serializable Struct using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param struct The struct type to log. - * @param value The value to log. - */ - public static R log(String entryName, Struct struct, R value) { - return log(entryName, struct, value, LogSink.NT); - } - - /** - * Logs a Serializable Struct using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param struct The struct type to log. - * @param value The value to log. - * @param sink The log sink to use. - */ - public static R log(String entryName, Struct struct, R value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, struct, value, sink)); - return value; - } - MonoEntryLayer.MonologueEntry.create(entryName, struct, sink).log(value); - - return value; - } - - /** - * Logs an array of Serializable Structs using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param struct The struct type to log. - * @param value The value to log. - */ - public static R[] log(String entryName, Struct struct, R[] value) { - return log(entryName, struct, value, LogSink.NT); - } - - /** - * Logs an array of Serializable Structs using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param struct The struct type to log. - * @param value The value to log. - * @param sink The log sink to use. - */ - @SuppressWarnings("unchecked") - public static R[] log(String entryName, Struct struct, R[] value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.log(entryNameFinal, struct, value, sink)); - return value; - } - Class clazz = (Class) value.getClass(); - MonoEntryLayer.MonologueEntry.create(entryName, struct, clazz, sink).log(value); - - return value; - } - - static R[] logStructArray(String entryName, Struct struct, R[] value, LogSink sink) { - return log(entryName, struct, value, sink); - } - - /** - * Logs a Sendable using the Monologue machinery. - * - *

Monologue only supports data going 1 way, from the robot to the driver station. This means - * that Sendables that are updated by the driver station will not be updated on the robot. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static void publishSendable(String entryName, Sendable value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.publishSendable(entryNameFinal, value, sink)); - return; - } - entryName = NetworkTable.normalizeKey(entryName, true); - var builder = new MonoSendableLayer.Builder(entryName, sink); - value.initSendable(builder); - builder.finish(); - } - - /** - * Logs a Sendable using the Monologue machinery. - * - *

This method is used to log a Sendable that is a Field2d. Field2d is an {@code NTSendable} - * which require specialized code to log to Datalog. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static void publishSendable(String entryName, Field2d value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.publishSendable(entryNameFinal, value, sink)); - return; - } - entryName = NetworkTable.normalizeKey(entryName, true); - NtSendableCompat.addField2d(entryName, value, sink); - } - - /** - * Logs a Sendable using the Monologue machinery. - * - *

This method is used to log a Sendable that is a Mechanism2d. Mechanism2d is an {@code - * NTSendable} which require specialized code to log to Datalog. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static void publishSendable(String entryName, Mechanism2d value, LogSink sink) { - if (!Monologue.hasBeenSetup() || Monologue.isMonologueDisabled()) { - String entryNameFinal = entryName; - Monologue.prematureLog(() -> GlobalLogged.publishSendable(entryNameFinal, value, sink)); - return; - } - entryName = NetworkTable.normalizeKey(entryName, true); - NtSendableCompat.addMechanism2d(entryName, value, sink); - } -} diff --git a/src/main/java/monologue/LogSink.java b/src/main/java/monologue/LogSink.java deleted file mode 100644 index 12090da..0000000 --- a/src/main/java/monologue/LogSink.java +++ /dev/null @@ -1,37 +0,0 @@ -package monologue; - -public enum LogSink { - /** Logs will be sent to NetworkTables and then mirrored to DataLog */ - NT, - /** Logs will be sent to DataLog only */ - DL, - /** - * Will behave like {@link #DL} if {@link Monologue#isBandwidthOptimizationEnabled()} is true, - * otherwise will behave like {@link #NT} - */ - OP; - - /** - * Whether or not to log under the current library/logger flags - * - * @param fileOnly If the library is in fileOnly mode - * @param nt If the logger asking is the nt logger - * @return - */ - boolean shouldLog(boolean fileOnly, boolean nt) { - switch (this) { - case NT: - return !nt; - case DL: - return !nt; - case OP: - if (fileOnly) { - DL.shouldLog(fileOnly, nt); - } else { - NT.shouldLog(fileOnly, nt); - } - default: - return false; - } - } -} diff --git a/src/main/java/monologue/Logged.java b/src/main/java/monologue/Logged.java deleted file mode 100644 index 7132fcd..0000000 --- a/src/main/java/monologue/Logged.java +++ /dev/null @@ -1,617 +0,0 @@ -package monologue; - -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.struct.Struct; -import edu.wpi.first.util.struct.StructSerializable; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.WeakHashMap; -import monologue.LoggingTree.LoggingNode; - -/** - * Interface for classes that can hold {@link Monologue} annotated fields for {@link - * Monologue#setupMonologue} and {@link Monologue#logObj} to log. - * - *

This class also allows for an imperative way to log values with the {@link #log} methods. - * - * @see Monologue - * @see Annotations.Log - * @see Annotations.Log.Once - */ -public interface Logged { - - static final WeakHashMap> registry = new WeakHashMap<>(); - static final HashMap, LoggingNode> singletons = new HashMap<>(); - - static void addNode(Object logged, LoggingNode node) { - var lst = getNodes(logged); - if (!lst.contains(node)) { - lst.add(node); - } - } - - static void addSingleton(Class logged, LoggingNode node) { - singletons.put(logged, node); - } - - static boolean singletonAlreadyAdded(Class logged) { - return singletons.containsKey(logged); - } - - static List getNodes(Object logged) { - registry.putIfAbsent(logged, new ArrayList<>()); - return registry.get(logged); - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default boolean log(String key, boolean value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default boolean log(String key, boolean value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default int log(String key, int value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default int log(String key, int value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default long log(String key, long value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default long log(String key, long value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default float log(String key, float value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default float log(String key, float value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default double log(String key, double value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default double log(String key, double value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default String log(String key, String value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default String log(String key, String value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default byte[] log(String key, byte[] value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default byte[] log(String key, byte[] value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default boolean[] log(String key, boolean[] value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default boolean[] log(String key, boolean[] value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default int[] log(String key, int[] value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default int[] log(String key, int[] value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default long[] log(String key, long[] value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default long[] log(String key, long[] value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default float[] log(String key, float[] value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default float[] log(String key, float[] value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default double[] log(String key, double[] value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default double[] log(String key, double[] value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default String[] log(String key, String[] value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default String[] log(String key, String[] value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default R log(String key, R value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default R log(String key, R value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default R[] log(String key, R[] value) { - return log(key, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default R[] log(String key, R[] value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default R log(String key, Struct struct, R value) { - return log(key, struct, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default R log(String key, Struct struct, R value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, struct, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, struct, value, sink); - } - return value; - } - - /** - * Logs a value with the default log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - */ - public default R[] log(String key, Struct struct, R[] value) { - return log(key, struct, value, LogSink.NT); - } - - /** - * Logs a value with the specified log sink. The key is relative to the objects path this is being - * called in. - * - * @param key The key to log the value under relative to the objects path. - * @param value The value to log. - * @param sink The log sink to log the value under. - */ - public default R[] log(String key, Struct struct, R[] value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> log(key, struct, value, sink)); - return value; - } - String slashkey = "/" + key; - for (LoggingNode node : getNodes(this)) { - Monologue.log(node.getPath() + slashkey, struct, value, sink); - } - return value; - } - - /** - * Logs a Sendable using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static void publishSendable(String entryName, Sendable value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> publishSendable(entryName, value, sink)); - return; - } - Monologue.publishSendable(entryName, value, sink); - } - - /** - * Logs a Sendable using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static void publishSendable(String entryName, Field2d value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> publishSendable(entryName, value, sink)); - return; - } - Monologue.publishSendable(entryName, value, sink); - } - - /** - * Logs a Sendable using the Monologue machinery. - * - * @param entryName The name of the entry to log, this is an absolute path. - * @param value The value to log. - */ - public static void publishSendable(String entryName, Mechanism2d value, LogSink sink) { - if (!Monologue.hasBeenSetup()) { - Monologue.prematureLog(() -> publishSendable(entryName, value, sink)); - return; - } - Monologue.publishSendable(entryName, value, sink); - } -} diff --git a/src/main/java/monologue/LoggingTree.java b/src/main/java/monologue/LoggingTree.java deleted file mode 100644 index 3b8cb88..0000000 --- a/src/main/java/monologue/LoggingTree.java +++ /dev/null @@ -1,565 +0,0 @@ -package monologue; - -import edu.wpi.first.util.struct.Struct; -import edu.wpi.first.util.struct.StructSerializable; -import java.lang.invoke.VarHandle; -import java.util.ArrayList; -import java.util.HashSet; -import java.util.List; -import java.util.Optional; -import java.util.function.Function; -import monologue.MonoEntryLayer.MonologueBooleanEntry; -import monologue.MonoEntryLayer.MonologueDoubleEntry; -import monologue.MonoEntryLayer.MonologueEntry; -import monologue.MonoEntryLayer.MonologueLongEntry; -import monologue.Primatives.BooleanGetter; -import monologue.Primatives.DoubleGetter; -import monologue.Primatives.LongGetter; - -public class LoggingTree { - private static class MonologueTriedToLogNull extends NullPointerException { - private static final long serialVersionUID = 1L; - } - - private static T throwIfNull(T obj) { - if (obj == null) { - throw new MonologueTriedToLogNull(); - } - return obj; - } - - public abstract static class LoggingNode { - private final String path; - - public LoggingNode(String path) { - this.path = path; - } - - public String getPath() { - return path; - } - - public boolean isImmutable() { - return false; - } - - public LoggingNode asImmutable() { - return new ImmutableNode(this); - } - - public LoggingNode asNullable() { - return new NullableNode(this); - } - - public LoggingNode asTypeGuarded(Class type) { - return new TypeGuardedNode(this, type); - } - - public abstract LoggingNode withNewPath(String path); - - public abstract void log(Object obj); - } - - public abstract static class ComposableNode extends LoggingNode { - protected final ArrayList children = new ArrayList<>(); - - public ComposableNode(String path) { - super(path); - } - - public void addChild(LoggingNode child) { - for (LoggingNode node : children) { - if (node.path.equals(child.path)) { - RuntimeLog.warn("Duplicate path: " + child.path); - return; - } - } - children.add(child); - } - - public void addAllChildren(List children) { - for (LoggingNode child : children) { - addChild(child); - } - } - } - - public static class NoopLoggingNode extends ComposableNode { - public NoopLoggingNode(String path) { - super(path); - } - - @Override - public void addChild(LoggingNode child) {} - - @Override - public LoggingNode asImmutable() { - return this; - } - - @Override - public LoggingNode asNullable() { - return this; - } - - @Override - public LoggingNode withNewPath(String path) { - return new NoopLoggingNode(path); - } - - @Override - public void log(Object obj) {} - } - - public static class ValueNode extends LoggingNode { - private final Function getter; - private final MonologueEntry entry; - - private final Class type; - private final Optional> struct; - - @SuppressWarnings("unchecked") - public ValueNode( - String path, LogSink sink, Function getter, Class type) { - super(path); - this.getter = getter; - this.type = type; - if (StructSerializable.class.isAssignableFrom(type)) { - this.struct = - ProceduralStructGenerator.extractClassStructDynamic(type).map(Struct.class::cast); - this.entry = - MonologueEntry.create( - path, - struct.orElseGet( - () -> { - RuntimeLog.warn("No struct for " + type); - return ProceduralStructGenerator.noopStruct(Object.class); - }), - sink); - } else { - this.struct = Optional.empty(); - this.entry = MonologueEntry.create(path, (Class) type, sink); - } - } - - public ValueNode( - String path, - LogSink sink, - Function getter, - Class type, - Struct struct) { - super(path); - this.getter = getter; - this.type = type; - this.struct = Optional.of(struct); - this.entry = MonologueEntry.create(path, struct, sink); - } - - @Override - public LoggingNode withNewPath(String path) { - if (struct.isPresent()) { - return new ValueNode(path, entry.sink(), getter, type, struct.get()); - } else { - return new ValueNode(path, entry.sink(), getter, type); - } - } - - @Override - public void log(Object obj) { - entry.log(throwIfNull(getter.apply(obj))); - } - } - - public static class DoubleValueNode extends LoggingNode { - private final DoubleGetter getter; - private final MonologueDoubleEntry entry; - - public DoubleValueNode(String path, LogSink sink, DoubleGetter getter) { - super(path); - this.getter = getter; - this.entry = MonologueEntry.createDouble(path, sink); - } - - @Override - public LoggingNode withNewPath(String path) { - return new DoubleValueNode(path, entry.sink(), getter); - } - - @Override - public void log(Object obj) { - entry.logDouble(getter.get(obj)); - } - } - - public static class LongValueNode extends LoggingNode { - private final LongGetter getter; - private final MonologueLongEntry entry; - - public LongValueNode(String path, LogSink sink, LongGetter getter) { - super(path); - this.getter = getter; - this.entry = MonologueEntry.createLong(path, sink); - } - - @Override - public LoggingNode withNewPath(String path) { - return new LongValueNode(path, entry.sink(), getter); - } - - @Override - public void log(Object obj) { - entry.logLong(getter.get(obj)); - } - } - - public static class BooleanValueNode extends LoggingNode { - private final BooleanGetter getter; - private final MonologueBooleanEntry entry; - - public BooleanValueNode(String path, LogSink sink, BooleanGetter getter) { - super(path); - this.getter = getter; - this.entry = MonologueEntry.createBoolean(path, sink); - } - - @Override - public LoggingNode withNewPath(String path) { - return new BooleanValueNode(path, entry.sink(), getter); - } - - @Override - public void log(Object obj) { - entry.logBoolean(getter.get(obj)); - } - } - - @SuppressWarnings("unchecked") - public static class ValueArrayNode extends LoggingNode { - private final Function getter; - private final MonologueEntry entry; - - private final Class type; - private final Optional> struct; - - public ValueArrayNode( - String path, - LogSink sink, - Function getter, - Class type) { - super(path); - this.getter = getter; - this.type = type; - this.struct = Optional.empty(); - this.entry = MonologueEntry.create(path, (Class) type, sink); - } - - public ValueArrayNode( - String path, - LogSink sink, - Function getter, - Class type, - Struct struct) { - super(path); - this.getter = getter; - this.type = type; - this.struct = Optional.of(struct); - this.entry = MonologueEntry.create(path, struct, (Class) type, sink); - } - - @Override - public LoggingNode withNewPath(String path) { - if (struct.isPresent()) { - return new ValueArrayNode(path, entry.sink(), getter, type, struct.get()); - } else { - return new ValueArrayNode(path, entry.sink(), getter, type); - } - } - - public void log(Object obj) { - entry.log(throwIfNull(getter.apply(obj))); - } - } - - public static class ImmutableNode extends LoggingNode { - private final LoggingNode node; - private boolean logged = false; - - public ImmutableNode(LoggingNode node) { - super(node.path); - this.node = node; - } - - @Override - public boolean isImmutable() { - return true; - } - - @Override - public LoggingNode withNewPath(String path) { - return new ImmutableNode(node.withNewPath(path)); - } - - public void log(Object obj) { - if (!logged) { - node.log(obj); - logged = true; - } - } - } - - public static class NullableNode extends LoggingNode { - private final LoggingNode node; - private final String err; - - public NullableNode(LoggingNode node) { - super(node.path); - this.node = node; - this.err = node.path + " is null"; - } - - @Override - public LoggingNode withNewPath(String path) { - return new NullableNode(node.withNewPath(path)); - } - - public void log(Object obj) { - try { - node.log(obj); - } catch (MonologueTriedToLogNull e) { - RuntimeLog.warn(err); - } - } - } - - public static class TypeGuardedNode extends LoggingNode { - private final LoggingNode node; - private final Class type; - - public TypeGuardedNode(LoggingNode node, Class type) { - super(node.path); - this.node = node; - this.type = type; - } - - @Override - public LoggingNode withNewPath(String path) { - return new TypeGuardedNode(node.withNewPath(path), type); - } - - public void log(Object obj) { - if (!type.isInstance(obj)) { - return; - } - node.log(obj); - } - } - - public static class ObjectNode extends ComposableNode { - protected final Function getter; - private final String err; - private final HashSet> seenTypes = new HashSet<>(); - - public ObjectNode(String path, Function getter, Class type) { - super(path); - this.getter = getter; - this.err = path + " is null"; - seenTypes.add(type); - } - - private ObjectNode(String path, Function getter, HashSet> seenTypes) { - super(path); - this.getter = getter; - this.err = path + " is null"; - this.seenTypes.addAll(seenTypes); - } - - @Override - public LoggingNode withNewPath(String path) { - var n = new ObjectNode(path, getter, seenTypes); - for (LoggingNode child : children) { - n.addChild(child.withNewPath(path + child.getPath().substring(getPath().length()))); - } - return n; - } - - protected void updateNodeRegistry(Object o) { - if (o instanceof Logged && !Logged.getNodes((Logged) o).contains(this)) { - Logged.addNode((Logged) o, this); - } - } - - public void log(Object obj) { - Object o = getter.apply(obj); - if (o == null) { - RuntimeLog.warn(err); - return; - } - if (!seenTypes.contains(o.getClass())) { - // explore the new class - } - updateNodeRegistry(o); - for (LoggingNode child : children) { - child.log(o); - } - } - - public void logDirect(Object obj) { - updateNodeRegistry(obj); - for (LoggingNode child : children) { - child.log(obj); - } - } - } - - public static class ObjectArrayNode extends ComposableNode { - private final Function getter; - private final String err; - private final ArrayList indexNodes = new ArrayList<>(); - - public ObjectArrayNode(String path, Function getter) { - super(path); - this.getter = getter; - this.err = path + " is null"; - indexNodes.add(getOrCreateIndexNode(0)); - } - - private ObjectNode getOrCreateIndexNode(int index) { - int highestIndex = indexNodes.size() - 1; - if (index > highestIndex) { - for (int i = highestIndex + 1; i <= index; i++) { - var node = new ObjectNode(getPath() + "/" + i, _v -> _v, Object.class); - node.addAllChildren( - children.stream() - .map(child -> child.withNewPath(node.getPath() + child.getPath())) - .toList()); - indexNodes.add(node); - } - } - return indexNodes.get(index); - } - - @Override - public void addChild(LoggingNode child) { - child = child.withNewPath(child.getPath().substring(getPath().length())); - super.addChild(child); - for (int i = 0; i < indexNodes.size(); i++) { - indexNodes.get(i).addChild(child.withNewPath(getPath() + "/" + i + child.getPath())); - } - } - - @Override - public LoggingNode withNewPath(String path) { - var n = new ObjectArrayNode(path, getter); - for (LoggingNode child : children) { - n.addChild(child.withNewPath(path + child.getPath().substring(getPath().length()))); - } - // preload the index nodes - n.getOrCreateIndexNode(indexNodes.size() - 1); - return n; - } - - public void log(Object obj) { - Object[] oa = (Object[]) getter.apply(obj); - if (oa == null) { - RuntimeLog.warn(err); - return; - } - for (int i = 0; i < oa.length; i++) { - getOrCreateIndexNode(i).log(oa[i]); - } - } - } - - public static class OptionalNode extends ObjectNode { - private final LoggingNode isPresentNode; - private final String err; - - @SuppressWarnings("unchecked") - public OptionalNode(String path, LogSink sink, Function getter, Class type) { - super(path, getter, type); - isPresentNode = - new BooleanValueNode( - path + "/isPresent", sink, _v -> ((Optional) _v).isPresent()); - this.err = path + " is null"; - } - - // @SuppressWarnings("unchecked") - // public OptionalNode(ObjectNode node) { - // super(node.getPath(), node.getter, node.seenTypes); - // isPresentNode = new BooleanValueNode( - // node.getPath() + "/isPresent", - // LogSink.NT, - // _v -> ((Optional) _v).isPresent()); - // this.err = node.getPath() + " is null"; - // for (LoggingNode child : node.children) { - // addChild(child); - // } - // } - - @SuppressWarnings("unchecked") - public void log(Object obj) { - Optional oo = (Optional) getter.apply(obj); - if (oo == null) { - RuntimeLog.warn(err); - isPresentNode.log(Optional.empty()); - return; - } - isPresentNode.log(oo); - if (oo.isPresent()) { - super.logDirect(oo.get()); - } - } - } - - public static class StaticObjectNode extends ComposableNode { - private final Object object; - - public StaticObjectNode(String path, Object object) { - super(path); - this.object = object; - } - - @Override - public boolean isImmutable() { - return true; - } - - @Override - public LoggingNode withNewPath(String path) { - return new StaticObjectNode(path, object); - } - - public void log(Object obj) { - for (LoggingNode child : children) { - child.log(object); - } - } - } - - public static class SingletonNode extends ComposableNode { - private final Class type; - private final VarHandle handle; - - public SingletonNode(String path, Class type, VarHandle handle) { - super(path); - this.type = type; - this.handle = handle; - } - - @Override - public LoggingNode withNewPath(String path) { - return new SingletonNode(path, type, handle); - } - - public void log(Object obj) { - Object o = handle.get(type); - if (o == null) { - RuntimeLog.warn(getPath() + " is null"); - return; - } - for (LoggingNode child : children) { - child.log(o); - } - } - } -} diff --git a/src/main/java/monologue/MonoEntryLayer.java b/src/main/java/monologue/MonoEntryLayer.java deleted file mode 100644 index ea4938a..0000000 --- a/src/main/java/monologue/MonoEntryLayer.java +++ /dev/null @@ -1,470 +0,0 @@ -package monologue; - -import edu.wpi.first.networktables.*; -import edu.wpi.first.util.datalog.*; -import edu.wpi.first.util.function.BooleanConsumer; -import edu.wpi.first.util.struct.Struct; -import edu.wpi.first.wpilibj.DataLogManager; -import java.util.HashMap; -import java.util.Optional; -import java.util.function.Consumer; -import java.util.function.DoubleConsumer; -import java.util.function.LongConsumer; - -class MonoEntryLayer { - private static final HashMap>> entries = - new HashMap<>() { - { - put(LogSink.NT, new HashMap<>()); - put(LogSink.DL, new HashMap<>()); - put(LogSink.OP, new HashMap<>()); - } - }; - - public static interface MonologueEntry { - public void log(T value); - - public LogSink sink(); - - @SuppressWarnings("unchecked") - public static MonologueEntry create(String path, Class clazz, LogSink sink) { - var map = entries.get(sink); - if (!map.containsKey(path)) { - String cleanPath = NetworkTable.normalizeKey(path, true); - var e = - switch (sink) { - case NT -> new MonologueNtEntry<>(cleanPath, Optional.empty(), clazz); - case DL -> new MonologueFileEntry<>(cleanPath, Optional.empty(), clazz); - case OP -> new MonologueOptimizedEntry<>(cleanPath, Optional.empty(), clazz); - }; - entries.get(sink).put(path, e); - return e; - } else { - return (MonologueEntry) map.get(path); - } - } - - @SuppressWarnings("unchecked") - public static MonologueEntry create(String path, Struct struct, LogSink sink) { - var map = entries.get(sink); - if (!map.containsKey(path)) { - String cleanPath = NetworkTable.normalizeKey(path, true); - var e = - switch (sink) { - case NT -> - new MonologueNtEntry<>(cleanPath, Optional.of(struct), struct.getTypeClass()); - case DL -> - new MonologueFileEntry<>(cleanPath, Optional.of(struct), struct.getTypeClass()); - case OP -> - new MonologueOptimizedEntry<>( - cleanPath, Optional.of(struct), struct.getTypeClass()); - }; - map.put(path, e); - return e; - } else { - return (MonologueEntry) map.get(path); - } - } - - @SuppressWarnings("unchecked") - public static MonologueEntry create( - String path, Struct struct, Class clazz, LogSink sink) { - var map = entries.get(sink); - if (!map.containsKey(path)) { - String cleanPath = NetworkTable.normalizeKey(path, true); - var e = - switch (sink) { - case NT -> new MonologueNtEntry<>(cleanPath, Optional.of(struct), clazz); - case DL -> new MonologueFileEntry<>(cleanPath, Optional.of(struct), clazz); - case OP -> new MonologueOptimizedEntry<>(cleanPath, Optional.of(struct), clazz); - }; - map.put(path, e); - return e; - } else { - return (MonologueEntry) map.get(path); - } - } - - public static MonologueDoubleEntry createDouble(String path, LogSink sink) { - var map = entries.get(sink); - if (!map.containsKey(path)) { - MonologueDoubleEntry e = new MonologueDoubleEntry(path, sink); - map.put(path, e); - return e; - } else { - return (MonologueDoubleEntry) map.get(path); - } - } - - public static MonologueBooleanEntry createBoolean(String path, LogSink sink) { - var map = entries.get(sink); - if (!map.containsKey(path)) { - MonologueBooleanEntry e = new MonologueBooleanEntry(path, sink); - map.put(path, e); - return e; - } else { - return (MonologueBooleanEntry) map.get(path); - } - } - - public static MonologueLongEntry createLong(String path, LogSink sink) { - var map = entries.get(sink); - if (!map.containsKey(path)) { - MonologueLongEntry e = new MonologueLongEntry(path, sink); - map.put(path, e); - return e; - } else { - return (MonologueLongEntry) map.get(path); - } - } - } - - private static class MonologueFileEntry implements MonologueEntry { - private final Consumer fileLog; - - @SuppressWarnings("unchecked") - public MonologueFileEntry(String path, Optional> optStruct, Class clazz) { - DataLog dl = DataLogManager.getLog(); - if (optStruct.isPresent()) { - if (clazz.isArray()) { - var entry = StructArrayLogEntry.create(dl, path, optStruct.get()); - fileLog = v -> ((StructArrayLogEntry) entry).append((Object[]) v); - } else { - StructLogEntry entry = StructLogEntry.create(dl, path, (Struct) optStruct.get()); - fileLog = entry::append; - } - } else if (clazz.equals(Double.class) || clazz.equals(double.class)) { - DoubleLogEntry entry = new DoubleLogEntry(dl, path); - fileLog = v -> entry.append((double) v); - } else if (clazz.equals(Float.class) || clazz.equals(float.class)) { - FloatLogEntry entry = new FloatLogEntry(dl, path); - fileLog = v -> entry.append((float) v); - } else if (clazz.equals(Boolean.class) || clazz.equals(boolean.class)) { - BooleanLogEntry entry = new BooleanLogEntry(dl, path); - fileLog = v -> entry.append((boolean) v); - } else if (clazz.equals(Integer.class) || clazz.equals(int.class)) { - IntegerLogEntry entry = new IntegerLogEntry(dl, path); - fileLog = v -> entry.append((int) v); - } else if (clazz.equals(Long.class) || clazz.equals(long.class)) { - IntegerLogEntry entry = new IntegerLogEntry(dl, path); - fileLog = v -> entry.append((long) v); - } else if (clazz.equals(String.class)) { - StringLogEntry entry = new StringLogEntry(dl, path); - fileLog = v -> entry.append((String) v); - } else if (clazz.equals(Double[].class) || clazz.equals(double[].class)) { - DoubleArrayLogEntry entry = new DoubleArrayLogEntry(dl, path); - fileLog = v -> entry.append((double[]) v); - } else if (clazz.equals(Float[].class) || clazz.equals(float[].class)) { - FloatArrayLogEntry entry = new FloatArrayLogEntry(dl, path); - fileLog = v -> entry.append((float[]) v); - } else if (clazz.equals(Boolean[].class) || clazz.equals(boolean[].class)) { - BooleanArrayLogEntry entry = new BooleanArrayLogEntry(dl, path); - fileLog = v -> entry.append((boolean[]) v); - } else if (clazz.equals(Integer[].class) || clazz.equals(int[].class)) { - IntegerArrayLogEntry entry = new IntegerArrayLogEntry(dl, path); - fileLog = - v -> { - int[] ints = (int[]) v; - long[] longs = new long[ints.length]; - for (int i = 0; i < ints.length; i++) { - longs[i] = ints[i]; - } - entry.append(longs); - }; - } else if (clazz.equals(String[].class)) { - StringArrayLogEntry entry = new StringArrayLogEntry(dl, path); - fileLog = v -> entry.append((String[]) v); - } else if (clazz.equals(byte[].class) || clazz.equals(Byte[].class)) { - RawLogEntry entry = new RawLogEntry(dl, path); - fileLog = v -> entry.append((byte[]) v); - } else { - throw new IllegalArgumentException("Unsupported type: " + clazz); - } - } - - @Override - public void log(T value) { - fileLog.accept(value); - } - - @Override - public LogSink sink() { - return LogSink.DL; - } - } - - private static class MonologueNtEntry implements MonologueEntry { - private final Consumer ntLog; - - @SuppressWarnings("unchecked") - public MonologueNtEntry(String path, Optional> optStruct, Class clazz) { - NetworkTableInstance nt = NetworkTableInstance.getDefault(); - if (optStruct.isPresent()) { - if (clazz.isArray()) { - var entry = nt.getStructArrayTopic(path, optStruct.get()).publish(); - ntLog = v -> ((StructArrayPublisher) entry).set((Object[]) v); - } else { - StructPublisher entry = nt.getStructTopic(path, (Struct) optStruct.get()).publish(); - ntLog = entry::set; - } - } else if (clazz.equals(Double.class) || clazz.equals(double.class)) { - DoublePublisher entry = nt.getDoubleTopic(path).publish(); - ntLog = v -> entry.set((double) v); - } else if (clazz.equals(Float.class) || clazz.equals(float.class)) { - FloatPublisher entry = nt.getFloatTopic(path).publish(); - ntLog = v -> entry.set((float) v); - } else if (clazz.equals(Boolean.class) || clazz.equals(boolean.class)) { - BooleanPublisher entry = nt.getBooleanTopic(path).publish(); - ntLog = v -> entry.set((boolean) v); - } else if (clazz.equals(Integer.class) || clazz.equals(int.class)) { - IntegerPublisher entry = nt.getIntegerTopic(path).publish(); - ntLog = v -> entry.set((int) v); - } else if (clazz.equals(Long.class) || clazz.equals(long.class)) { - IntegerPublisher entry = nt.getIntegerTopic(path).publish(); - ntLog = v -> entry.set((long) v); - } else if (clazz.equals(String.class)) { - StringPublisher entry = nt.getStringTopic(path).publish(); - ntLog = v -> entry.set((String) v); - } else if (clazz.equals(Double[].class) || clazz.equals(double[].class)) { - DoubleArrayPublisher entry = nt.getDoubleArrayTopic(path).publish(); - ntLog = v -> entry.set((double[]) v); - } else if (clazz.equals(Float[].class) || clazz.equals(float[].class)) { - FloatArrayPublisher entry = nt.getFloatArrayTopic(path).publish(); - ntLog = v -> entry.set((float[]) v); - } else if (clazz.equals(Boolean[].class) || clazz.equals(boolean[].class)) { - BooleanArrayPublisher entry = nt.getBooleanArrayTopic(path).publish(); - ntLog = v -> entry.set((boolean[]) v); - } else if (clazz.equals(Integer[].class) || clazz.equals(int[].class)) { - IntegerArrayPublisher entry = nt.getIntegerArrayTopic(path).publish(); - ntLog = - v -> { - int[] ints = (int[]) v; - long[] longs = new long[ints.length]; - for (int i = 0; i < ints.length; i++) { - longs[i] = ints[i]; - } - entry.set(longs); - }; - } else if (clazz.equals(String[].class)) { - StringArrayPublisher entry = nt.getStringArrayTopic(path).publish(); - ntLog = v -> entry.set((String[]) v); - } else if (clazz.equals(byte[].class) || clazz.equals(Byte[].class)) { - RawPublisher entry = nt.getRawTopic(path).publish("raw"); - ntLog = v -> entry.set((byte[]) v); - } else { - throw new IllegalArgumentException("Unsupported type: " + clazz); - } - } - - @Override - public void log(T value) { - ntLog.accept(value); - } - - @Override - public LogSink sink() { - return LogSink.NT; - } - } - - private static class MonologueOptimizedEntry implements MonologueEntry { - private final MonologueFileEntry fileEntry; - private final MonologueNtEntry ntEntry; - - public MonologueOptimizedEntry(String path, Optional> optStruct, Class clazz) { - fileEntry = new MonologueFileEntry<>(path, optStruct, clazz); - ntEntry = new MonologueNtEntry<>(path, optStruct, clazz); - } - - @Override - public void log(T value) { - if (Monologue.isBandwidthOptimizationEnabled()) { - fileEntry.log(value); - } else { - ntEntry.log(value); - } - } - - @Override - public LogSink sink() { - return LogSink.OP; - } - } - - public static class MonologueDoubleEntry implements MonologueEntry { - private final LogSink sink; - private final DoubleConsumer fileLog; - private final DoubleConsumer ntLog; - - public MonologueDoubleEntry(String path, LogSink sink) { - this.sink = sink; - fileLog = - new DoubleConsumer() { - private Optional entry = Optional.empty(); - - @Override - public void accept(double value) { - if (entry.isEmpty()) { - entry = Optional.of(new DoubleLogEntry(DataLogManager.getLog(), path)); - } - entry.get().append(value); - } - }; - ntLog = - new DoubleConsumer() { - private Optional entry = Optional.empty(); - - @Override - public void accept(double value) { - if (entry.isEmpty()) { - entry = - Optional.of(NetworkTableInstance.getDefault().getDoubleTopic(path).publish()); - } - entry.get().set(value); - } - }; - } - - public void logDouble(double value) { - switch (sink) { - case NT -> ntLog.accept(value); - case DL -> fileLog.accept(value); - case OP -> { - if (Monologue.isBandwidthOptimizationEnabled()) { - fileLog.accept(value); - } else { - ntLog.accept(value); - } - } - } - } - - @Override - public void log(Double value) { - logDouble(value); - } - - @Override - public LogSink sink() { - return sink; - } - } - - public static class MonologueBooleanEntry implements MonologueEntry { - private final LogSink sink; - private final BooleanConsumer fileLog; - private final BooleanConsumer ntLog; - - public MonologueBooleanEntry(String path, LogSink sink) { - this.sink = sink; - fileLog = - new BooleanConsumer() { - private Optional entry = Optional.empty(); - - @Override - public void accept(boolean value) { - if (entry.isEmpty()) { - entry = Optional.of(new BooleanLogEntry(DataLogManager.getLog(), path)); - } - entry.get().append(value); - } - }; - ntLog = - new BooleanConsumer() { - private Optional entry = Optional.empty(); - - @Override - public void accept(boolean value) { - if (entry.isEmpty()) { - entry = - Optional.of(NetworkTableInstance.getDefault().getBooleanTopic(path).publish()); - } - entry.get().set(value); - } - }; - } - - public void logBoolean(boolean value) { - switch (sink) { - case NT -> ntLog.accept(value); - case DL -> fileLog.accept(value); - case OP -> { - if (Monologue.isBandwidthOptimizationEnabled()) { - fileLog.accept(value); - } else { - ntLog.accept(value); - } - } - } - } - - @Override - public void log(Boolean value) { - logBoolean(value); - } - - @Override - public LogSink sink() { - return sink; - } - } - - public static class MonologueLongEntry implements MonologueEntry { - private final LogSink sink; - private final LongConsumer fileLog; - private final LongConsumer ntLog; - - public MonologueLongEntry(String path, LogSink sink) { - this.sink = sink; - fileLog = - new LongConsumer() { - private Optional entry = Optional.empty(); - - @Override - public void accept(long value) { - if (entry.isEmpty()) { - entry = Optional.of(new IntegerLogEntry(DataLogManager.getLog(), path)); - } - entry.get().append(value); - } - }; - ntLog = - new LongConsumer() { - private Optional entry = Optional.empty(); - - @Override - public void accept(long value) { - if (entry.isEmpty()) { - entry = - Optional.of(NetworkTableInstance.getDefault().getIntegerTopic(path).publish()); - } - entry.get().set(value); - } - }; - } - - public void logLong(long value) { - switch (sink) { - case NT -> ntLog.accept(value); - case DL -> fileLog.accept(value); - case OP -> { - if (Monologue.isBandwidthOptimizationEnabled()) { - fileLog.accept(value); - } else { - ntLog.accept(value); - } - } - } - } - - @Override - public void log(Long value) { - logLong(value); - } - - @Override - public LogSink sink() { - return sink; - } - } -} diff --git a/src/main/java/monologue/MonoSendableLayer.java b/src/main/java/monologue/MonoSendableLayer.java deleted file mode 100644 index 035b907..0000000 --- a/src/main/java/monologue/MonoSendableLayer.java +++ /dev/null @@ -1,659 +0,0 @@ -package monologue; - -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.util.function.BooleanConsumer; -import edu.wpi.first.util.function.FloatConsumer; -import edu.wpi.first.util.function.FloatSupplier; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismObject2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import java.lang.invoke.MethodHandles; -import java.lang.invoke.VarHandle; -import java.util.ArrayList; -import java.util.List; -import java.util.Map; -import java.util.function.BooleanSupplier; -import java.util.function.Consumer; -import java.util.function.DoubleConsumer; -import java.util.function.DoubleSupplier; -import java.util.function.LongConsumer; -import java.util.function.LongSupplier; -import java.util.function.Supplier; -import monologue.MonoEntryLayer.MonologueBooleanEntry; -import monologue.MonoEntryLayer.MonologueDoubleEntry; -import monologue.MonoEntryLayer.MonologueEntry; -import monologue.MonoEntryLayer.MonologueLongEntry; - -public class MonoSendableLayer { - private static final ArrayList sendables = new ArrayList<>(); - - static void updateAll() { - for (SendableContainer sendable : sendables) { - sendable.update(); - } - } - - static void addSendableContainer(SendableContainer container) { - container.postConstants(); - sendables.add(container); - } - - static class SendableContainer { - final ArrayList> queue = new ArrayList<>(); - final ArrayList updates = new ArrayList<>(); - final ArrayList constants = new ArrayList<>(); - final LogSink sink; - boolean useQueue = false; - - SendableContainer(LogSink sink) { - this.sink = sink; - } - - private void emptyQueue() { - if (queue.isEmpty()) { - return; - } - for (Pair pair : queue) { - if (pair.getFirst()) { - updates.add(pair.getSecond()); - } else { - constants.add(pair.getSecond()); - } - } - queue.clear(); - } - - void addUpdatable(Runnable r) { - if (useQueue) { - queue.add(new Pair<>(true, r)); - } else { - updates.add(r); - } - } - - void addConstant(Runnable r) { - if (useQueue) { - queue.add(new Pair<>(false, r)); - } else { - constants.add(r); - } - } - - void update() { - useQueue = true; - emptyQueue(); - updates.forEach(Runnable::run); - useQueue = false; - } - - void postConstants() { - useQueue = true; - emptyQueue(); - constants.forEach(Runnable::run); - useQueue = false; - } - } - - static class Builder implements SendableBuilder { - private final String path; - private final SendableContainer sendable; - private final LogSink sink; - - Builder(String path, LogSink sink) { - this.path = path; - this.sendable = new SendableContainer(sink); - this.sink = sink; - } - - void finish() { - addSendableContainer(sendable); - } - - @Override - public void setSmartDashboardType(String type) { - sendable.addConstant( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/.type", String.class, sink); - - public void run() { - entry.log(type); - } - }); - } - - @Override - public void setActuator(boolean value) {} - - @Override - public void setSafeState(Runnable func) {} - - @Override - public void addCloseable(AutoCloseable closeable) {} - - @Override - public void clearProperties() {} - - @Override - public BackendKind getBackendKind() { - return BackendKind.kUnknown; - } - - @Override - public boolean isPublished() { - return true; - } - - @Override - public void close() throws Exception {} - - @Override - public void update() {} - - @Override - public void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer _setter) { - if (getter == null) return; - sendable.addUpdatable( - new Runnable() { - MonologueBooleanEntry entry = MonologueEntry.createBoolean(path + "/" + key, sink); - - public void run() { - entry.logBoolean(getter.getAsBoolean()); - } - }); - } - - @Override - public void addBooleanArrayProperty( - String key, Supplier getter, Consumer _setter) { - if (getter == null) return; - sendable.addUpdatable( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, boolean[].class, sink); - - public void run() { - entry.log(getter.get()); - } - }); - } - - @Override - public void publishConstBoolean(String key, boolean value) { - sendable.addConstant( - new Runnable() { - MonologueBooleanEntry entry = MonologueEntry.createBoolean(path + "/" + key, sink); - - public void run() { - entry.logBoolean(value); - } - }); - } - - @Override - public void publishConstBooleanArray(String key, boolean[] value) { - sendable.addConstant( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, boolean[].class, sink); - - public void run() { - entry.log(value); - } - }); - } - - @Override - public void addIntegerProperty(String key, LongSupplier getter, LongConsumer _setter) { - if (getter == null) return; - sendable.addUpdatable( - new Runnable() { - MonologueLongEntry entry = MonologueEntry.createLong(path + "/" + key, sink); - - public void run() { - entry.logLong(getter.getAsLong()); - } - }); - } - - @Override - public void addIntegerArrayProperty( - String key, Supplier getter, Consumer _setter) { - if (getter == null) return; - sendable.addUpdatable( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, long[].class, sink); - - public void run() { - entry.log(getter.get()); - } - }); - } - - @Override - public void publishConstInteger(String key, long value) { - sendable.addConstant( - new Runnable() { - MonologueLongEntry entry = MonologueEntry.createLong(path + "/" + key, sink); - - public void run() { - entry.logLong(value); - } - }); - } - - @Override - public void publishConstIntegerArray(String key, long[] value) { - sendable.addConstant( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, long[].class, sink); - - public void run() { - entry.log(value); - } - }); - } - - @Override - public void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer _setter) { - if (getter == null) return; - sendable.addUpdatable( - new Runnable() { - MonologueDoubleEntry entry = MonologueEntry.createDouble(path + "/" + key, sink); - - public void run() { - entry.logDouble(getter.getAsDouble()); - } - }); - } - - @Override - public void addDoubleArrayProperty( - String key, Supplier getter, Consumer _setter) { - if (getter == null) return; - sendable.addUpdatable( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, double[].class, sink); - - public void run() { - entry.log(getter.get()); - } - }); - } - - @Override - public void publishConstDouble(String key, double value) { - sendable.addConstant( - new Runnable() { - MonologueDoubleEntry entry = MonologueEntry.createDouble(path + "/" + key, sink); - - public void run() { - entry.logDouble(value); - } - }); - } - - @Override - public void publishConstDoubleArray(String key, double[] value) { - sendable.addConstant( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, double[].class, sink); - - public void run() { - entry.log(value); - } - }); - } - - @Override - public void addFloatProperty(String key, FloatSupplier getter, FloatConsumer _setter) { - if (getter == null) return; - sendable.addUpdatable( - new Runnable() { - MonologueDoubleEntry entry = MonologueEntry.createDouble(path + "/" + key, sink); - - public void run() { - entry.logDouble(getter.getAsFloat()); - } - }); - } - - @Override - public void addFloatArrayProperty( - String key, Supplier getter, Consumer _setter) { - if (getter == null) return; - sendable.addUpdatable( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, float[].class, sink); - - public void run() { - entry.log(getter.get()); - } - }); - } - - @Override - public void publishConstFloat(String key, float value) { - sendable.addConstant( - new Runnable() { - MonologueDoubleEntry entry = MonologueEntry.createDouble(path + "/" + key, sink); - - public void run() { - entry.logDouble(value); - } - }); - } - - @Override - public void publishConstFloatArray(String key, float[] value) { - sendable.addConstant( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, float[].class, sink); - - public void run() { - entry.log(value); - } - }); - } - - @Override - public void addStringProperty(String key, Supplier getter, Consumer _setter) { - if (getter == null) return; - sendable.addUpdatable( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, String.class, sink); - - public void run() { - entry.log(getter.get()); - } - }); - } - - @Override - public void addStringArrayProperty( - String key, Supplier getter, Consumer _setter) { - if (getter == null) return; - sendable.addUpdatable( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, String[].class, sink); - - public void run() { - entry.log(getter.get()); - } - }); - } - - @Override - public void publishConstString(String key, String value) { - sendable.addConstant( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, String.class, sink); - - public void run() { - entry.log(value); - } - }); - } - - @Override - public void publishConstStringArray(String key, String[] value) { - sendable.addConstant( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, String[].class, sink); - - public void run() { - entry.log(value); - } - }); - } - - @Override - public void addRawProperty( - String key, String typeString, Supplier getter, Consumer setter) { - if (getter == null) return; - sendable.addUpdatable( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, byte[].class, sink); - - public void run() { - entry.log(getter.get()); - } - }); - } - - @Override - public void publishConstRaw(String key, String typeString, byte[] value) { - sendable.addConstant( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/" + key, byte[].class, sink); - - public void run() { - entry.log(value); - } - }); - } - } - - static class NtSendableCompat { - static final VarHandle field2dObject; - static final VarHandle field2dObjectName; - static final VarHandle field2dObjectPoses; - - static final VarHandle mechanism2dDims; - static final VarHandle mechanism2dColor; - static final VarHandle mechanism2dRoots; - static final VarHandle mechanism2dRootX; - static final VarHandle mechanism2dRootY; - static final VarHandle mechanism2dLigamentAngle; - static final VarHandle mechanism2dLigamentColor; - static final VarHandle mechanism2dLigamentLength; - static final VarHandle mechanism2dLigamentWeight; - static final VarHandle mechanism2dObjects; - - static { - final MethodHandles.Lookup lookup = MethodHandles.lookup(); - try { - MethodHandles.Lookup field2dLookup = MethodHandles.privateLookupIn(Field2d.class, lookup); - MethodHandles.Lookup fieldObjectLookup = - MethodHandles.privateLookupIn(FieldObject2d.class, lookup); - field2dObject = field2dLookup.findVarHandle(Field2d.class, "m_objects", List.class); - field2dObjectName = - fieldObjectLookup.findVarHandle(FieldObject2d.class, "m_name", String.class); - field2dObjectPoses = - fieldObjectLookup.findVarHandle(FieldObject2d.class, "m_poses", List.class); - - MethodHandles.Lookup mechanism2dLookup = - MethodHandles.privateLookupIn(Mechanism2d.class, lookup); - MethodHandles.Lookup rootLookup = - MethodHandles.privateLookupIn(MechanismRoot2d.class, lookup); - MethodHandles.Lookup ligamentLookup = - MethodHandles.privateLookupIn(MechanismLigament2d.class, lookup); - MethodHandles.Lookup objectLookup = - MethodHandles.privateLookupIn(MechanismObject2d.class, lookup); - mechanism2dDims = - mechanism2dLookup.findVarHandle(Mechanism2d.class, "m_dims", double[].class); - mechanism2dColor = - mechanism2dLookup.findVarHandle(Mechanism2d.class, "m_color", String.class); - mechanism2dRoots = mechanism2dLookup.findVarHandle(Mechanism2d.class, "m_roots", Map.class); - mechanism2dRootX = rootLookup.findVarHandle(MechanismRoot2d.class, "m_x", double.class); - mechanism2dRootY = rootLookup.findVarHandle(MechanismRoot2d.class, "m_y", double.class); - mechanism2dLigamentAngle = - ligamentLookup.findVarHandle(MechanismLigament2d.class, "m_angle", double.class); - mechanism2dLigamentColor = - ligamentLookup.findVarHandle(MechanismLigament2d.class, "m_color", String.class); - mechanism2dLigamentLength = - ligamentLookup.findVarHandle(MechanismLigament2d.class, "m_length", double.class); - mechanism2dLigamentWeight = - ligamentLookup.findVarHandle(MechanismLigament2d.class, "m_weight", double.class); - mechanism2dObjects = - objectLookup.findVarHandle(MechanismObject2d.class, "m_objects", Map.class); - } catch (NoSuchFieldException | IllegalAccessException e) { - throw new RuntimeException(e); - } - } - - public static void addField2d(String path, Field2d field, LogSink sink) { - SendableContainer sendable = new SendableContainer(sink); - List objects = (List) field2dObject.get(field); - - sendable.addUpdatable( - new Runnable() { - public void run() { - for (FieldObject2d object : objects) { - String name = (String) field2dObjectName.get(object); - MonologueEntry entry = - MonologueEntry.create(path + "/" + name, double[].class, sink); - List poses = (List) field2dObjectPoses.get(object); - double[] arr = new double[3 * poses.size()]; - int ndx = 0; - for (Pose2d pose : poses) { - var translation = pose.getTranslation(); - arr[ndx + 0] = translation.getX(); - arr[ndx + 1] = translation.getY(); - arr[ndx + 2] = pose.getRotation().getDegrees(); - ndx += 3; - } - entry.log(arr); - } - } - }); - - sendable.addConstant( - new Runnable() { - MonologueEntry entry = - MonologueEntry.create(path + "/.type", String.class, sink); - - public void run() { - entry.log("Field2d"); - } - }); - - addSendableContainer(sendable); - } - - public static void addMechanism2dLigament( - String path, MechanismLigament2d ligament, SendableContainer sendable, LogSink sink) { - double angle = (double) mechanism2dLigamentAngle.get(ligament); - String color = (String) mechanism2dLigamentColor.get(ligament); - double length = (double) mechanism2dLigamentLength.get(ligament); - double weight = (double) mechanism2dLigamentWeight.get(ligament); - - sendable.addUpdatable( - new Runnable() { - MonologueDoubleEntry angleEntry = MonologueEntry.createDouble(path + "/angle", sink); - MonologueEntry colorEntry = - MonologueEntry.create(path + "/color", String.class, sink); - MonologueDoubleEntry lengthEntry = MonologueEntry.createDouble(path + "/length", sink); - MonologueDoubleEntry weightEntry = MonologueEntry.createDouble(path + "/weight", sink); - - public void run() { - angleEntry.logDouble(angle); - colorEntry.log(color); - lengthEntry.logDouble(length); - weightEntry.logDouble(weight); - } - }); - - sendable.addConstant( - new Runnable() { - MonologueEntry typeEntry = - MonologueEntry.create(path + "/.type", String.class, sink); - - public void run() { - typeEntry.log("line"); - } - }); - } - - public static void addMechanism2dRoot( - String path, MechanismRoot2d root, SendableContainer sendable, LogSink sink) { - double x = (double) mechanism2dRootX.get(root); - double y = (double) mechanism2dRootY.get(root); - - sendable.addUpdatable( - new Runnable() { - MonologueDoubleEntry xEntry = MonologueEntry.createDouble(path + "/x", sink); - MonologueDoubleEntry yEntry = MonologueEntry.createDouble(path + "/y", sink); - - public void run() { - xEntry.logDouble(x); - yEntry.logDouble(y); - } - }); - } - - public static void addMechanism2dObject( - String path, MechanismObject2d object, SendableContainer sendable, LogSink sink) { - Map objects = - (Map) mechanism2dObjects.get(object); - - if (object instanceof MechanismLigament2d) { - addMechanism2dLigament(path, (MechanismLigament2d) object, sendable, sink); - } else if (object instanceof MechanismRoot2d) { - addMechanism2dRoot(path, (MechanismRoot2d) object, sendable, sink); - } - - for (Map.Entry entry : objects.entrySet()) { - addMechanism2dObject(path + "/" + entry.getKey(), entry.getValue(), sendable, sink); - } - } - - public static void addMechanism2d(String path, Mechanism2d mech, LogSink sink) { - SendableContainer sendable = new SendableContainer(sink); - Map roots = - (Map) mechanism2dRoots.get(mech); - - sendable.addUpdatable( - new Runnable() { - MonologueEntry dimsEntry = - MonologueEntry.create(path + "/dims", double[].class, sink); - MonologueEntry colorEntry = - MonologueEntry.create(path + "/backgroundColor", String.class, sink); - - public void run() { - dimsEntry.log((double[]) mechanism2dDims.get(mech)); - colorEntry.log((String) mechanism2dColor.get(mech)); - } - }); - - for (Map.Entry entry : roots.entrySet()) { - addMechanism2dObject(path + "/" + entry.getKey(), entry.getValue(), sendable, sink); - } - - sendable.addConstant( - new Runnable() { - MonologueEntry typeEntry = - MonologueEntry.create(path + "/.type", String.class, sink); - MonologueEntry controllableEntry = - MonologueEntry.create(path + "/.controllable", Boolean.class, sink); - MonologueEntry nameEntry = - MonologueEntry.create(path + "/.name", String.class, sink); - - public void run() { - typeEntry.log("Mechanism2d"); - controllableEntry.log(true); - nameEntry.log(path); - } - }); - - addSendableContainer(sendable); - } - } -} diff --git a/src/main/java/monologue/Monologue.java b/src/main/java/monologue/Monologue.java deleted file mode 100644 index 47869ca..0000000 --- a/src/main/java/monologue/Monologue.java +++ /dev/null @@ -1,349 +0,0 @@ -package monologue; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DataLogManager; -import edu.wpi.first.wpilibj.Timer; -import java.util.ArrayList; -import java.util.function.BooleanSupplier; -import monologue.LoggingTree.StaticObjectNode; - -/** - * The Monologue class is the main entry point for the Monologue library. It is responsible for - * setting up the Monologue library, updating the loggers, and logging objects. - * - *

Monologue is a library that allows for easy logging of objects to NetworkTables and Datalog. - * It has {@link Annotations} that allow implicit logging of fields and methods on objects that - * implement the {@link Logged} interface. - * - *

Monologue works by creating a tree of objects that implement the {@link Logged} interface and - * then logging the fields and methods of those objects to NetworkTables and Datalog based on their - * annotations. For example let's say the root object is {@code Robot.java}, you would implemenet - * {@link Logged} on the root object and then call {@link #setupMonologue(Logged, String, - * MonologueConfig)} with the root object and a root path (typically "/Robot"). This will recurse - * through all the fields in {@code RobotContainer.java} and search for more objects that implement - * {@link Logged} and repeat the process until all fields and methods have been logged. - * - *

Monologue has a rich error handling system that will tell you what you did wrong and where you - * did it wrong. If you would like to run Monologue in whole robot Unit Tests you can use {@link - * #setupMonologueDisabled(Logged, String, boolean)} to disable logging and only run the error - * checking. - * - *

WARNING: Any use of `DatalogManager` before Monologue.setupMonologue() is undefined - * behavior and can result in a crash. - */ -public class Monologue extends GlobalLogged { - /** The Monologue library wide OPTIMIZE_BANDWIDTH flag, is used to divert logging */ - private static boolean OPTIMIZE_BANDWIDTH = true; - - private static MonologueConfig config = new MonologueConfig(); - - private static boolean HAS_SETUP_BEEN_CALLED = false; - private static boolean IS_DISABLED = false; - private static boolean THROW_ON_WARN = false; - - private static final ArrayList prematureCalls = new ArrayList(); - private static final ArrayList trees = new ArrayList(); - - /** - * An object to hold the configuration for the Monologue library. This allows for easier default - * values, more readable code, and ability to add more configuration later without breaking - * existing code. - */ - public static record MonologueConfig( - BooleanSupplier optimizeBandwidthSupplier, - String datalogPrefix, - boolean throwOnWarn, - boolean allowNonFinalLoggedFields) { - public MonologueConfig { - if (optimizeBandwidthSupplier == null) { - RuntimeLog.warn( - "shouldOptimizeBandwidthSupplier cannot be null in MonologueConfig, falling back to false (always log NT)"); - - optimizeBandwidthSupplier = () -> false; - } - if (datalogPrefix == null) { - RuntimeLog.warn("datalogPrefix cannot be null in MonologueConfig, falling back to \"NT:\""); - datalogPrefix = "NT:"; - } - } - - public MonologueConfig() { - this(() -> false, "NT:", false, false); - } - - /** - * Updates the OptimizeBandwidth flag supplier. - * - * @param optimizeBandwidth The new OptimizeBandwidth flag supplier - * @return A new MonologueConfig object with the updated OptimizeBandwidth flag supplier - */ - public MonologueConfig withOptimizeBandwidth(BooleanSupplier optimizeBandwidth) { - return new MonologueConfig( - optimizeBandwidth, datalogPrefix, throwOnWarn, allowNonFinalLoggedFields); - } - - /** - * Updates the OptimizeBandwidth static flag. - * - * @param optimizeBandwidth The new OptimizeBandwidth flag - * @return A new MonologueConfig object with the updated OptimizeBandwidth flag - */ - public MonologueConfig withOptimizeBandwidth(boolean optimizeBandwidth) { - return new MonologueConfig( - () -> optimizeBandwidth, datalogPrefix, throwOnWarn, allowNonFinalLoggedFields); - } - - /** - * Updates the lazyLogging flag. - * - * @param lazyLogging The new lazyLogging flag - * @return A new MonologueConfig object with the updated lazyLogging flag - */ - public MonologueConfig withLazyLogging(boolean lazyLogging) { - return new MonologueConfig( - optimizeBandwidthSupplier, datalogPrefix, throwOnWarn, allowNonFinalLoggedFields); - } - - /** - * Updates the datalogPrefix. - * - * @param datalogPrefix The new datalogPrefix - * @return A new MonologueConfig object with the updated datalogPrefix - */ - public MonologueConfig withDatalogPrefix(String datalogPrefix) { - return new MonologueConfig( - optimizeBandwidthSupplier, datalogPrefix, throwOnWarn, allowNonFinalLoggedFields); - } - - /** - * Updates the throwOnWarn flag. If true, Monologue will throw an exception when a Monologue - * internal warning is emitted. This is useful for catching issues in CI / Unit Tests. - * - * @param throwOnWarn The new throwOnWarn flag - * @return A new MonologueConfig object with the updated throwOnWarn flag - */ - public MonologueConfig withThrowOnWarning(boolean throwOnWarn) { - return new MonologueConfig( - optimizeBandwidthSupplier, datalogPrefix, throwOnWarn, allowNonFinalLoggedFields); - } - - /** - * Updates the allowNonFinalLoggedFields flag. If true, Monologue will allow non-final fields - * containing {@link Logged} objects to be logged. This is not reccomended as it can lead to - * unexpected behavior. - * - * @param allowNonFinalLoggedFields The new allowNonFinalLoggedFields flag - * @return A new MonologueConfig object with the updated allowNonFinalLoggedFields flag - */ - public MonologueConfig withAllowNonFinalLoggedFields(boolean allowNonFinalLoggedFields) { - return new MonologueConfig( - optimizeBandwidthSupplier, datalogPrefix, throwOnWarn, allowNonFinalLoggedFields); - } - } - - /** - * Is the main entry point for the monologue library. It will interate over every member of the - * provided Logged object and evaluated if it should be logged to the network tables or to a file. - * - *

Will also recursively check field values for classes that implement Logged and log those as - * well. - * - * @param loggable the root Logged object to log - * @param rootpath the root path to log to\ - * @param config the configuration for the Monologue library - * @apiNote Should only be called once, if another {@link Logged} tree needs to be created use - * {@link #logTree(Logged, String)} for additional trees - */ - public static void setupMonologue(Logged loggable, String rootpath, MonologueConfig config) { - if (HAS_SETUP_BEEN_CALLED) { - RuntimeLog.warn( - "Monologue.setupMonologue() has already been called, further calls will do nothing"); - return; - } - - GlobalField.publish(); - - NetworkTableInstance.getDefault() - .startEntryDataLog(DataLogManager.getLog(), "", config.datalogPrefix); - - // create and start a timer to time the setup process - Timer timer = new Timer(); - timer.start(); - - Monologue.config = config; - HAS_SETUP_BEEN_CALLED = true; - rootpath = NetworkTable.normalizeKey(rootpath, true); - Monologue.setRootPath(rootpath); - RuntimeLog.info( - "Monologue.setupMonologue() called on " - + loggable.getClass().getName() - + " with rootpath " - + rootpath - + " and config" - + config); - - THROW_ON_WARN = config.throwOnWarn; - - OPTIMIZE_BANDWIDTH = config.optimizeBandwidthSupplier.getAsBoolean(); - - logTree(loggable, rootpath); - - prematureCalls.forEach(Runnable::run); - prematureCalls.clear(); - - System.gc(); - - RuntimeLog.info("Monologue.setupMonologue() finished in " + timer.get() + " seconds"); - } - - /** - * Sets up Monologue in a disabled state, will not log anything. - * - *

This can be helpful for applications like unit tests where you want to validate Monoluge - * logic and logging types without actually logging anything. - * - *

This method can also be called multiple times, this allows this to be called multiple times - * in one unit test session without throwing an exception. - * - * @param loggable the root Logged object to log - * @param rootpath the root path to log to - * @param throwOnWarn if true, will throw an exception when a Monologue internal warning is - * emitted - */ - public static void setupMonologueDisabled(Logged loggable, String rootpath, boolean throwOnWarn) { - if (HAS_SETUP_BEEN_CALLED && !IS_DISABLED) { - RuntimeLog.warn( - "Monologue.setupMonologue() has already been called, disabling after setup will do nothing"); - return; - } - - HAS_SETUP_BEEN_CALLED = true; - IS_DISABLED = true; - THROW_ON_WARN = throwOnWarn; - - RuntimeLog.info( - "Monologue.setupMonologueDisabled() called on " - + loggable.getClass().getName() - + " with rootpath " - + rootpath); - - // wont actually log anything, will just do state and type validation to provide use in CI/unit - // tests - logTree(loggable, rootpath); - - Logged.registry.clear(); - - RuntimeLog.info("Monologue.setupMonologueDisabled() finished"); - } - - /** - * Creates a logging tree for the provided {@link Logged} object. Will also recursively check - * field values for classes that implement {@link Logged} and log those as well. - * - * @param loggable the obj to scrape - * @param path the path to log to - * @throws IllegalStateException If {@link #setupMonologue()} or {@link #setupMonologueDisabled()} - * is not called first - */ - public static void logTree(Logged loggable, String path) { - if (!hasBeenSetup()) - throw new IllegalStateException( - "Tried to use Monologue.logTree() before using a Monologue setup method"); - - if (path == null || path.isEmpty()) { - RuntimeLog.warn("Invalid path for Monologue.logTree(): " + path); - return; - } else if (path == "/") { - RuntimeLog.warn("Root path of / is not allowed for Monologue.logTree()"); - return; - } - RuntimeLog.info( - "Monologue.logTree() called on " + loggable.getClass().getName() + " with path " + path); - - StaticObjectNode node = new LoggingTree.StaticObjectNode(path, loggable); - Eval.exploreNodes(Eval.getLoggedClasses(loggable.getClass()), node); - Logged.addNode(loggable, node); - - trees.add(node); - - updateAll(); - } - - /** - * Updates all the loggers, ideally called every cycle. - * - * @apiNote Should only be called on the same thread monologue was setup on - */ - public static void updateAll() { - if (isMonologueDisabled()) return; - if (!hasBeenSetup()) RuntimeLog.warn("Called Monologue.updateAll before Monologue was setup"); - boolean newOptimizeBandwidth = config.optimizeBandwidthSupplier.getAsBoolean(); - if (newOptimizeBandwidth != OPTIMIZE_BANDWIDTH) { - RuntimeLog.info("Monologue.updateAll() updated FILE_ONLY flag to " + newOptimizeBandwidth); - log("MonologueOptimizeBandwidth", newOptimizeBandwidth); - } - OPTIMIZE_BANDWIDTH = newOptimizeBandwidth; - MonoSendableLayer.updateAll(); - for (StaticObjectNode tree : trees) { - tree.log(null); - } - } - - static void prematureLog(Runnable runnable) { - prematureCalls.add(runnable); - } - - /** - * Checks if the Monologue library is in file only mode. - * - * @return true if Monologue is in file only mode, false otherwise - */ - static boolean isBandwidthOptimizationEnabled() { - return OPTIMIZE_BANDWIDTH; - } - - /** - * Checks if the Monologue library is disabled. - * - * @return true if Monologue is disabled, false otherwise - * @apiNote This is useful for unit tests where you want to validate Monologue logic and logging - */ - static boolean isMonologueDisabled() { - return IS_DISABLED; - } - - /** - * Checks if the Monologue library has been setup. - * - * @return true if Monologue has been setup, false otherwise - */ - static boolean hasBeenSetup() { - return HAS_SETUP_BEEN_CALLED; - } - - /** - * Checks if the Monologue library should throw an exception when a Monologue internal warning is - * emitted. - * - * @return true if Monologue should throw an exception on warning, false otherwise - */ - static boolean shouldThrowOnWarn() { - return THROW_ON_WARN; - } - - /** - * Checks if the Monologue library is ready to log. If it is not ready, it will log a warning - * using the key provided. - * - * @param key The key to log if Monologue is not ready - * @return true if Monologue is ready, false otherwise - */ - static boolean isMonologueReady(String key) { - if (!hasBeenSetup()) { - RuntimeLog.warn("Tried to log \"" + key + "\" before Monologue was setup"); - return false; - } - return true; - } -} diff --git a/src/main/java/monologue/Primatives.java b/src/main/java/monologue/Primatives.java deleted file mode 100644 index e6ea7f2..0000000 --- a/src/main/java/monologue/Primatives.java +++ /dev/null @@ -1,57 +0,0 @@ -package monologue; - -import java.lang.invoke.VarHandle; - -class Primatives { - - @FunctionalInterface - public interface DoubleGetter { - double get(Object o); - } - - @FunctionalInterface - public interface LongGetter { - long get(Object o); - } - - @FunctionalInterface - public interface BooleanGetter { - boolean get(Object o); - } - - public static final class DoubleVarHandle { - private final VarHandle handle; - - public DoubleVarHandle(VarHandle handle) { - this.handle = handle; - } - - public double get(Object o) { - return (double) handle.get(o); - } - } - - public static final class LongVarHandle { - private final VarHandle handle; - - public LongVarHandle(VarHandle handle) { - this.handle = handle; - } - - public long get(Object o) { - return (long) handle.get(o); - } - } - - public static final class BooleanVarHandle { - private final VarHandle handle; - - public BooleanVarHandle(VarHandle handle) { - this.handle = handle; - } - - public boolean get(Object o) { - return (boolean) handle.get(o); - } - } -} diff --git a/src/main/java/monologue/ProceduralStructGenerator.java b/src/main/java/monologue/ProceduralStructGenerator.java deleted file mode 100644 index dc8f39b..0000000 --- a/src/main/java/monologue/ProceduralStructGenerator.java +++ /dev/null @@ -1,1012 +0,0 @@ -package monologue; - -import edu.wpi.first.math.Pair; -import edu.wpi.first.units.Measure; -import edu.wpi.first.util.struct.Struct; -import edu.wpi.first.util.struct.StructSerializable; -import java.lang.annotation.Documented; -import java.lang.annotation.ElementType; -import java.lang.annotation.Retention; -import java.lang.annotation.RetentionPolicy; -import java.lang.annotation.Target; -import java.lang.reflect.AnnotatedElement; -import java.lang.reflect.Field; -import java.lang.reflect.InvocationTargetException; -import java.lang.reflect.Modifier; -import java.lang.reflect.RecordComponent; -import java.nio.ByteBuffer; -import java.nio.charset.Charset; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Collection; -import java.util.HashMap; -import java.util.HashSet; -import java.util.Iterator; -import java.util.List; -import java.util.Optional; -import java.util.OptionalInt; -import java.util.Set; -import java.util.function.Supplier; -import java.util.stream.BaseStream; - -/** A utility class for procedurally generating {@link Struct}s from records and enums. */ -public final class ProceduralStructGenerator { - private ProceduralStructGenerator() { - throw new UnsupportedOperationException("This is a utility class!"); - } - - /** - * A functional interface representing a method that retrives a value from a {@link ByteBuffer}. - */ - @FunctionalInterface - private interface Unpacker { - T unpack(ByteBuffer buffer); - } - - /** A functional interface representing a method that packs a value into a {@link ByteBuffer}. */ - @FunctionalInterface - private interface Packer { - ByteBuffer pack(ByteBuffer buffer, T value); - - static Packer fromStruct(Struct struct) { - return (buffer, value) -> { - struct.pack(buffer, value); - return buffer; - }; - } - } - - private static class DoublePacker implements Packer { - public ByteBuffer packDouble(ByteBuffer buffer, double value) { - return buffer.putDouble(value); - } - - public ByteBuffer pack(ByteBuffer buffer, Double value) { - return packDouble(buffer, value); - } - } - - private record PrimType(String name, int size, Unpacker unpacker, Packer packer) {} - - /** A map of primitive types to their schema types. */ - private static final HashMap, PrimType> primitiveTypeMap = new HashMap<>(); - - private static void addPrimType( - Class boxedClass, - Class primitiveClass, - String name, - int size, - Unpacker unpacker, - Packer packer) { - PrimType primType = new PrimType<>(name, size, unpacker, packer); - primitiveTypeMap.put(boxedClass, primType); - primitiveTypeMap.put(primitiveClass, primType); - } - - // Add primitive types to the map - static { - addPrimType( - Long.class, long.class, "int64", Long.BYTES, ByteBuffer::getLong, ByteBuffer::putLong); - addPrimType( - Integer.class, int.class, "int32", Integer.BYTES, ByteBuffer::getInt, ByteBuffer::putInt); - addPrimType( - Double.class, - double.class, - "float64", - Double.BYTES, - ByteBuffer::getDouble, - new DoublePacker()); - addPrimType( - Float.class, - float.class, - "float32", - Float.BYTES, - ByteBuffer::getFloat, - ByteBuffer::putFloat); - addPrimType( - Boolean.class, - boolean.class, - "bool", - Byte.BYTES, - buffer -> buffer.get() != 0, - (buffer, value) -> buffer.put((byte) (value ? 1 : 0))); - addPrimType( - Character.class, - char.class, - "char", - Character.BYTES, - ByteBuffer::getChar, - ByteBuffer::putChar); - addPrimType(Byte.class, byte.class, "uint8", Byte.BYTES, ByteBuffer::get, ByteBuffer::put); - addPrimType( - Short.class, short.class, "int16", Short.BYTES, ByteBuffer::getShort, ByteBuffer::putShort); - addPrimType( - Long.class, long.class, "int64", Long.BYTES, ByteBuffer::getLong, ByteBuffer::putLong); - } - - /** - * A map of types to their custom struct schemas. - * - *

This allows adding custom struct implementations for types that are not supported by - * default. Think of vendor-specific. - */ - private static final HashMap, Struct> customStructTypeMap = new HashMap<>(); - - /** - * Add a custom struct to the structifier. - * - * @param The type the struct is for. - * @param clazz The class of the type. - * @param struct The struct to add. - * @param override Whether to override an existing struct. An existing struct could mean the type - * already has a {@code struct} field and implemnts {@link StructSerializable} or that the - * type is already in the custom struct map. - */ - public static void addCustomStruct(Class clazz, Struct struct, boolean override) { - if (override) { - customStructTypeMap.put(clazz, struct); - } else if (!StructSerializable.class.isAssignableFrom(clazz)) { - customStructTypeMap.putIfAbsent(clazz, struct); - } - } - - /** - * Returns a {@link Struct} for the given {@link StructSerializable} marked class. Due to the - * non-contractual nature of the marker this can fail. If the {@code struct} field could not be - * accessed for any reason, an empty {@link Optional} is returned. - * - * @param The type of the class. - * @param clazz The class object to extract the struct from. - * @return An optional containing the struct if it could be extracted. - */ - @SuppressWarnings("unchecked") - public static Optional> extractClassStruct( - Class clazz) { - try { - var possibleField = Optional.ofNullable(clazz.getDeclaredField("struct")); - return possibleField.flatMap( - field -> { - field.setAccessible(true); - if (Struct.class.isAssignableFrom(field.getType())) { - try { - return Optional.ofNullable((Struct) field.get(null)); - } catch (IllegalAccessException e) { - return Optional.empty(); - } - } else { - return Optional.empty(); - } - }); - } catch (NoSuchFieldException e) { - return Optional.empty(); - } - } - - /** - * Returns a {@link Struct} for the given class. This does not do compile time checking that the - * class is a {@link StructSerializable}. Whenever possible it is reccomended to use {@link - * #extractClassStruct(Class)}. - * - * @param clazz The class object to extract the struct from. - * @return An optional containing the struct if it could be extracted. - */ - @SuppressWarnings("unchecked") - public static Optional> extractClassStructDynamic(Class clazz) { - if (StructSerializable.class.isAssignableFrom(clazz)) { - return extractClassStruct((Class) clazz).map(struct -> struct); - } else { - return Optional.empty(); - } - } - - /** - * Returns a byte array of the given size with the given string at the beginning. If the string is - * longer than the size, it will be truncated. If the string is shorter than the size, the rest of - * the array will be filled with spaces. - * - *

The byte array is encoded in US-ASCII. - * - * @param str The string to put in the byte array. - * @param size The size of the byte array. - * @return The byte array. - */ - public static byte[] fixedSizeString(String str, int size) { - // get the ascii value for a space character - byte[] bytes = new byte[size]; - Charset charset = Charset.forName("us-ascii"); - byte whitespace = charset.encode(" ").get(); - byte[] strBytes = str.getBytes(charset); - Arrays.fill(bytes, whitespace); - System.arraycopy(strBytes, 0, bytes, 0, Math.min(strBytes.length, size)); - return bytes; - } - - @Retention(RetentionPolicy.RUNTIME) - @Target({ElementType.FIELD, ElementType.RECORD_COMPONENT}) - @Documented - public @interface IgnoreStructField {} - - @Retention(RetentionPolicy.RUNTIME) - @Target({ElementType.FIELD, ElementType.RECORD_COMPONENT}) - @Documented - public @interface FixedSizeArray { - int size(); - } - - private static OptionalInt arraySize(AnnotatedElement field) { - return Optional.ofNullable(field.getAnnotation(FixedSizeArray.class)) - .map(FixedSizeArray::size) - .map(OptionalInt::of) - .orElse(OptionalInt.empty()); - } - - private static boolean shouldIgnore(AnnotatedElement field) { - return field.isAnnotationPresent(IgnoreStructField.class); - } - - private record StructField( - String name, - String type, - int size, - boolean immutable, - Set> structsToLoad, - Unpacker unpacker, - Packer packer) { - - public static StructField fromField(Field field) { - return StructField.fromNameAndClass( - field.getName(), - field.getType(), - arraySize(field), - Modifier.isFinal(field.getModifiers())); - } - - public static StructField fromRecordComponent(RecordComponent component) { - return StructField.fromNameAndClass( - component.getName(), component.getType(), arraySize(component), true); - } - - @SuppressWarnings("unchecked") - public static StructField fromNameAndClass( - String name, Class clazz, OptionalInt arraySize, boolean isFinal) { - if (!isFixedSize(clazz, arraySize)) { - return null; - } - if (clazz.isArray() && arraySize.isPresent()) { - final Class componentType = clazz.getComponentType(); - final int size = arraySize.getAsInt(); - final StructField componentField = - fromNameAndClass( - componentType.getSimpleName(), componentType, OptionalInt.empty(), false); - return new StructField( - name + "[" + size + "]", - componentField.type, - componentField.size * size, - isFinal, - componentField.structsToLoad, - buffer -> { - Object[] array = new Object[size]; - for (int i = 0; i < size; i++) { - array[i] = componentField.unpacker.unpack(buffer); - } - return array; - }, - (buffer, value) -> { - for (Object obj : (Object[]) value) { - ((Packer) componentField.packer).pack(buffer, obj); - } - return buffer; - }); - } else if (Measure.class.isAssignableFrom(clazz)) { - return new StructField( - name, - "float64", - Double.BYTES, - isFinal, - Set.of(), - buffer -> { - throw new UnsupportedOperationException("Cannot unpack Measure"); - }, - (buffer, value) -> buffer.putDouble(((Measure) value).baseUnitMagnitude())); - } else if (primitiveTypeMap.containsKey(clazz)) { - PrimType primType = primitiveTypeMap.get(clazz); - return new StructField( - name, - primType.name, - primType.size, - isFinal, - Set.of(), - primType.unpacker, - primType.packer); - } else { - Struct struct = null; - if (customStructTypeMap.containsKey(clazz)) { - struct = customStructTypeMap.get(clazz); - } else if (StructSerializable.class.isAssignableFrom(clazz)) { - struct = extractClassStructDynamic(clazz).orElse(null); - } - if (struct == null) { - RuntimeLog.warn("Could not structify field: " + name); - return null; - } - Set> structsToLoad = new HashSet<>(); - for (Struct nestedStruct : struct.getNested()) { - // `Set.of` crashes on duplicate elements - if (structsToLoad.contains(nestedStruct)) { - continue; - } - structsToLoad.add(nestedStruct); - } - structsToLoad.add(struct); - return new StructField( - name, - struct.getTypeName(), - struct.getSize(), - struct.isImmutable() && isFinal, - structsToLoad, - struct::unpack, - Packer.fromStruct(struct)); - } - } - } - - /** - * Introspects a class to determine if it's a fixed size. - * - *

Fixed size means no collections, no strings, no arrays, etc. - * - * @param clazz The class to introspect. - * @return Whether the class is fixed size. - */ - public static boolean isFixedSize(Class clazz, OptionalInt arraySize) { - if (clazz.isArray()) { - if (arraySize.isEmpty()) { - return false; - } else { - Class componentType = clazz.getComponentType(); - return isFixedSize(componentType, OptionalInt.empty()); - } - } else if (clazz.isRecord()) { - for (RecordComponent component : clazz.getRecordComponents()) { - if (!isFixedSize(component.getType(), arraySize(component))) { - return false; - } - } - } else { - for (Field field : clazz.getDeclaredFields()) { - Class fieldClass = field.getType(); - if (field.isEnumConstant() || Modifier.isStatic(field.getModifiers())) { - continue; - } - if (Collection.class.isAssignableFrom(fieldClass) - || Iterator.class.isAssignableFrom(fieldClass) - || Iterable.class.isAssignableFrom(fieldClass) - || BaseStream.class.isAssignableFrom(fieldClass) - || fieldClass.isArray() - || fieldClass == String.class - || fieldClass == Optional.class) { - return false; - } - if (!primitiveTypeMap.containsKey(fieldClass) - && !isFixedSize(fieldClass, arraySize(field))) { - return false; - } - } - } - return true; - } - - /** - * Introspects a class to determine if it's interiorly mutable. - * - *

Interior mutability means that the class has fields that are mutable. - * - * @param clazz The class to introspect. - * @return Whether the class is interiorly mutable. - */ - public static boolean isInteriorlyMutable(Class clazz) { - if (clazz.isArray()) { - return true; - } else if (clazz.isRecord()) { - for (RecordComponent component : clazz.getRecordComponents()) { - if (isInteriorlyMutable(component.getType())) { - return true; - } - } - } else { - for (Field field : clazz.getDeclaredFields()) { - if (field.isEnumConstant() || Modifier.isStatic(field.getModifiers())) { - continue; - } - if (!Modifier.isFinal(field.getModifiers())) { - return true; - } - if (!primitiveTypeMap.containsKey(field.getType()) - && isInteriorlyMutable(field.getType())) { - return true; - } - } - } - return false; - } - - /** A utility for building schema syntax in a procedural manner. */ - @SuppressWarnings("PMD.AvoidStringBufferField") - public static class SchemaBuilder { - /** A utility for building enum fields in a procedural manner. */ - public static final class EnumFieldBuilder { - private final StringBuilder m_builder = new StringBuilder(); - private final String m_fieldName; - private boolean m_firstVariant = true; - - /** - * Creates a new enum field builder. - * - * @param fieldName The name of the field. - */ - public EnumFieldBuilder(String fieldName) { - this.m_fieldName = fieldName; - m_builder.append("enum {"); - } - - /** - * Adds a variant to the enum field. - * - * @param name The name of the variant. - * @param value The value of the variant. - * @return The builder for chaining. - */ - public EnumFieldBuilder addVariant(String name, int value) { - if (!m_firstVariant) { - m_builder.append(','); - } - m_firstVariant = false; - m_builder.append(name).append('=').append(value); - return this; - } - - /** - * Builds the enum field. If this object is being used with {@link SchemaBuilder#addEnumField} - * then {@link #build()} does not have to be called by the user. - * - * @return The built enum field. - */ - public String build() { - m_builder.append("} int8 ").append(m_fieldName).append(';'); - return m_builder.toString(); - } - } - - /** Creates a new schema builder. */ - public SchemaBuilder() {} - - private final StringBuilder m_builder = new StringBuilder(); - - /** - * Adds a field to the schema. - * - * @param name The name of the field. - * @param type The type of the field. - * @return The builder for chaining. - */ - public SchemaBuilder addField(StructField field) { - m_builder.append(field.type).append(' ').append(field.name).append(';'); - return this; - } - - /** - * Adds an inline enum field to the schema. - * - * @param enumFieldBuilder The builder for the enum field. - * @return The builder for chaining. - */ - public SchemaBuilder addEnumField(EnumFieldBuilder enumFieldBuilder) { - m_builder.append(enumFieldBuilder.build()); - return this; - } - - /** - * Builds the schema. - * - * @return The built schema. - */ - public String build() { - return m_builder.toString(); - } - } - - public static Struct noopStruct(Class cls) { - return new Struct<>() { - @Override - public Class getTypeClass() { - return cls; - } - - @Override - public String getTypeName() { - return cls.getSimpleName(); - } - - @Override - public String getSchema() { - return ""; - } - - @Override - public int getSize() { - return 0; - } - - @Override - public void pack(ByteBuffer buffer, T value) {} - - @Override - public T unpack(ByteBuffer buffer) { - return null; - } - }; - } - - private abstract static class ProcStruct implements Struct { - protected final Class typeClass; - protected final List fields; - private final String schema; - - // stored values so we never recompute them - private final int size; - private final boolean isImmutable; - private final Struct[] nested; - - public ProcStruct(Class typeClass, List fields, String schema) { - this.typeClass = typeClass; - this.fields = fields; - this.schema = schema; - - this.size = fields.stream().mapToInt(StructField::size).sum(); - this.isImmutable = fields.stream().allMatch(StructField::immutable); - this.nested = - fields.stream() - .map(StructField::structsToLoad) - .flatMap(Collection::stream) - .toArray(Struct[]::new); - - ProceduralStructGenerator.customStructTypeMap.put(typeClass, this); - } - - @Override - public Class getTypeClass() { - return typeClass; - } - - @Override - public String getTypeName() { - return typeClass.getSimpleName(); - } - - @Override - public String getSchema() { - return schema; - } - - @Override - public int getSize() { - return size; - } - - @Override - public boolean isCloneable() { - return Cloneable.class.isAssignableFrom(typeClass); - } - - @Override - @SuppressWarnings("unchecked") - public T clone(T obj) throws CloneNotSupportedException { - if (isCloneable()) { - try { - return (T) typeClass.getMethod("clone").invoke(obj); - } catch (IllegalAccessException | InvocationTargetException | NoSuchMethodException e) { - throw new CloneNotSupportedException(); - } - } else { - throw new CloneNotSupportedException(); - } - } - - @Override - public boolean isImmutable() { - return isImmutable; - } - - @Override - public Struct[] getNested() { - return nested; - } - - @Override - public String toString() { - return this.getTypeName() + "<" + this.getSize() + ">" + " {" + this.schema + "}"; - } - } - - /** - * Generates a {@link Struct} for the given {@link Record} class. If a {@link Struct} cannot be - * generated from the {@link Record}, the errors encountered will be printed and a no-op {@link - * Struct} will be returned. - * - * @param The type of the record. - * @param recordClass The class of the record. - * @return The generated struct. - */ - @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) - public static Struct genRecord(final Class recordClass) { - final RecordComponent[] components = recordClass.getRecordComponents(); - final SchemaBuilder schemaBuilder = new SchemaBuilder(); - final ArrayList fields = new ArrayList<>(); - - for (final RecordComponent component : components) { - if (shouldIgnore(component)) { - continue; - } - component.getAccessor().setAccessible(true); - fields.add(StructField.fromRecordComponent(component)); - } - - if (fields.stream().anyMatch(f -> f == null)) { - return noopStruct(recordClass); - } - fields.forEach(schemaBuilder::addField); - - return new ProcStruct<>(recordClass, fields, schemaBuilder.build()) { - @Override - public void pack(ByteBuffer buffer, R value) { - boolean failed = false; - int startingPosition = buffer.position(); - for (int i = 0; i < components.length; i++) { - if (fields.get(i).packer() instanceof DoublePacker doublePacker) { - try { - double d = (double) components[i].getAccessor().invoke(value); - doublePacker.packDouble(buffer, d); - continue; - } catch (IllegalAccessException - | IllegalArgumentException - | InvocationTargetException e) { - RuntimeLog.warn( - "Could not pack record component: " - + recordClass.getSimpleName() - + "#" - + components[i].getName() - + "\n " - + e.getMessage()); - failed = true; - break; - } - } - Packer packer = (Packer) fields.get(i).packer(); - try { - Object componentValue = components[i].getAccessor().invoke(value); - if (componentValue == null) { - throw new IllegalArgumentException("Component is null"); - } - packer.pack(buffer, componentValue); - } catch (IllegalAccessException - | IllegalArgumentException - | InvocationTargetException e) { - RuntimeLog.warn( - "Could not pack record component: " - + recordClass.getSimpleName() - + "#" - + components[i].getName() - + "\n " - + e.getMessage()); - failed = true; - break; - } - } - if (failed) { - buffer.put(startingPosition, new byte[this.getSize()]); - } - } - - @Override - public R unpack(ByteBuffer buffer) { - try { - Object[] args = new Object[components.length]; - Class[] argTypes = new Class[components.length]; - for (int i = 0; i < components.length; i++) { - args[i] = fields.get(i).unpacker().unpack(buffer); - argTypes[i] = components[i].getType(); - } - return recordClass.getConstructor(argTypes).newInstance(args); - } catch (InstantiationException - | IllegalAccessException - | InvocationTargetException - | NoSuchMethodException - | SecurityException e) { - System.err.println( - "Could not unpack record: " - + recordClass.getSimpleName() - + "\n " - + e.getMessage()); - return null; - } - } - }; - } - - /** - * Generates a {@link Struct} for the given {@link Enum} class. If a {@link Struct} cannot be - * generated from the {@link Enum}, the errors encountered will be printed and a no-op {@link - * Struct} will be returned. - * - * @param The type of the enum. - * @param enumClass The class of the enum. - * @return The generated struct. - */ - @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) - public static > Struct genEnum(Class enumClass) { - final E[] enumVariants = enumClass.getEnumConstants(); - final Field[] allEnumFields = enumClass.getDeclaredFields(); - final SchemaBuilder schemaBuilder = new SchemaBuilder(); - final SchemaBuilder.EnumFieldBuilder enumFieldBuilder = - new SchemaBuilder.EnumFieldBuilder("variant"); - final HashMap enumMap = new HashMap<>(); - final ArrayList fields = new ArrayList<>(); - - if (enumVariants == null || enumVariants.length == 0) { - RuntimeLog.warn( - "Could not structify enum: " + enumClass.getSimpleName() + "\n Enum has no constants"); - return noopStruct(enumClass); - } - - for (final E constant : enumVariants) { - final String name = constant.name(); - final int ordinal = constant.ordinal(); - - enumFieldBuilder.addVariant(name, ordinal); - enumMap.put(ordinal, constant); - } - schemaBuilder.addEnumField(enumFieldBuilder); - fields.add( - new StructField( - "variant", - "int8", - 1, - true, - Set.of(), - ByteBuffer::get, - (buffer, value) -> buffer.put((byte) ((Enum) value).ordinal()))); - - final List enumFields = - List.of(allEnumFields).stream() - .filter( - f -> - !f.isEnumConstant() && !Modifier.isStatic(f.getModifiers()) && !shouldIgnore(f)) - .toList(); - - for (final Field field : enumFields) { - field.setAccessible(true); - fields.add(StructField.fromField(field)); - } - if (fields.stream().anyMatch(f -> f == null)) { - return noopStruct(enumClass); - } - for (int i = 1; i < fields.size(); i++) { - // do this to skip the variant field - schemaBuilder.addField(fields.get(i)); - } - - return new ProcStruct<>(enumClass, fields, schemaBuilder.build()) { - @Override - public void pack(ByteBuffer buffer, E value) { - boolean failed = false; - int startingPosition = buffer.position(); - buffer.put((byte) value.ordinal()); - for (int i = 0; i < enumFields.size(); i++) { - Packer packer = (Packer) fields.get(i + 1).packer(); - Field field = enumFields.get(i); - try { - Object fieldValue = field.get(value); - if (fieldValue == null) { - throw new IllegalArgumentException("Field is null"); - } - packer.pack(buffer, fieldValue); - } catch (IllegalArgumentException | IllegalAccessException e) { - System.err.println( - "Could not pack enum field: " - + enumClass.getSimpleName() - + "#" - + field.getName() - + "\n " - + e.getMessage()); - failed = true; - break; - } - } - if (failed) { - buffer.put(startingPosition, new byte[this.getSize()]); - } - } - - final byte[] m_spongeBuffer = new byte[this.getSize() - 1]; - - @Override - public E unpack(ByteBuffer buffer) { - int ordinal = buffer.get(); - buffer.get(m_spongeBuffer); - return enumMap.getOrDefault(ordinal, null); - } - - public boolean isCloneable() { - return true; - } - ; - - public E clone(E obj) throws CloneNotSupportedException { - return obj; - } - ; - - public boolean isImmutable() { - return true; - } - ; - }; - } - - /** - * Generates a {@link Struct} for the given {@link Object} class. If a {@link Struct} cannot be - * generated from the {@link Object}, the errors encountered will be printed and a no-op {@link - * Struct} will be returned. - * - * @param The type of the object. - * @param objectClass The class of the object. - * @param objectSupplier A supplier for the object. - * @return The generated struct. - */ - @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) - public static Struct genObject(Class objectClass, Supplier objectSupplier) { - final SchemaBuilder schemaBuilder = new SchemaBuilder(); - final Field[] allFields = - List.of(objectClass.getDeclaredFields()).stream() - .filter(f -> !shouldIgnore(f) && !Modifier.isStatic(f.getModifiers())) - .toArray(Field[]::new); - final ArrayList fields = new ArrayList<>(allFields.length); - - final Optional> parentStruct; - if (StructSerializable.class.isAssignableFrom(objectClass.getSuperclass())) { - parentStruct = extractClassStructDynamic(objectClass.getSuperclass()); - } else { - parentStruct = Optional.empty(); - } - - for (final Field field : allFields) { - field.setAccessible(true); - fields.add(StructField.fromField(field)); - } - - if (fields.stream().anyMatch(f -> f == null)) { - return noopStruct(objectClass); - } - fields.forEach(schemaBuilder::addField); - - parentStruct.ifPresent( - struct -> { - fields.add( - new StructField( - "parent", - struct.getTypeName(), - struct.getSize(), - struct.isImmutable(), - Set.of(struct.getNested()), - buffer -> struct.unpack(buffer), - Packer.fromStruct(struct))); - }); - - return new ProcStruct<>( - objectClass, - fields, - schemaBuilder.build() + parentStruct.map(Struct::getSchema).orElse("")) { - @Override - public void pack(ByteBuffer buffer, O value) { - boolean failed = false; - int startingPosition = buffer.position(); - for (int i = 0; i < allFields.length; i++) { - if (fields.get(i).packer() instanceof DoublePacker doublePacker) { - try { - doublePacker.packDouble(buffer, allFields[i].getDouble(value)); - } catch (IllegalArgumentException | IllegalAccessException e) { - System.err.println( - "Could not pack object field: " - + objectClass.getSimpleName() - + "#" - + allFields[i].getName() - + "\n " - + e.getMessage()); - failed = true; - break; - } - continue; - } - Packer packer = (Packer) fields.get(i).packer(); - try { - Object fieldValue = allFields[i].get(value); - if (fieldValue == null) { - throw new IllegalArgumentException("Field is null"); - } - packer.pack(buffer, fieldValue); - } catch (IllegalArgumentException | IllegalAccessException e) { - System.err.println( - "Could not pack object field: " - + objectClass.getSimpleName() - + "#" - + allFields[i].getName() - + "\n " - + e.getMessage()); - failed = true; - break; - } - } - if (failed) { - buffer.put(startingPosition, new byte[this.getSize()]); - } - } - - @Override - public O unpack(ByteBuffer buffer) { - try { - O obj = objectSupplier.get(); - for (int i = 0; i < allFields.length; i++) { - Object fieldValue = fields.get(i).unpacker().unpack(buffer); - allFields[i].set(obj, fieldValue); - } - return obj; - } catch (IllegalArgumentException | IllegalAccessException e) { - System.err.println( - "Could not unpack object: " - + objectClass.getSimpleName() - + "\n " - + e.getMessage()); - return null; - } - } - }; - } - - /** - * Generates a {@link Struct} for the given {@link Object} class. If a {@link Struct} cannot be - * generated from the {@link Object}, the errors encountered will be printed and a no-op {@link - * Struct} will be returned. - * - * @param The type of the object. - * @param objectClass The class of the object. - * @return The generated struct. - */ - @SuppressWarnings("PMD.AvoidAccessibilityAlteration") - public static Struct genObjectNoUnpack(Class objectClass) { - return genObject(objectClass, null); - } - - public static class SerdePair extends Pair { - @Retention(RetentionPolicy.RUNTIME) - @Target({ElementType.FIELD, ElementType.RECORD_COMPONENT}) - @Documented - public @interface SerdePairHint { - Class a(); - - Class b(); - } - - private SerdePair(A a, B b) { - super(a, b); - } - - public static SerdePair of( - A a, B b) { - return new SerdePair<>(a, b); - } - - public static , B extends Measure> SerdePair ofMeasure(A a, B b) { - return new SerdePair<>(a, b); - } - } -} diff --git a/src/main/java/monologue/RuntimeLog.java b/src/main/java/monologue/RuntimeLog.java deleted file mode 100644 index a6a9786..0000000 --- a/src/main/java/monologue/RuntimeLog.java +++ /dev/null @@ -1,35 +0,0 @@ -package monologue; - -import edu.wpi.first.hal.DriverStationJNI; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringPublisher; - -class RuntimeLog { - private static final StringPublisher entry; - - static { - // we need to make sure we never log network tables through the implicit wpilib logger - entry = NetworkTableInstance.getDefault().getStringTopic("/MonologueSetup").publish(); - info("Monologue Setup Logger initialized"); - } - - private static class MonologueRuntimeError extends RuntimeException { - MonologueRuntimeError(String message) { - super(message); - } - } - - public static void info(String message) { - entry.set("[Monologue] " + message); - } - - public static void warn(String warning) { - if (Monologue.shouldThrowOnWarn()) { - throw new MonologueRuntimeError("[Monologue] " + warning); - } else { - String message = "[Monologue] (WARNING) " + warning; - entry.set(message); - DriverStationJNI.sendError(false, 1, false, message, "", "", true); - } - } -} diff --git a/src/main/java/monologue/TimeSensitiveLogger.java b/src/main/java/monologue/TimeSensitiveLogger.java deleted file mode 100644 index 077a8e4..0000000 --- a/src/main/java/monologue/TimeSensitiveLogger.java +++ /dev/null @@ -1,32 +0,0 @@ -package monologue; - -import edu.wpi.first.util.datalog.DoubleLogEntry; -import edu.wpi.first.wpilibj.DataLogManager; - -public class TimeSensitiveLogger { - public record TimestampedDouble(double value, double timestamp) {} - - private final DoubleLogEntry logEntry; - - public TimeSensitiveLogger(String key) { - logEntry = new DoubleLogEntry(DataLogManager.getLog(), key); - } - - private long secondsToMicros(double seconds) { - return (long) (seconds * 1_000_000); - } - - public void log(double value, double timestampSeconds) { - logEntry.append(value, secondsToMicros(timestampSeconds)); - } - - public void log(TimestampedDouble value) { - log(value.value, value.timestamp); - } - - public void log(TimestampedDouble... values) { - for (TimestampedDouble value : values) { - log(value); - } - } -} diff --git a/src/main/java/monologue/TypeChecker.java b/src/main/java/monologue/TypeChecker.java deleted file mode 100644 index 985b650..0000000 --- a/src/main/java/monologue/TypeChecker.java +++ /dev/null @@ -1,53 +0,0 @@ -package monologue; - -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.struct.StructSerializable; - -class TypeChecker { - private static final Class[] LITERAL_TYPES = { - boolean.class, - int.class, - long.class, - float.class, - double.class, - Boolean.class, - Integer.class, - Long.class, - Float.class, - Double.class, - String.class, - StructSerializable.class - }; - - private static final Class[] EXTENDABLE_TYPES = {Sendable.class}; - - static boolean isValidLiteralType(Class type) { - for (Class literalType : LITERAL_TYPES) { - if (literalType.isAssignableFrom(type)) { - return true; - } - } - return false; - } - - static boolean isValidExtendableType(Class type) { - for (Class extendableType : EXTENDABLE_TYPES) { - if (extendableType.isAssignableFrom(type)) { - return true; - } - } - return false; - } - - static boolean isValidType(Class type) { - Class et = type.isArray() ? type.getComponentType() : type; - return isValidLiteralType(et) || isValidExtendableType(et); - } - - static enum TypeRoutes { - DOUBLE, - INTEGER, - BOOLEAN, - OBJECT - } -} diff --git a/src/main/java/wpilibExt/MeasureMath.java b/src/main/java/wpilibExt/MeasureMath.java deleted file mode 100644 index 973ba1e..0000000 --- a/src/main/java/wpilibExt/MeasureMath.java +++ /dev/null @@ -1,232 +0,0 @@ -package wpilibExt; - -import static edu.wpi.first.units.Units.KilogramSquareMeters; -import static edu.wpi.first.units.Units.Kilograms; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.NewtonMeters; -import static edu.wpi.first.units.Units.Newtons; -import static edu.wpi.first.units.Units.Radian; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; -import static edu.wpi.first.units.Units.Second; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.units.AccelerationUnit; -import edu.wpi.first.units.AngleUnit; -import edu.wpi.first.units.DistanceUnit; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.Unit; -import edu.wpi.first.units.VelocityUnit; -import edu.wpi.first.units.measure.Acceleration; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularAcceleration; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.Force; -import edu.wpi.first.units.measure.LinearAcceleration; -import edu.wpi.first.units.measure.Mass; -import edu.wpi.first.units.measure.MomentOfInertia; -import edu.wpi.first.units.measure.Mult; -import edu.wpi.first.units.measure.Torque; -import edu.wpi.first.units.measure.Velocity; -import edu.wpi.first.util.struct.Struct; -import java.util.function.BiFunction; -import monologue.ProceduralStructGenerator; - -public class MeasureMath { - - public static final class Constants { - public static final Angle kPi = Radian.of(Math.PI); - public static final Angle kCCW_2Pi = Radian.of(2.0 * Math.PI); - public static final Angle kCW_2Pi = Radian.of(-2.0 * Math.PI); - - public static final LinearAcceleration kGravity = MetersPerSecondPerSecond.of(9.8); - } - - @SuppressWarnings("unchecked") - public static > M abs(M m) { - return (M) m.unit().ofBaseUnits(Math.abs(m.baseUnitMagnitude())); - } - - @SuppressWarnings("unchecked") - public static > M negate(M m) { - return (M) m.unit().ofBaseUnits(-m.baseUnitMagnitude()); - } - - public static > M max(M m1, M m2) { - return m1.baseUnitMagnitude() > m2.baseUnitMagnitude() ? m1 : m2; - } - - @SuppressWarnings("unchecked") - public static > M max(M m1, M m2, M... otherMs) { - M max = max(m1, m2); - for (M m : otherMs) { - max = max(max, m); - } - return max; - } - - public static > M maxAbs(M m1, M m2) { - return abs(m1).baseUnitMagnitude() > abs(m2).baseUnitMagnitude() ? m1 : m2; - } - - @SuppressWarnings("unchecked") - public static > M maxAbs(M m1, M m2, M... otherMs) { - M max = maxAbs(m1, m2); - for (M m : otherMs) { - max = max(max, m); - } - return max; - } - - public static > M min(M m1, M m2) { - return m1.baseUnitMagnitude() < m2.baseUnitMagnitude() ? m1 : m2; - } - - @SuppressWarnings("unchecked") - public static > M min(M m1, M m2, M... otherMs) { - M min = min(m1, m2); - for (M m : otherMs) { - min = min(min, m); - } - return min; - } - - public static > M minAbs(M m1, M m2) { - return abs(m1).baseUnitMagnitude() < abs(m2).baseUnitMagnitude() ? m1 : m2; - } - - @SuppressWarnings("unchecked") - public static > M minAbs(M m1, M m2, M... otherMs) { - M min = minAbs(m1, m2); - for (M m : otherMs) { - min = min(min, m); - } - return min; - } - - public static > M clamp(M m, M min, M max) { - if (min.gt(max)) { - throw new IllegalArgumentException("min must be less than or equal to max"); - } - return max(min, min(max, m)); - } - - public static > M clamp(M m, M magnitudeLimit) { - return max(negate(abs(magnitudeLimit)), min(abs(magnitudeLimit), m)); - } - - public static > double signum(M m) { - return Math.signum(m.baseUnitMagnitude()); - } - - @SuppressWarnings("unchecked") - public static > M nudgeZero(M m, M tolerance) { - return abs(m).baseUnitMagnitude() < tolerance.baseUnitMagnitude() ? (M) m.unit().zero() : m; - } - - public record XY>(M x, M y) { - public static XY of(Translation2d t) { - return new XY<>(Meters.of(t.getX()), Meters.of(t.getY())); - } - - public static > XY of(M x, M y) { - return new XY<>(x, y); - } - - @SuppressWarnings("unchecked") - public static > XY of(M mag, Rotation2d rot) { - return new XY<>((M) mag.times(rot.getCos()), (M) mag.times(rot.getSin())); - } - - @SuppressWarnings("unchecked") - public static XY> zero(U unit) { - return new XY<>((Measure) unit.zero(), (Measure) unit.zero()); - } - - public , R extends Measure> N cross( - XY rhs, Class cls, BiFunction f) { - var a = f.apply(x, rhs.y); - var b = f.apply(y, rhs.x); - return cls.cast(a.minus(b)); - } - - @SuppressWarnings("unchecked") - public M magnitude() { - return (M) x.unit().ofBaseUnits(Math.hypot(x.baseUnitMagnitude(), y.baseUnitMagnitude())); - } - - @SuppressWarnings("unchecked") - public XY normalize() { - M mag = magnitude(); - return new XY<>((M) x.div(mag), (M) y.div(mag)); - } - - @SuppressWarnings("unchecked") - public XY times(M scalar) { - return new XY<>((M) x.times(scalar), (M) y.times(scalar)); - } - - public XY desaturate(M maxMagnitude) { - if (magnitude().baseUnitMagnitude() <= maxMagnitude.baseUnitMagnitude()) { - return this; - } - return normalize().times(maxMagnitude); - } - - @SuppressWarnings("rawtypes") - public static final Struct struct = ProceduralStructGenerator.genRecord(XY.class); - } - - public static Torque times(Distance d, Force f) { - return NewtonMeters.of(d.in(Meters) * f.in(Newtons)); - } - - public static Torque times(Force f, Distance d) { - return NewtonMeters.of(d.in(Meters) * f.in(Newtons)); - } - - // https://openstax.org/books/university-physics-volume-1/pages/10-7-newtons-second-law-for-rotation - public static AngularAcceleration div(Torque t, MomentOfInertia moi) { - return RadiansPerSecondPerSecond.of(t.in(NewtonMeters) / moi.in(KilogramSquareMeters)); - } - - public static Force times(LinearAcceleration a, Mass m) { - return Newtons.of(m.in(Kilograms) * a.in(MetersPerSecondPerSecond)); - } - - public static Torque times(AngularAcceleration a, MomentOfInertia moi) { - return NewtonMeters.of(moi.in(KilogramSquareMeters) * a.in(RadiansPerSecondPerSecond)); - } - - public static LinearAcceleration times(AngularAcceleration a, Distance d) { - return MetersPerSecondPerSecond.of(a.in(RadiansPerSecondPerSecond) * d.in(Meters)); - } - - public static MomentOfInertia times(Mass m, Mult d) { - return KilogramSquareMeters.of(m.in(Kilograms) * d.baseUnitMagnitude()); - } - - public static Mass div(MomentOfInertia moi, Mult d) { - return Kilograms.of(moi.in(KilogramSquareMeters) / d.baseUnitMagnitude()); - } - - public static Velocity genericize(AngularVelocity av) { - return VelocityUnit.combine(Radian, Second).of(av.in(RadiansPerSecond)); - } - - public static Acceleration genericize(AngularAcceleration aa) { - return AccelerationUnit.combine(VelocityUnit.combine(Radian, Second), Second) - .of(aa.in(RadiansPerSecondPerSecond)); - } - - @SuppressWarnings("unchecked") - public static > M zeroIfNAN(M m) { - if (Double.isNaN(m.baseUnitMagnitude())) { - return (M) m.unit().zero(); - } - return m; - } -} diff --git a/src/main/java/wpilibExt/Speeds.java b/src/main/java/wpilibExt/Speeds.java deleted file mode 100644 index 09ec9ee..0000000 --- a/src/main/java/wpilibExt/Speeds.java +++ /dev/null @@ -1,266 +0,0 @@ -package wpilibExt; - -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.util.struct.Struct; -import edu.wpi.first.util.struct.StructSerializable; -import java.nio.ByteBuffer; -import monologue.ProceduralStructGenerator; - -/** - * An interface to add type safety to the frame of reference of speeds. - * - *
    - *
  • Field relative: The speeds are relative to the field coordinate system. - *
  • Robot relative: The speeds are relative to the robot coordinate system. - *
- */ -public sealed interface Speeds extends StructSerializable { - /** - * Speeds in the field coordinate system. - * - *
    - *
  • Positive x is towards the away from the blue alliance wall. - *
  • Positive y is left from the perspective of a blue alliance driver. - *
  • Positive omega is counter-clockwise. - *
  • (0, 0) is the corner between the blue alliance wall and the wall adjacent in the -y - * direction. - *
- */ - public record FieldSpeeds(double vx, double vy, double omega) implements Speeds { - @Override - public FieldSpeeds asFieldRelative(Rotation2d robotAngle) { - return this; - } - - @Override - public RobotSpeeds asRobotRelative(Rotation2d robotAngle) { - return new RobotSpeeds( - vx * robotAngle.getCos() + vy * robotAngle.getSin(), - -vx * robotAngle.getSin() + vy * robotAngle.getCos(), - omega); - } - - @Override - public ChassisSpeeds toWpilib() { - return new ChassisSpeeds(vx, vy, omega); - } - - public LinearVelocity vxMeasure() { - return MetersPerSecond.of(vx()); - } - - public LinearVelocity vyMeasure() { - return MetersPerSecond.of(vy()); - } - - public AngularVelocity omegaMeasure() { - return RadiansPerSecond.of(omega()); - } - - @Override - public FieldSpeeds discretize(double dtSeconds) { - return Internals.discretizePrim(vx, vy, omega, dtSeconds, FieldSpeeds::new); - } - - public static final FieldSpeeds kZero = new FieldSpeeds(0.0, 0.0, 0.0); - - public static final SpeedsStruct struct = new SpeedsStruct(); - } - - /** - * Speeds in the robot coordinate system. - * - *
    - *
  • Positive x is forward. - *
  • Positive y is left. - *
  • Positive omega is counter-clockwise. - *
  • (0, 0) is the center of the robot. - *
- */ - public record RobotSpeeds(double vx, double vy, double omega) implements Speeds { - @Override - public FieldSpeeds asFieldRelative(Rotation2d robotAngle) { - return new FieldSpeeds( - vx * robotAngle.getCos() - vy * robotAngle.getSin(), - vx * robotAngle.getSin() + vy * robotAngle.getCos(), - omega); - } - - @Override - public RobotSpeeds asRobotRelative(Rotation2d robotAngle) { - return this; - } - - @Override - public ChassisSpeeds toWpilib() { - return new ChassisSpeeds(vx, vy, omega); - } - - public LinearVelocity vxMeasure() { - return MetersPerSecond.of(vx()); - } - - public LinearVelocity vyMeasure() { - return MetersPerSecond.of(vy()); - } - - public AngularVelocity omegaMeasure() { - return RadiansPerSecond.of(omega()); - } - - @Override - public RobotSpeeds discretize(double dtSeconds) { - return Internals.discretizePrim(vx, vy, omega, dtSeconds, RobotSpeeds::new); - } - - public static final RobotSpeeds kZero = new RobotSpeeds(0.0, 0.0, 0.0); - - public static final SpeedsStruct struct = new SpeedsStruct(); - } - - static FieldSpeeds fromFieldRelative(double vx, double vy, double omega) { - return new FieldSpeeds(vx, vy, omega); - } - - static RobotSpeeds fromRobotRelative(double vx, double vy, double omega) { - return new RobotSpeeds(vx, vy, omega); - } - - static RobotSpeeds fromRobotRelative(ChassisSpeeds speeds) { - return new RobotSpeeds( - speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); - } - - static FieldSpeeds fromFieldRelative(ChassisSpeeds speeds) { - return new FieldSpeeds( - speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); - } - - FieldSpeeds asFieldRelative(Rotation2d robotAngle); - - RobotSpeeds asRobotRelative(Rotation2d robotAngle); - - ChassisSpeeds toWpilib(); - - Speeds discretize(double dtSeconds); - - static class Internals { - - @FunctionalInterface - private interface SpeedConstructor { - S construct(double vx, double vy, double omega); - } - - private static final S discretizePrim( - double vx, double vy, double omega, double dt, SpeedConstructor constructor) { - double cos = Pose2d.kZero.getRotation().getCos(); - double sin = Pose2d.kZero.getRotation().getSin(); - - double dx = vx * dt; - double dy = vy * dt; - double dtheta = omega * dt; - - double transformSin = Math.sin(dtheta); - double transformCos = Math.cos(dtheta); - - double s; - double c; - if (Math.abs(dtheta) < 1E-9) { - s = 1.0 - 1.0 / 6.0 * dtheta * dtheta; - c = 0.5 * dtheta; - } else { - s = transformSin / dtheta; - c = (1 - transformCos) / dtheta; - } - - double transformX = (dx * s) - (dy * c); - double transformY = (dx * c) + (dy * s); - - double rotatedX = (transformX * cos) - (transformY * sin); - double rotatedY = (transformX * sin) + (transformY * cos); - - return constructor.construct(rotatedX / dt, rotatedY / dtheta, omega); - } - } - - class SpeedsStruct implements Struct { - enum SpeedsType implements StructSerializable { - FIELD, - ROBOT; - - public static final Struct struct = - ProceduralStructGenerator.genEnum(SpeedsType.class); - } - - @Override - public Class getTypeClass() { - return Speeds.class; - } - - @Override - public String getTypeName() { - return "Speeds"; - } - - @Override - public int getSize() { - return SpeedsType.struct.getSize() + 3 * Double.BYTES; - } - - @Override - public Struct[] getNested() { - return new Struct[] {SpeedsType.struct}; - } - - @Override - public boolean isImmutable() { - return true; - } - - @Override - public void pack(ByteBuffer bb, Speeds value) { - if (value instanceof FieldSpeeds fieldSpeeds) { - SpeedsType.struct.pack(bb, SpeedsType.FIELD); - bb.putDouble(fieldSpeeds.vx()); - bb.putDouble(fieldSpeeds.vy()); - bb.putDouble(fieldSpeeds.omega()); - } else if (value instanceof RobotSpeeds robotSpeeds) { - SpeedsType.struct.pack(bb, SpeedsType.ROBOT); - bb.putDouble(robotSpeeds.vx()); - bb.putDouble(robotSpeeds.vy()); - bb.putDouble(robotSpeeds.omega()); - } else { - throw new IllegalArgumentException("Unknown Speeds type"); - } - } - - @Override - public Speeds unpack(ByteBuffer bb) { - SpeedsType type = SpeedsType.struct.unpack(bb); - switch (type) { - case FIELD: - return new FieldSpeeds(bb.getDouble(), bb.getDouble(), bb.getDouble()); - case ROBOT: - return new RobotSpeeds(bb.getDouble(), bb.getDouble(), bb.getDouble()); - default: - throw new IllegalArgumentException("Unknown Speeds type"); - } - } - - @Override - public String getSchema() { - return "SpeedsType type; double vx; double vy; double omega;"; - } - } -}

Speeds can come in two variants for different frame - * of references: - * - *