Merge pull request #52 from CrossTheRoadElec/swerve-add-vision-measur…

…ement

Add override for `SwerveDrivetrain::addVisionMeasurement` to convert timebase

cpp/SwerveWithPathPlanner/src/main/cpp/Robot.cpp

@@ -28,7 +28,7 @@ void Robot::RobotPeriodic() {
     LimelightHelpers::SetRobotOrientation("limelight", heading.value(), 0, 0, 0, 0, 0);
     auto llMeasurement = LimelightHelpers::getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
     if (llMeasurement && llMeasurement->tagCount > 0 && units::math::abs(omega) < 2_tps) {
-      m_container.drivetrain.AddVisionMeasurement(llMeasurement->pose, utils::FPGAToCurrentTime(llMeasurement->timestampSeconds));
+      m_container.drivetrain.AddVisionMeasurement(llMeasurement->pose, llMeasurement->timestampSeconds);
     }
   }
 }

cpp/SwerveWithPathPlanner/src/main/include/subsystems/CommandSwerveDrivetrain.h

@@ -254,6 +254,38 @@ class CommandSwerveDrivetrain : public frc2::SubsystemBase, public TunerSwerveDr
         return m_sysIdRoutineToApply->Dynamic(direction);
     }
 
+    /**
+     * \brief Adds a vision measurement to the Kalman Filter. This will correct the
+     * odometry pose estimate while still accounting for measurement noise.
+     *
+     * \param visionRobotPose The pose of the robot as measured by the vision camera.
+     * \param timestamp The timestamp of the vision measurement in seconds.
+     */
+    void AddVisionMeasurement(frc::Pose2d visionRobotPose, units::second_t timestamp) override
+    {
+        TunerSwerveDrivetrain::AddVisionMeasurement(std::move(visionRobotPose), utils::FPGAToCurrentTime(timestamp));
+    }
+
+    /**
+     * \brief Adds a vision measurement to the Kalman Filter. This will correct the
+     * odometry pose estimate while still accounting for measurement noise.
+     *
+     * Note that the vision measurement standard deviations passed into this method
+     * will continue to apply to future measurements until a subsequent call to
+     * #SetVisionMeasurementStdDevs or this method.
+     *
+     * \param visionRobotPose The pose of the robot as measured by the vision camera.
+     * \param timestamp The timestamp of the vision measurement in seconds.
+     * \param visionMeasurementStdDevs Standard deviations of the vision pose measurement.
+     */
+    void AddVisionMeasurement(
+        frc::Pose2d visionRobotPose,
+        units::second_t timestamp,
+        std::array<double, 3> visionMeasurementStdDevs) override
+    {
+        TunerSwerveDrivetrain::AddVisionMeasurement(std::move(visionRobotPose), utils::FPGAToCurrentTime(timestamp), visionMeasurementStdDevs);
+    }
+
 private:
     void ConfigureAutoBuilder();
     void StartSimThread();

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

@@ -4,8 +4,6 @@
 
 package frc.robot;
 
-import com.ctre.phoenix6.Utils;
-
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -42,7 +40,7 @@ public void robotPeriodic() {
       LimelightHelpers.SetRobotOrientation("limelight", headingDeg, 0, 0, 0, 0, 0);
       var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
       if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) {
-        m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, Utils.fpgaToCurrentTime(llMeasurement.timestampSeconds));
+        m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds);
       }
     }
   }

java/SwerveWithChoreo/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -15,6 +15,7 @@
 import choreo.trajectory.SwerveSample;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
@@ -315,4 +316,38 @@ private void startSimThread() {
         });
         m_simNotifier.startPeriodic(kSimLoopPeriod);
     }
+
+    /**
+     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * while still accounting for measurement noise.
+     *
+     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     */
+    @Override
+    public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+        super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds));
+    }
+
+    /**
+     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * while still accounting for measurement noise.
+     * <p>
+     * Note that the vision measurement standard deviations passed into this method
+     * will continue to apply to future measurements until a subsequent call to
+     * {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
+     *
+     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
+     *     in the form [x, y, theta]ᵀ, with units in meters and radians.
+     */
+    @Override
+    public void addVisionMeasurement(
+        Pose2d visionRobotPoseMeters,
+        double timestampSeconds,
+        Matrix<N3, N1> visionMeasurementStdDevs
+    ) {
+        super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs);
+    }
 }

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

@@ -4,8 +4,6 @@
 
 package frc.robot;
 
-import com.ctre.phoenix6.Utils;
-
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -42,7 +40,7 @@ public void robotPeriodic() {
       LimelightHelpers.SetRobotOrientation("limelight", headingDeg, 0, 0, 0, 0, 0);
       var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
       if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) {
-        m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, Utils.fpgaToCurrentTime(llMeasurement.timestampSeconds));
+        m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds);
       }
     }
   }

java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -16,6 +16,7 @@
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 
 import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
@@ -290,4 +291,38 @@ private void startSimThread() {
         });
         m_simNotifier.startPeriodic(kSimLoopPeriod);
     }
+
+    /**
+     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * while still accounting for measurement noise.
+     *
+     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     */
+    @Override
+    public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+        super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds));
+    }
+
+    /**
+     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * while still accounting for measurement noise.
+     * <p>
+     * Note that the vision measurement standard deviations passed into this method
+     * will continue to apply to future measurements until a subsequent call to
+     * {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
+     *
+     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
+     *     in the form [x, y, theta]ᵀ, with units in meters and radians.
+     */
+    @Override
+    public void addVisionMeasurement(
+        Pose2d visionRobotPoseMeters,
+        double timestampSeconds,
+        Matrix<N3, N1> visionMeasurementStdDevs
+    ) {
+        super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs);
+    }
 }

python/SwerveWithPathPlanner/subsystems/command_swerve_drivetrain.py

@@ -7,7 +7,7 @@
 from typing import Callable, overload
 from wpilib import DriverStation, Notifier, RobotController
 from wpilib.sysid import SysIdRoutineLog
-from wpimath.geometry import Rotation2d
+from wpimath.geometry import Pose2d, Rotation2d
 
 
 class CommandSwerveDrivetrain(Subsystem, swerve.SwerveDrivetrain):
@@ -329,3 +329,23 @@ def _sim_periodic():
         self._last_sim_time = utils.get_current_time_seconds()
         self._sim_notifier = Notifier(_sim_periodic)
         self._sim_notifier.startPeriodic(self._SIM_LOOP_PERIOD)
+
+    def add_vision_measurement(self, vision_robot_pose: Pose2d, timestamp: units.second, vision_measurement_std_devs: tuple[float, float, float] | None = None):
+        """
+        Adds a vision measurement to the Kalman Filter. This will correct the
+        odometry pose estimate while still accounting for measurement noise.
+
+        Note that the vision measurement standard deviations passed into this method
+        will continue to apply to future measurements until a subsequent call to
+        set_vision_measurement_std_devs or this method.
+
+        :param vision_robot_pose:           The pose of the robot as measured by the vision camera.
+        :type vision_robot_pose:            Pose2d
+        :param timestamp:                   The timestamp of the vision measurement in seconds.
+        :type timestamp:                    second
+        :param vision_measurement_std_devs: Standard deviations of the vision pose measurement
+                                            in the form [x, y, theta]ᵀ, with units in meters
+                                            and radians.
+        :type vision_measurement_std_devs:  tuple[float, float, float] | None
+        """
+        swerve.SwerveDrivetrain.add_vision_measurement(self, vision_robot_pose, utils.fpga_to_current_time(timestamp), vision_measurement_std_devs)