Added new logging command classes, tests

src/main/java/org/usfirst/frc4048/common/logging/CommandUtil.java

@@ -32,6 +32,18 @@
  *     Command pickUp = CommandUtil.sequence("ArmSequence", new OpenGripper(), new RaiseArm(), new CloseGripper());
  *     Command pickupAndDrive = CommandUtil.parallel("WalkWhileChewingGum", pickup, new Drive());
  * </pre>
+ *
+ * and also for nesting command groups that are custom-created:
+ * (Note the "extends SequentialLoggingCommandGroup" - DO NOT extend regular SequentialCommandGroup or this will not work)
+ * <pre>
+ *     class PickupGroup extends SequentialLoggingCommandGroup {
+ *         public PickupGroup() {
+ *             super("Pickup", new OpenGripper(), new RaiseArm(), new CloseGripper());
+ *         }
+ *     }
+ *     Command pickUp = new PickupGroup();
+ *     Command pickupAndDrive = CommandUtil.parallel("WalkWhileChewingGum", pickup, new Drive());
+ * </pre>
  */
 public class CommandUtil {
     public static final String COMMAND_PREFIX = "Commands";
@@ -52,6 +64,8 @@ public static Command logged(Command command) {
     /**
      * Make a logging command from the given command, with a custom name hierarchy.
      * The logging command will be placed within the given name prefix naming hierarchy instead of at the root.
+     * @param namePrefix the nesting hierarchy prefix for the command. If null the command is going to log at the
+     *                   root level. If not null it will be nested in this namespace.
      * @param command the command to wrap
      * @return a {@link LoggingCommand} that wraps the given command
      */
@@ -60,7 +74,7 @@ public static Command logged(String namePrefix, Command command) {
     }
 
     /**
-     * Return a logged command that runs once and stops (see {@link Commands#runOnce(Runnable, Subsystem...)}.
+     * Return a logged command that runs once and stops (see {@link edu.wpi.first.wpilibj2.command.Commands#runOnce(Runnable, Subsystem...)}.
      * This will return a loggable anonymous command with the given name.
      * @param name the name of the resulting command
      * @param action the action to perform (only once)
@@ -76,28 +90,30 @@ public static Command runOnce(String name, Runnable action, Subsystem... require
     /**
      * Return a loggable {@link SequentialCommandGroup} that executes the given commands sequentially. The given commands
      * can be any command or command group - they will be converted to loggable commands implicitly.
-     * @param sequenceName the name of the resaulting command group
+     * @param sequenceName the name of the resulting command group
      * @param commands the commands to put in the group
      * @return the created loggable command group
      */
     public static Command sequence(String sequenceName, Command... commands) {
-        LoggingCommand[] loggingCommands = wrapForLogging(sequenceName, commands);
-        return new SequentialLoggingCommand(sequenceName, loggingCommands);
+        return new SequentialLoggingCommand(sequenceName, commands);
     }
 
     /**
      * Return a loggable {@link ParallelCommandGroup} that executes the given commands in parallel. The given commands
      * can be any command or command group - they will be converted to loggable commands implicitly.
-     * @param sequenceName the name of the resaulting command group
+     * @param sequenceName the name of the resulting command group
      * @param commands the commands to put in the group
      * @return the created loggable command group
      */
     public static Command parallel(String sequenceName, Command... commands) {
-        LoggingCommand[] loggingCommands = wrapForLogging(sequenceName, commands);
-        return new ParallelLoggingCommand(sequenceName, loggingCommands);
+        return new ParallelLoggingCommand(sequenceName, commands);
+    }
+
+    public static Command race(String sequenceName, Command... commands) {
+        return new RaceLoggingCommand(sequenceName, commands);
     }
 
-    private static LoggingCommand[] wrapForLogging(String prefix, Command... commands) {
+    public static LoggingCommand[] wrapForLogging(String prefix, Command... commands) {
         // Do not use streams due to efficiency
         LoggingCommand[] newCommands = new LoggingCommand[commands.length];
         for (int i = 0; i < commands.length; i++) {
@@ -108,9 +124,7 @@ private static LoggingCommand[] wrapForLogging(String prefix, Command... command
 
     private static LoggingCommand wrapForLogging(String prefix, Command command) {
         if (command instanceof LoggingCommand loggingCommand) {
-            // change the prefix to include the current new parent
-            String childPrefix = prefix + CommandUtil.NAME_SEPARATOR + loggingCommand.getNamePrefix();
-            loggingCommand.setNamePrefix(childPrefix);
+            loggingCommand.appendNamePrefix(prefix);
             return loggingCommand;
         } else {
             // New command located in the given sequence root

src/main/java/org/usfirst/frc4048/common/logging/GroupLoggingCommand.java

@@ -0,0 +1,72 @@
+package org.usfirst.frc4048.common.logging;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Base class for logged grouped commands (parallel, race, sequential...)
+ */
+public abstract class GroupLoggingCommand extends LoggingCommand {
+    // the "leaf" name - this comes after the name prefix to make up the full command name
+    private static final String THIS_NAME = "-this";
+
+    // A copy of the children (since we can't access them from the regular groups)
+    private final List<LoggingCommand> childLoggingCommands;
+
+    /**
+     * Constructor for logged group command.
+     *
+     * @param namePrefix the prefix for the name (comes in front of hte group name)
+     * @param underlying the group command that is wrapped by this command
+     */
+    public GroupLoggingCommand(String namePrefix, Command underlying) {
+        super(namePrefix, THIS_NAME, underlying);
+        childLoggingCommands = new ArrayList<>();
+    }
+
+    /**
+     * A special constructor to allow for the creation of the command when we can't create the underlying up front.
+     * It is assumed that the {@link #setUnderlying(Command)} method is called immediately following the construction.
+     */
+    protected GroupLoggingCommand(String namePrefix) {
+        super(namePrefix, THIS_NAME);
+        childLoggingCommands = new ArrayList<>();
+    }
+
+    public final void addLoggingCommands(List<LoggingCommand> commands) {
+        childLoggingCommands.addAll(commands);
+    }
+
+    @Override
+    public void setName(String name) {
+        // Do not change the logging name for this command (it is fixed)
+        getUnderlying().setName(name);
+    }
+
+    @Override
+    public void appendNamePrefix(String prefix) {
+        // Change the name for this command
+        super.appendNamePrefix(prefix);
+        // Change the name for the children
+        appendChildrenPrefix(prefix);
+    }
+
+    @Override
+    public String toString() {
+        return getFullyQualifiedName();
+    }
+
+    // For testing
+    public List<LoggingCommand> getChildLoggingCommands() {
+        return childLoggingCommands;
+    }
+
+    private void appendChildrenPrefix(String prefix) {
+        // Recursively change the prefix for all child commands
+        for (LoggingCommand loggingCommand : childLoggingCommands) {
+            loggingCommand.appendNamePrefix(prefix);
+        }
+    }
+}

src/main/java/org/usfirst/frc4048/common/logging/LoggingCommand.java

@@ -8,10 +8,14 @@
 import java.util.Set;
 
 public abstract class LoggingCommand extends Command {
+    // Prefix for the command name (appended ahead of the command name) - this can be appended to when the command gets
+    // embedded at another command through appendNamePrefix
     private String namePrefix;
+    // Private name for the command (last in the hierarchy for this command) - this can be changed through setName
     private String commandName;
-    private final Command underlying;
-
+    // The delegate command where we refer all calls
+    private Command underlying;
+    // The name as it would be used in the logs
     private String fullyQualifiedName;
     // Lazy initialized log entry to make sure we don't create it until it is needed (and command is scheduled)
     // otherwise we get an empty line in the log visualization tool
@@ -24,6 +28,23 @@ public LoggingCommand(String namePrefix, String commandName, Command underlying)
         resetLoggingName(namePrefix, commandName);
     }
 
+    /**
+     * A special constructor to allow for the creation of the command when we can't create the underlying up front.
+     * It is assumed that the {@link #setUnderlying(Command)} method is called immediately following the construction.
+     */
+    protected LoggingCommand(String namePrefix, String commandName) {
+        this.namePrefix = namePrefix;
+        this.commandName = commandName;
+        resetLoggingName(namePrefix, commandName);
+    }
+
+    /**
+     * Second phase of initialization - set the underlying command
+     */
+    protected void setUnderlying(Command underlying) {
+        this.underlying = underlying;
+    }
+
     @Override
     public void initialize() {
         getLoggingEntry().append(true);
@@ -82,8 +103,12 @@ public String getNamePrefix() {
         return namePrefix;
     }
 
-    public void setNamePrefix(String prefix) {
-        this.namePrefix = prefix;
+    /**
+     * Add a new prefix in front of the current one
+     * @param prefix the new prefix
+     */
+    public void appendNamePrefix(String prefix) {
+        this.namePrefix = prefix + CommandUtil.NAME_SEPARATOR + namePrefix;
         resetLoggingName(namePrefix, commandName);
     }
 
@@ -111,3 +136,5 @@ private BooleanLogEntry getLoggingEntry() {
         return logEntry;
     }
 }
+
+

src/main/java/org/usfirst/frc4048/common/logging/ParallelDeadlineLoggingCommand.java

@@ -0,0 +1,41 @@
+package org.usfirst.frc4048.common.logging;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+
+import java.util.Arrays;
+
+public class ParallelDeadlineLoggingCommand extends GroupLoggingCommand {
+    private LoggingCommand deadlineLoggingCommand;
+
+    /**
+     * Constructor for parallel deadline command group.
+     *
+     * @param namePrefix the name for the group - this is where the sub-commands for this group will be nested in
+     * @param deadline   the deadline command - the one that when ends, stops other commands
+     * @param commands   the sub commands for this group (either regular commands or LoggingCommand are OK)
+     */
+    public ParallelDeadlineLoggingCommand(String namePrefix, Command deadline, Command... commands) {
+        // Since we can't initialize the deadlineGroup with empty commands, use the special constructor to pass in the
+        // underlying afterward
+        super(namePrefix);
+
+        deadlineLoggingCommand = CommandUtil.wrapForLogging(namePrefix, deadline)[0];
+        LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands);
+        setUnderlying(new ParallelDeadlineGroup(deadlineLoggingCommand, wrapped));
+        addLoggingCommands(Arrays.asList(wrapped));
+    }
+
+    public final void addCommands(Command... commands) {
+        LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands);
+        ((ParallelDeadlineGroup) getUnderlying()).addCommands(wrapped);
+        addLoggingCommands(Arrays.asList(wrapped));
+    }
+
+    @Override
+    public void appendNamePrefix(String prefix) {
+        super.appendNamePrefix(prefix);
+        // Change the name for the deadline
+        deadlineLoggingCommand.appendNamePrefix(prefix);
+    }
+}

src/main/java/org/usfirst/frc4048/common/logging/ParallelLoggingCommand.java

@@ -1,38 +1,29 @@
 package org.usfirst.frc4048.common.logging;
 
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 
-public class ParallelLoggingCommand extends LoggingCommand {
-    private static final String THIS_NAME = "-this";
-
-    private LoggingCommand[] loggingCommands;
-
-    public ParallelLoggingCommand(String namePrefix, LoggingCommand... commands) {
-        super(namePrefix, THIS_NAME, new ParallelCommandGroup(commands));
-        this.loggingCommands = commands;
-    }
-
-    @Override
-    public void setName(String name) {
-        // Do not change the logging name for this command (it is fixed)
-        getUnderlying().setName(name);
-    }
-
-    @Override
-    public void setNamePrefix(String prefix) {
-        super.setNamePrefix(prefix);
-        setChildrenPrefix(prefix);
-    }
-
-    @Override
-    public String toString() {
-        return getFullyQualifiedName();
+import java.util.Arrays;
+
+public class ParallelLoggingCommand extends GroupLoggingCommand {
+    /**
+     * Constructor for parallel command group.
+     *
+     * @param namePrefix the name for the group - this is where the sub-commands for this group will be nested in
+     * @param commands   the sub commands for this group (either regular commands or LoggingCommand are OK)
+     */
+    public ParallelLoggingCommand(String namePrefix, Command... commands) {
+        // Call super with an empty group, populate children afterward
+        super(namePrefix, new ParallelCommandGroup());
+
+        LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands);
+        ((ParallelCommandGroup) getUnderlying()).addCommands(wrapped);
+        addLoggingCommands(Arrays.asList(wrapped));
     }
 
-    private void setChildrenPrefix(String prefix) {
-        // Recursively change the prefix for all child commands
-        for (LoggingCommand loggingCommand : loggingCommands) {
-            loggingCommand.setNamePrefix(prefix);
-        }
+    public final void addCommands(Command... commands) {
+        LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands);
+        ((ParallelCommandGroup) getUnderlying()).addCommands(wrapped);
+        addLoggingCommands(Arrays.asList(wrapped));
     }
 }

src/main/java/org/usfirst/frc4048/common/logging/RaceLoggingCommand.java

@@ -0,0 +1,29 @@
+package org.usfirst.frc4048.common.logging;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+
+import java.util.Arrays;
+
+public class RaceLoggingCommand extends GroupLoggingCommand {
+    /**
+     * Constructor for race command group.
+     *
+     * @param namePrefix the name for the group - this is where the sub-commands for this group will be nested in
+     * @param commands   the sub commands for this group (either regular commands or LoggingCommand are OK)
+     */
+    public RaceLoggingCommand(String namePrefix, Command... commands) {
+        // Call super with an empty group, populate children afterward
+        super(namePrefix, new ParallelRaceGroup());
+
+        LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands);
+        ((ParallelRaceGroup) getUnderlying()).addCommands(wrapped);
+        addLoggingCommands(Arrays.asList(wrapped));
+    }
+
+    public final void addCommands(Command... commands) {
+        LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands);
+        ((ParallelRaceGroup) getUnderlying()).addCommands(wrapped);
+        addLoggingCommands(Arrays.asList(wrapped));
+    }
+}