Localizer dev - Mishawaka with vision#8
Closed
Anunaya08 wants to merge 16 commits into
Closed
Conversation
Co-authored-by: Anunaya08 <181024395+Anunaya08@users.noreply.github.com>
Merge alpha into localizer-dev
Alpha-Localizerdev
Contributor
Author
|
@copilot suggest merge conflict resolution based on what is most updated/consistent with the lates working tree |
I reviewed the merge resolution against the latest working tree. The current resolution is already the consistent/latest one: keep the alpha-side Java files (Localizer/Vision/Swerve/Constants) and keep Warning Firewall rules blocked me from connecting to one or more addresses (expand for details)I tried to connect to the following addresses, but was blocked by firewall rules:
If you need me to access, download, or install something from one of these locations, you can either:
|
There was a problem hiding this comment.
Pull request overview
This PR wires a vision-assisted localization pipeline into the robot code by enabling the Localizer subsystem, centralizing Vision constants, and updating a PathPlanner auto to reference red-side paths.
Changes:
- Re-enabled
Localizerand integrated it intoSwerve.periodic()to fuse odometry + vision measurements. - Reworked
Constants.VisionConstantsto provide concrete camera transforms and vision/odom standard deviation matrices; updatedVisionto use renamed constants and Vision-scoped NT instance. - Updated a PathPlanner auto to reference
Red ...path names.
Reviewed changes
Copilot reviewed 5 out of 5 changed files in this pull request and generated 6 comments.
Show a summary per file
| File | Description |
|---|---|
src/main/java/frc/robot/util/vision/Vision.java |
Updates constant names/NT instance usage for PhotonVision cameras; leaves NetworkTables publishing commented out. |
src/main/java/frc/robot/subsystems/localizer/Localizer.java |
Restores a pose estimator + Vision integration and publishes Field2d pose. |
src/main/java/frc/robot/subsystems/drivetrain/Swerve.java |
Instantiates Localizer and calls it from Swerve.periodic(). |
src/main/java/frc/robot/constants/Constants.java |
Defines concrete Vision constants (camera poses, std devs) and removes some unused/placeholder constants. |
src/main/deploy/pathplanner/autos/Left Trench shoot and Human Player.auto |
Switches referenced path names to the Red ... variants. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Comment on lines
16
to
+19
| public final class VisionConstants { | ||
|
|
||
|
|
||
| public static String CAMERA_FR_NAME; | ||
| public static double CAMERA_FR_YAW; | ||
| public static double CAMERA_FR_PITCH; | ||
| public static double CAMERA_FR_ROLL; | ||
| public static double CAMERA_FR_FORWARD; | ||
| public static double CAMERA_FR_LEFT; | ||
| public static double CAMERA_FR_UP; | ||
|
|
||
| public static String CAMERA_FL_NAME; | ||
| public static double CAMERA_FL_YAW; | ||
| public static double CAMERA_FL_PITCH; | ||
| public static double CAMERA_FL_ROLL; | ||
| public static double CAMERA_FL_FORWARD; | ||
| public static double CAMERA_FL_LEFT; | ||
| public static double CAMERA_FL_UP; | ||
|
|
||
| public static String CAMERA_BR_NAME; | ||
| public static double CAMERA_BR_YAW; | ||
| public static double CAMERA_BR_PITCH; | ||
| public static double CAMERA_BR_ROLL; | ||
| public static double CAMERA_BR_FORWARD; | ||
| public static double CAMERA_BR_LEFT; | ||
| public static double CAMERA_BR_UP; | ||
| public static final NetworkTableInstance NT_INSTANCE = NetworkTableInstance.getDefault(); | ||
|
|
There was a problem hiding this comment.
VisionConstants is declared as a non-static inner class, but it now contains non-compile-time static members (e.g., NT_INSTANCE, Matrix values). Java forbids static members in non-static inner classes, so this should be public static final class VisionConstants (and similarly for other nested constants classes that hold static non-constants).
Comment on lines
+3
to
+39
| import java.util.List; | ||
| import java.util.Optional; | ||
|
|
||
|
|
||
| import org.photonvision.EstimatedRobotPose; | ||
| import org.photonvision.PhotonCamera; | ||
| import org.photonvision.PhotonPoseEstimator; | ||
| import org.photonvision.PhotonUtils; | ||
| import org.photonvision.targeting.PhotonPipelineResult; | ||
| import org.photonvision.targeting.PhotonTrackedTarget; | ||
|
|
||
| import edu.wpi.first.apriltag.AprilTagFieldLayout; | ||
| import edu.wpi.first.apriltag.AprilTagFields; | ||
| import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; | ||
| import edu.wpi.first.math.geometry.*; | ||
| import edu.wpi.first.math.kinematics.SwerveDriveKinematics; | ||
| import edu.wpi.first.math.kinematics.SwerveModulePosition; | ||
| import edu.wpi.first.math.numbers.N1; | ||
| import edu.wpi.first.math.numbers.N3; | ||
| import edu.wpi.first.networktables.NetworkTable; | ||
| import edu.wpi.first.networktables.NetworkTableInstance; | ||
| import edu.wpi.first.networktables.StructPublisher; | ||
| import edu.wpi.first.wpilibj.smartdashboard.Field2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import edu.wpi.first.math.Matrix; | ||
| import frc.robot.Robot; | ||
| import frc.robot.constants.Constants; | ||
| import frc.robot.subsystems.drivetrain.Swerve; | ||
| import frc.robot.util.vision.Vision.BW; | ||
| import frc.robot.util.vision.Vision.BW.BWCamera; | ||
| import frc.robot.util.vision.Vision; | ||
|
|
||
| public class Localizer extends SubsystemBase { | ||
|
|
||
| private final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); | ||
|
|
There was a problem hiding this comment.
This file includes a large set of imports that are currently unused (e.g., PhotonVision types like EstimatedRobotPose, PhotonPoseEstimator, PhotonUtils, etc.). Please remove unused imports (and any unused helper fields) to keep the file focused and avoid potential lint/tooling failures.
Suggested change
| import java.util.List; | |
| import java.util.Optional; | |
| import org.photonvision.EstimatedRobotPose; | |
| import org.photonvision.PhotonCamera; | |
| import org.photonvision.PhotonPoseEstimator; | |
| import org.photonvision.PhotonUtils; | |
| import org.photonvision.targeting.PhotonPipelineResult; | |
| import org.photonvision.targeting.PhotonTrackedTarget; | |
| import edu.wpi.first.apriltag.AprilTagFieldLayout; | |
| import edu.wpi.first.apriltag.AprilTagFields; | |
| import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; | |
| import edu.wpi.first.math.geometry.*; | |
| import edu.wpi.first.math.kinematics.SwerveDriveKinematics; | |
| import edu.wpi.first.math.kinematics.SwerveModulePosition; | |
| import edu.wpi.first.math.numbers.N1; | |
| import edu.wpi.first.math.numbers.N3; | |
| import edu.wpi.first.networktables.NetworkTable; | |
| import edu.wpi.first.networktables.NetworkTableInstance; | |
| import edu.wpi.first.networktables.StructPublisher; | |
| import edu.wpi.first.wpilibj.smartdashboard.Field2d; | |
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | |
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | |
| import edu.wpi.first.math.Matrix; | |
| import frc.robot.Robot; | |
| import frc.robot.constants.Constants; | |
| import frc.robot.subsystems.drivetrain.Swerve; | |
| import frc.robot.util.vision.Vision.BW; | |
| import frc.robot.util.vision.Vision.BW.BWCamera; | |
| import frc.robot.util.vision.Vision; | |
| public class Localizer extends SubsystemBase { | |
| private final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); | |
| import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; | |
| import edu.wpi.first.math.geometry.*; | |
| import edu.wpi.first.math.kinematics.SwerveModulePosition; | |
| import edu.wpi.first.wpilibj.smartdashboard.Field2d; | |
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | |
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | |
| import frc.robot.constants.Constants; | |
| import frc.robot.subsystems.drivetrain.Swerve; | |
| import frc.robot.util.vision.Vision; | |
| public class Localizer extends SubsystemBase { |
Comment on lines
+82
to
+83
| vision.getEstimatedGlobalPoses(poseEstimator.getEstimatedPosition()); | ||
| currentPose = poseEstimator.getEstimatedPosition(); |
There was a problem hiding this comment.
vision.getEstimatedGlobalPoses(poseEstimator.getEstimatedPosition()) passes a reference pose, but Vision#getEstimatedGlobalPoses(Pose2d) currently does not use its referencePose parameter at all. Either wire the reference pose into the PhotonPoseEstimator (e.g., set the estimator's reference pose before estimating) or remove/rename the parameter/call to avoid a misleading API.
Suggested change
| vision.getEstimatedGlobalPoses(poseEstimator.getEstimatedPosition()); | |
| currentPose = poseEstimator.getEstimatedPosition(); | |
| currentPose = poseEstimator.getEstimatedPosition(); | |
| // Vision#getEstimatedGlobalPoses(Pose2d) currently ignores its Pose2d argument, | |
| // so avoid passing the estimator pose as though it were used as a reference pose. | |
| vision.getEstimatedGlobalPoses(new Pose2d()); |
Comment on lines
+83
to
+85
| currentPose = poseEstimator.getEstimatedPosition(); | ||
| fieldOdom.setRobotPose(currentPose); | ||
| SmartDashboard.putData("Field2d Pose", fieldOdom); |
There was a problem hiding this comment.
SmartDashboard.putData("Field2d Pose", fieldOdom) is being called every periodic(). putData typically only needs to be called once (e.g., in the constructor); in periodic() you should only update the Field2d pose. Re-publishing every loop can add unnecessary NetworkTables overhead.
Comment on lines
+52
to
61
| //private final NetworkTable visionTable; | ||
|
|
||
| private final Field2d debugField = new Field2d(); | ||
|
|
||
| //TODO: PUBLISH VALUES TO NETWORK TABLES | ||
| public Vision(EstimateConsumer estC) { | ||
|
|
||
| visionTable = NetworkTableInstance.getDefault().getTable("Vision"); | ||
| //visionTable = NetworkTableInstance.getDefault().getTable("Vision"); | ||
|
|
||
| for(BWCamera cam : BW.BWCamera.values()) { |
There was a problem hiding this comment.
The visionTable field and initialization are commented out, but the file still imports NetworkTables types. If NetworkTables publishing is not being used anymore, please delete the dead/commented code and remove the unused imports; if it is planned, consider implementing the publishing rather than leaving commented-out declarations.
| public void updateOdometry(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { | ||
| poseEstimator.update(gyroAngle, modulePositions); | ||
| } | ||
|
|
There was a problem hiding this comment.
periodic() overrides SubsystemBase.periodic(), but it’s missing an @Override annotation. Adding @Override here will make the override explicit and catch signature mistakes at compile time (consistent with other subsystems in this repo).
Suggested change
| @Override |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
No description provided.