38 Rotation2d m_previousAngle;
39 Rotation2d m_gyroOffset;
51 m_kinematics{kinematics},
52 m_pose{std::move(initialPose)},
53 m_previousWheelPositions{std::move(modulePositions)},
54 m_previousAngle{m_pose.Rotation()},
55 m_gyroOffset{m_pose.Rotation() - gyroAngle}
71 m_previousAngle = m_pose.Rotation();
72 m_gyroOffset = m_pose.Rotation() - gyroAngle;
73 m_previousWheelPositions = std::move(wheelPositions);
81 Pose2d
const &
Pose()
const {
return m_pose; }
113 struct InterpolationRecord {
117 Rotation2d gyroAngle;
129 InterpolationRecord Interpolate(
SwerveDriveKinematics const &kinematics, InterpolationRecord endValue,
double i)
const;
131 bool operator==(InterpolationRecord
const &other)
const
133 return pose == other.pose &&
134 gyroAngle == other.gyroAngle &&
135 wheelPositions == other.wheelPositions;
137 bool operator!=(InterpolationRecord
const &other)
const
139 return !(*
this == other);
143 static constexpr units::second_t kBufferDuration = 1.5_s;
147 std::array<double, 3> m_q{};
149 struct VisionMatrices;
150 std::unique_ptr<VisionMatrices> m_matrices;
153 kBufferDuration, [
this](
auto const &start,
auto const &end,
double t) {
154 return start.Interpolate(this->m_kinematics, end, t);
178 {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}}
200 std::array<double, 3>
const &stateStdDevs, std::array<double, 3>
const &visionMeasurementStdDevs);
228 m_odometry.
ResetPosition(gyroAngle, std::move(wheelPositions), pose);
229 m_poseBuffer.Clear();
239 return m_odometry.
Pose();
249 std::optional<Pose2d>
SampleAt(units::second_t timestamp)
const
251 std::optional<InterpolationRecord>
const sample = m_poseBuffer.Sample(timestamp);
310 std::array<double, 3>
const &visionMeasurementStdDevs)
312 SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
313 AddVisionMeasurement(visionRobotPose, timestamp);
342 m_odometry.
Update(gyroAngle, wheelPositions);
343 m_poseBuffer.AddSample(
345 {GetEstimatedPosition(), gyroAngle, wheelPositions}
347 return GetEstimatedPosition();
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.hpp:235
Class for swerve drive odometry.
Definition: SwerveDrivePoseEstimator.hpp:28
Pose2d const & Pose() const
Returns the position of the robot on the field.
Definition: SwerveDrivePoseEstimator.hpp:81
void ResetPosition(Rotation2d const &gyroAngle, WheelPositions wheelPositions, Pose2d const &pose)
Resets the robot's position on the field.
Definition: SwerveDrivePoseEstimator.hpp:68
std::vector< SwerveModulePosition > WheelPositions
Definition: SwerveDrivePoseEstimator.hpp:31
Pose2d const & Update(Rotation2d const &gyroAngle, WheelPositions wheelPositions)
Updates the robot's position on the field using forward kinematics and integration of the pose over t...
std::vector< SwerveModuleState > WheelSpeeds
Definition: SwerveDrivePoseEstimator.hpp:30
SwerveDriveOdometry(SwerveDriveKinematics const &kinematics, Rotation2d const &gyroAngle, WheelPositions modulePositions, Pose2d initialPose=Pose2d{})
Constructs a SwerveDriveOdometry object.
Definition: SwerveDrivePoseEstimator.hpp:50
This class wraps Swerve Drive Odometry to fuse latency-compensated vision measurements with swerve dr...
Definition: SwerveDrivePoseEstimator.hpp:107
Pose2d UpdateWithTime(units::second_t currentTime, Rotation2d const &gyroAngle, WheelPositions const &wheelPositions)
Updates the pose estimator with wheel encoder and gyro information.
Definition: SwerveDrivePoseEstimator.hpp:340
std::vector< SwerveModulePosition > WheelPositions
Definition: SwerveDrivePoseEstimator.hpp:110
~SwerveDrivePoseEstimator()
void ResetPosition(Rotation2d const &gyroAngle, WheelPositions wheelPositions, Pose2d const &pose)
Resets the robot's position on the field.
Definition: SwerveDrivePoseEstimator.hpp:226
void AddVisionMeasurement(Pose2d const &visionRobotPose, units::second_t timestamp)
Adds a vision measurement to the Kalman Filter.
SwerveDrivePoseEstimator(SwerveDriveKinematics const &kinematics, Rotation2d const &gyroAngle, WheelPositions modulePositions, Pose2d initialPose, std::array< double, 3 > const &stateStdDevs, std::array< double, 3 > const &visionMeasurementStdDevs)
Constructs a SwerveDrivePoseEstimator.
Pose2d GetEstimatedPosition() const
Gets the estimated robot pose.
Definition: SwerveDrivePoseEstimator.hpp:237
std::vector< SwerveModuleState > WheelSpeeds
Definition: SwerveDrivePoseEstimator.hpp:109
SwerveDrivePoseEstimator(SwerveDriveKinematics const &kinematics, Rotation2d const &gyroAngle, WheelPositions modulePositions, Pose2d initialPose)
Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision measu...
Definition: SwerveDrivePoseEstimator.hpp:175
std::optional< Pose2d > SampleAt(units::second_t timestamp) const
Return the pose at a given timestamp, if the buffer is not empty.
Definition: SwerveDrivePoseEstimator.hpp:249
void SetVisionMeasurementStdDevs(std::array< double, 3 > const &visionMeasurementStdDevs)
Sets the pose estimator's trust in vision measurements.
Pose2d Update(Rotation2d const &gyroAngle, WheelPositions const &wheelPositions)
Updates the pose estimator with wheel encoder and gyro information.
Definition: SwerveDrivePoseEstimator.hpp:325
void AddVisionMeasurement(Pose2d const &visionRobotPose, units::second_t timestamp, std::array< double, 3 > const &visionMeasurementStdDevs)
Adds a vision measurement to the Kalman Filter.
Definition: SwerveDrivePoseEstimator.hpp:309
CTREXPORT double GetCurrentTimeSeconds()
Get the current timestamp in seconds.
Represents the state of one swerve module.
Definition: StatusCodes.h:18