Extend logging utility

src/main/java/frc/robot/Robot.java

@@ -16,6 +16,7 @@
 import frc.robot.commands.drivetrain.ResetGyro;
 import frc.robot.commands.drivetrain.WheelAlign;
 import frc.robot.commands.ramp.ResetRamp;
+import frc.robot.commands.sequences.StartIntakeAndFeeder;
 import frc.robot.constants.Constants;
 import frc.robot.utils.TimeoutCounter;
 import frc.robot.utils.diag.Diagnostics;

src/main/java/frc/robot/RobotContainer.java

@@ -314,6 +314,10 @@ public Command getAutoCommand() {
         return autoChooser.getAutoCommand();
     }
 
+    public Feeder getFeeder() {
+        return feeder;
+    }
+
     /**
      * Returns a boolean based on the current alliance color assigned by the FMS.
      *
@@ -328,7 +332,7 @@ public AutoChooser getAutoChooser() {
         return autoChooser;
     }
 
-    public Feeder getFeeder() {
-        return feeder;
+    public IntakeSubsystem getIntake() {
+        return intake;
     }
 }

src/main/java/frc/robot/commands/sequences/StartIntakeAndFeeder.java

@@ -1,43 +1,47 @@
 package frc.robot.commands.sequences;
 
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.deployer.LowerDeployer;
 import frc.robot.commands.deployer.RaiseDeployer;
+import frc.robot.commands.feeder.FeederBackDrive;
 import frc.robot.commands.feeder.StartFeeder;
 import frc.robot.commands.intake.StartIntake;
-import frc.robot.commands.feeder.FeederBackDrive;
 import frc.robot.commands.ramp.RampMoveAndWait;
 import frc.robot.constants.GameConstants;
 import frc.robot.subsystems.Deployer;
 import frc.robot.subsystems.Feeder;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.Ramp;
+import frc.robot.utils.logging.CommandUtil;
+import frc.robot.utils.logging.SequentialLoggingCommand;
 
 /**
  * Sequence to start intaking, this takes care of the lowering/raising the deployer,
  * as well as starting/stopping the intake and feeder.
  */
-public class StartIntakeAndFeeder extends SequentialCommandGroup{
+public class StartIntakeAndFeeder extends SequentialLoggingCommand {
+    @Override
+    public void execute() {
+        super.execute();
+    }
+
     public StartIntakeAndFeeder(Feeder feeder, IntakeSubsystem intake, Deployer deployer, Ramp ramp) {
-        addCommands(
-            new ParallelCommandGroup(
-                new LowerDeployer(deployer),
-                new RampMoveAndWait(ramp, () -> GameConstants.RAMP_POS_STOW)
-            ),
-            new ParallelRaceGroup(
-                new StartIntake(intake, 10), //intake stops by ParallelRaceGroup when note in feeder
-                new StartFeeder(feeder)
-            ),
-            new ParallelCommandGroup(
-                new RaiseDeployer(deployer),
-                new SequentialCommandGroup(
-                        new WaitCommand(GameConstants.FEEDER_WAIT_TIME_BEFORE_BACKDRIVE),
-                        new FeederBackDrive(feeder)
+        super("StartIntakeAndFeeder",
+                CommandUtil.parallel("First",
+                        new LowerDeployer(deployer),
+                        new RampMoveAndWait(ramp, () -> GameConstants.RAMP_POS_STOW)
+                ),
+                CommandUtil.race("second",
+                        new StartIntake(intake, 10), //intake stops by ParallelRaceGroup when note in feeder
+                        new StartFeeder(feeder)
+                ),
+                CommandUtil.parallel("third",
+                        new RaiseDeployer(deployer),
+                        CommandUtil.sequence("Fourth",
+                                new WaitCommand(GameConstants.FEEDER_WAIT_TIME_BEFORE_BACKDRIVE),
+                                new FeederBackDrive(feeder)
+                        )
                 )
-            )
         );
     }
 }

src/main/java/frc/robot/utils/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
      */
@@ -76,30 +90,27 @@ 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) {
-        LoggingCommand[] loggingCommands = wrapForLogging(sequenceName, commands);
-        return new RaceLoggingCommand(sequenceName, loggingCommands);
+        return new RaceLoggingCommand(sequenceName, commands);
     }
 
     public static LoggingCommand[] wrapForLogging(String prefix, Command... commands) {
@@ -113,9 +124,7 @@ public static LoggingCommand[] wrapForLogging(String prefix, Command... commands
 
     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/frc/robot/utils/logging/GroupLoggingCommand.java

@@ -0,0 +1,69 @@
+package frc.robot.utils.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 {
+    // 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, String commandName, Command underlying) {
+        super(namePrefix, commandName, 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, String commandName) {
+        super(namePrefix, commandName);
+        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/frc/robot/utils/logging/LoggingCommand.java

@@ -9,10 +9,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
@@ -25,6 +29,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() {
         if (Constants.ENABLE_LOGGING) getLoggingEntry().append(true);
@@ -83,8 +104,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);
     }
 
@@ -112,3 +137,5 @@ private BooleanLogEntry getLoggingEntry() {
         return logEntry;
     }
 }
+
+

src/main/java/frc/robot/utils/logging/ParallelDeadlineLoggingCommand.java

@@ -0,0 +1,41 @@
+package frc.robot.utils.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, "(Deadline)");
+
+        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);
+    }
+}