vision logging by Levercpu · Pull Request #85 · FRC4048/Java_2026

Levercpu · 2026-02-21T23:15:05Z

No description provided.

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

armadilloz · 2026-02-25T04:34:03Z

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

                Constants.SET_MODULE_ENCODER_AUTO_SYNCHRONIZE_DEADBAND); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving.
        // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible
-
+        rawOdometry = new SwerveDriveOdometry(


Looking more closely at what we need to do, I am not certain we want raw odometry without vision. The YAGSL maintains a SwerveDrivePoseEstimator which wraps around a SwerveDriveOdometry which extends PoseEstimator which has support for vision measurements.
So it seems to me that when we are processing a vision input from the camera, we should be comparing it against the latest known position, including all the previous vision adjustments, to help us filter them odd ones out.
So I am no longer sure we need getOdom that only considers odometry from the swerve, certainly YAGSL does not maintain one.
Also, maintaining an extra odometry calculation seems expensive.
Am I missing something?

our vision filter would need to be refactored then

I don't see why. Maybe we should discuss so we can plan.

maybe put in sim wrapper (with utility class)

put in wrapper

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

armadilloz · 2026-02-26T02:54:20Z

src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java


  @Override
  public void processQueue() {
+    double currentTime = Logger.getTimestamp()/1000000.0;


I don't see what these changes amount to: Other than more log entries, what is the purpose?
With all the details being change like variable names and locations, it's really hard to distill the semantics of what changed.

easier to count how many tags we saw over the last second, allowing us to know if we had steady vision in the last second. (one thing that batchen said she wanted to be able to know)

So we should ask BatChen what she asked for. Can you ping us both on Discord so we can discuss?

src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java

armadilloz · 2026-02-26T02:58:41Z

src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java

-      FilterResult r = entry.getValue();
+    for (Map.Entry<VisionMeasurement, FilterResult> filterEntry : filteredData.entrySet()) {
+      VisionMeasurement v = filterEntry.getKey();
+      Apriltag tag = v.tag().tag();


If I'm not mistaken, this is the only place where the TagPose class is used. I believe everything this class could be achieved by the AprilTag itself since it already has its own pose.
Also, since the pose is fixed per tag, logging the pose is not that interesting since it will always be the same.

batchen said she wanted to be able to see the tag poses and tag ids together in one logging for easier lookup

Lets discuss with her...

…ision_Simulation

…a_2026 into LS_Vision_Simulation_+Logging # Conflicts: # src/main/java/frc/robot/apriltags/SimApriltagIO.java # src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

…a_2026 into LS_Vision_Simulation_+Logging # Please enter a commit message to explain why this merge is necessary, # especially if it merges an updated upstream into a topic branch. # # Lines starting with '#' will be ignored, and an empty message aborts # the commit.

…ision_Simulation

…a_2026 into LS_Vision_Simulation_+Logging

…ision_Simulation_+Logging # Conflicts: # src/main/java/frc/robot/Robot.java # src/main/java/frc/robot/apriltags/SimApriltagIO.java # src/main/java/frc/robot/constants/GameConstants.java # src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

armadilloz · 2026-03-10T23:49:36Z

src/main/java/frc/robot/constants/GameConstants.java


    //Debugs
-    public static final boolean DEBUG = false;
+    public static final boolean DEBUG = true;


This should be reverted

armadilloz · 2026-03-10T23:53:53Z

src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java

-    Logger.recordOutput("Apriltag/acceptedMeasurements", validMeasurements.toArray(Pose2d[]::new));
-    Logger.recordOutput(
-        "Apriltag/rejectedMeasurements", invalidMeasurements.toArray(Pose2d[]::new));
+//    Logger.recordOutput("Apriltag/acceptedMeasurementsPose", validMeasurementsPose.toArray(Pose2d[]::new));


armadilloz · 2026-03-10T23:58:40Z

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

+                    Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getOdom());
+                }
            }
-


Why remove these?

armadilloz · 2026-03-11T00:05:30Z

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

                Constants.SET_MODULE_ENCODER_AUTO_SYNCHRONIZE_DEADBAND); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving.
        // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible
-
+        if (Constants.currentMode==Constants.simMode) {


I think this was meant to be

Suggested change

if (Constants.currentMode==Constants.simMode) {

if (Constants.currentMode==Mode.SIM) {

armadilloz · 2026-03-11T00:08:43Z

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

+    // might be broken
    public void resetOdometry(Pose2d initialHolonomicPose) {
        swerveDrive.resetOdometry(initialHolonomicPose);
+        if (Constants.currentMode==Constants.simMode) {


Suggested change

if (Constants.currentMode==Constants.simMode) {

if (Constants.currentMode==Mode.SIM) {

armadilloz · 2026-03-11T00:17:14Z

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

     */
    private final SwerveDrive swerveDrive;
    private Vector<N3> variance = VecBuilder.fill(0.1,0.1,0.1);
+    private SwerveDriveOdometry rawOdometry;


I Think that the rawOdometry should be pulled into a separate class, like RawOdometry, that we can instantiate in SIM and update via periodic. Then all the state can be kept in it and not in the subsystem and we can clearly identify where It is used (and for what, since it be documented).

armadilloz · 2026-03-11T00:19:27Z

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

    public Pose2d getPose() {
        return swerveDrive.getPose();
    }
+    public double getError() {


This can be moved into the new RawOdometry class and then it can be used to calculate error from subsystem pose, and log if necessary.

armadilloz · 2026-03-11T00:20:18Z

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

    // Todo: fix to only get odomtry
    public Pose2d getOdom() {
-        return swerveDrive.getPose();
+        if (Constants.currentMode==Constants.simMode) {


This can be reverted, and the rawOdometry.getPose can be used directly by the SimAprilTagIo if needed.

Levercpu added 3 commits February 21, 2026 17:30

initial commit

88b1f69

initial commit

1781c98

initial commit

16ab803

Levercpu requested review from MikeKantrowitz, armadilloz and batchen1 as code owners February 21, 2026 23:15

Levercpu added 3 commits February 22, 2026 16:43

initial commit

f244dae

merge main

370ea9a

merge main

c0ae709

armadilloz requested changes Feb 26, 2026

View reviewed changes

Levercpu added 5 commits March 1, 2026 13:38

most pr comments addressed

2c7560a

Merge branch 'main' of https://github.com/FRC4048/Java_2026 into LS_V…

4e2456f

…ision_Simulation

all addressed or comment or question

50d395e

Merge branch 'LS_Vision_Simulation' of https://github.com/FRC4048/Jav…

d449065

…a_2026 into LS_Vision_Simulation_+Logging # Conflicts: # src/main/java/frc/robot/apriltags/SimApriltagIO.java # src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

pr comments fixed.

e059a2f

Levercpu requested a review from armadilloz March 1, 2026 18:53

armadilloz requested changes Mar 3, 2026

View reviewed changes

Levercpu added 10 commits March 4, 2026 19:37

fixed pr comments

49f822e

only way to not thrown null pointer

5754b46

small logging change

bd2fac0

Merge branch 'main' of https://github.com/FRC4048/Java_2026 into LS_V…

c4a51e7

…ision_Simulation

Merge branch 'main' of https://github.com/FRC4048/Java_2026 into LS_V…

1bb160f

…ision_Simulation

Merge branch 'LS_Vision_Simulation' of https://github.com/FRC4048/Jav…

382f6b6

…a_2026 into LS_Vision_Simulation_+Logging

fixed

a47b8f7

fixed

76bf35f

Levercpu changed the base branch from LS_Vision_Simulation to main March 10, 2026 00:16

Levercpu added 2 commits March 9, 2026 20:38

fixed

66f7fd9

wip

5c60892

Levercpu added 2 commits March 9, 2026 20:44

wip

cdde152

done

e11869c

Levercpu requested a review from armadilloz March 10, 2026 00:52

armadilloz requested changes Mar 11, 2026

View reviewed changes

	if (Constants.currentMode==Constants.simMode) {
	if (Constants.currentMode==Mode.SIM) {

Conversation

Levercpu commented Feb 21, 2026

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

armadilloz Feb 26, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

armadilloz Feb 26, 2026 •

edited

Loading