CommandLogger.java

package frc.robot.util;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.Subsystem;
import java.lang.reflect.Field;
import java.util.*;
import java.util.stream.Collectors;
import org.littletonrobotics.junction.Logger;

/**
 * Utility to track active commands and log their "Leaf" (deepest active child) state to the
 * Dashboard. Uses Java Reflection to peek inside Sequential and Parallel CommandGroups.
 *
 * <p>Notably this does not work with Pathplanner autos, as they utilize a different structure. <br>
 * <br>
 * This is hopefully unnecessary in 2027 with coming WPILib updates.
 */
public class CommandLogger {

  private static final CommandLogger instance = new CommandLogger();

  private final Set<Command> m_activeCommands = Collections.newSetFromMap(new IdentityHashMap<>());

  private CommandLogger() {}

  public static CommandLogger getInstance() {
    return instance;
  }

  /** Call this in robotInit() to hook into the CommandScheduler. */
  public void init() {
    CommandScheduler.getInstance().onCommandInitialize(m_activeCommands::add);
    CommandScheduler.getInstance().onCommandInterrupt(this::handleCommandEnding);
    CommandScheduler.getInstance().onCommandFinish(this::handleCommandEnding);
  }

  public void handleCommandEnding(Command endingCommand) {
    this.m_activeCommands.remove(endingCommand);
    for (Subsystem sub : endingCommand.getRequirements()) {
      Logger.recordOutput("Subsystems/" + sub.getName() + "/Command", "Nothing");
    }
  }

  /**
   * Updates SmartDashboard with the active command for every subsystem. Call this in
   * robotPeriodic().
   */
  public void updateTelemetry() {
    LoggedTracer.reset(); // reset start time
    Map<String, String> subsystemStatus = new HashMap<>();

    for (Command topLevelCmd : m_activeCommands) {
      String activeLeafName = resolveActiveLeafName(topLevelCmd); // find actual command running

      Set<Subsystem> requirements = topLevelCmd.getRequirements();

      if (requirements.isEmpty()) {
        String status = (activeLeafName == null) ? "Nothing" : activeLeafName;
        Logger.recordOutput("ActiveCommands/NoSubsystem/" + topLevelCmd.getName(), status);
      } else {
        for (Subsystem sub : requirements) {
          if (activeLeafName == null) {
            subsystemStatus.put(sub.getName(), "Nothing");
            continue;
          }
          subsystemStatus.put(sub.getName(), activeLeafName);
        }
      }
    }

    // Publish active commands
    for (Map.Entry<String, String> entry : subsystemStatus.entrySet()) {
      Logger.recordOutput("Subsystems/" + entry.getKey() + "/Command", entry.getValue());
    }

    LoggedTracer.record("CommandLogger"); // log time taken
  }

  /**
   * Recursively uses reflection to peel back CommandGroup layers to find the currently running
   * child.
   */
  private String resolveActiveLeafName(Command command) {

    if (command instanceof SequentialCommandGroup) {
      try {
        Field indexField = SequentialCommandGroup.class.getDeclaredField("m_currentCommandIndex");
        indexField.setAccessible(true);
        int index = indexField.getInt(command);

        Field commandsField = SequentialCommandGroup.class.getDeclaredField("m_commands");
        commandsField.setAccessible(true);
        @SuppressWarnings("unchecked")
        List<Command> commands = (List<Command>) commandsField.get(command);

        if (index >= 0 && index < commands.size()) {
          return resolveActiveLeafName(commands.get(index));
        }
        return null;
      } catch (Exception e) {
        return command.getName() + " (Reflection Error)";
      }
    } else if (command instanceof ParallelCommandGroup) {
      try {
        Field commandsField = ParallelCommandGroup.class.getDeclaredField("m_commands");
        commandsField.setAccessible(true);

        @SuppressWarnings("unchecked")
        Map<Command, Boolean> commands = (Map<Command, Boolean>) commandsField.get(command);

        // Filter to only show commands where Value is TRUE (Running)
        return commands.entrySet().stream()
            .filter(Map.Entry::getValue)
            .map(entry -> resolveActiveLeafName(entry.getKey())) // Recurse
            .collect(Collectors.joining(" + "));

      } catch (Exception e) {
        return command.getName();
      }
    }

    // Leaf command
    return command.getName();
  }
}