From 31e92d85d685370c72bbcc0bfa85eb84f6f80890 Mon Sep 17 00:00:00 2001 From: Nate Laverdure <41549690+nlaverdure@users.noreply.github.com> Date: Fri, 24 Jul 2026 11:27:27 -0400 Subject: [PATCH] Fix PathPlanner trajectory logging in sim (#25) - 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 41a6c05..f6686d3 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -89,6 +89,9 @@ public class Drive extends SubsystemBase { }; private ChassisVelocities chassisVelocities; + // 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); @@ -127,12 +130,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 @@ -217,6 +215,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;