From 0cb05ad18515cdd451dac32a796fb614161a27e8 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 24 Jul 2026 11:22:57 -0400 Subject: [PATCH] Fix PathPlanner trajectory logging in sim - Store the active PathPlanner path in a lastTrajectory field (updated by the callback) instead of calling Logger.recordOutput directly from the callback - Log Odometry/Trajectory every periodic() loop for AdvantageKit compatibility - Only update lastTrajectory when the incoming path is non-empty, so the plot persists in AdvantageScope after the path command ends - Removes unused setLogTargetPoseCallback / Odometry/TrajectorySetpoint Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/robot/subsystems/drive/Drive.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index b46a1ba..c6de68e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -92,6 +92,9 @@ public class Drive extends SubsystemBase { }; private ChassisSpeeds chassisSpeeds; + // PathPlanner trajectory logging (stored each loop for AKit compatibility) + private Pose2d[] lastTrajectory = new Pose2d[0]; + // PID controllers for following Choreo trajectories private final PIDController xController = new PIDController(8.01, 0.0, 0.0); private final PIDController yController = new PIDController(8.01, 0.0, 0.0); @@ -129,12 +132,7 @@ public Drive( Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); - PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + if (!activePath.isEmpty()) lastTrajectory = activePath.toArray(new Pose2d[0]); }); // Configure SysId @@ -220,6 +218,9 @@ public void periodic() { gyroDisconnectedAlert.set(gyroDisconnected); Logger.recordOutput("Faults/Drive/GyroDisconnected", gyroDisconnected); + // Log PathPlanner trajectory (stored by callback, recorded here for AKit compatibility) + Logger.recordOutput("Odometry/Trajectory", lastTrajectory); + // Profiling output if (FeatureFlags.PROFILING_ENABLED) { long totalMs = (t6 - startNanos) / 1_000_000;