From c48044375b6c4d7b46cde74e09fedddb9911f570 Mon Sep 17 00:00:00 2001 From: Benjamin Hall Date: Mon, 20 Jan 2025 20:06:25 -0500 Subject: [PATCH] Add override for SwerveDrivetrain::addVisionMeasurement to convert timebase --- .../src/main/cpp/Robot.cpp | 2 +- .../subsystems/CommandSwerveDrivetrain.h | 32 +++++++++++++++++ .../src/main/java/frc/robot/Robot.java | 4 +-- .../subsystems/CommandSwerveDrivetrain.java | 35 +++++++++++++++++++ .../src/main/java/frc/robot/Robot.java | 4 +-- .../subsystems/CommandSwerveDrivetrain.java | 35 +++++++++++++++++++ .../subsystems/command_swerve_drivetrain.py | 22 +++++++++++- 7 files changed, 126 insertions(+), 8 deletions(-) diff --git a/cpp/SwerveWithPathPlanner/src/main/cpp/Robot.cpp b/cpp/SwerveWithPathPlanner/src/main/cpp/Robot.cpp index 0ba9f25..4c7a5ea 100644 --- a/cpp/SwerveWithPathPlanner/src/main/cpp/Robot.cpp +++ b/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); } } } diff --git a/cpp/SwerveWithPathPlanner/src/main/include/subsystems/CommandSwerveDrivetrain.h b/cpp/SwerveWithPathPlanner/src/main/include/subsystems/CommandSwerveDrivetrain.h index 1c4a1b1..f566806 100644 --- a/cpp/SwerveWithPathPlanner/src/main/include/subsystems/CommandSwerveDrivetrain.h +++ b/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 visionMeasurementStdDevs) override + { + TunerSwerveDrivetrain::AddVisionMeasurement(std::move(visionRobotPose), utils::FPGAToCurrentTime(timestamp), visionMeasurementStdDevs); + } + private: void ConfigureAutoBuilder(); void StartSimThread(); diff --git a/java/SwerveWithChoreo/src/main/java/frc/robot/Robot.java b/java/SwerveWithChoreo/src/main/java/frc/robot/Robot.java index a2dcd64..ddf6877 100644 --- a/java/SwerveWithChoreo/src/main/java/frc/robot/Robot.java +++ b/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); } } } diff --git a/java/SwerveWithChoreo/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/java/SwerveWithChoreo/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 87d9dd6..4686dd3 100644 --- a/java/SwerveWithChoreo/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/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. + *

+ * 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 visionMeasurementStdDevs + ) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); + } } diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/Robot.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/Robot.java index a2dcd64..ddf6877 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/Robot.java +++ b/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); } } } diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 8805188..0b88057 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/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. + *

+ * 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 visionMeasurementStdDevs + ) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); + } } diff --git a/python/SwerveWithPathPlanner/subsystems/command_swerve_drivetrain.py b/python/SwerveWithPathPlanner/subsystems/command_swerve_drivetrain.py index 3900841..025c0b4 100644 --- a/python/SwerveWithPathPlanner/subsystems/command_swerve_drivetrain.py +++ b/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)