29template <
typename WheelSpeeds,
typename WheelPositions>
42 const WheelPositions& wheelPositions,
44 : m_kinematics(kinematics),
46 m_previousWheelPositions(wheelPositions) {
47 m_previousAngle = m_pose.Rotation();
48 m_gyroOffset = m_pose.Rotation() - gyroAngle;
62 const WheelPositions& wheelPositions,
const Pose2d& pose) {
65 m_gyroOffset = m_pose.Rotation() - gyroAngle;
66 m_previousWheelPositions = wheelPositions;
75 m_gyroOffset = m_gyroOffset + (pose.
Rotation() - m_pose.Rotation());
86 m_pose =
Pose2d{translation, m_pose.Rotation()};
95 m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation());
96 m_pose =
Pose2d{m_pose.Translation(), rotation};
97 m_previousAngle = rotation;
118 const WheelPositions& wheelPositions) {
119 auto angle = gyroAngle + m_gyroOffset;
122 m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
123 twist.dtheta = (angle - m_previousAngle).Radians();
125 auto newPose = m_pose.Exp(twist);
127 m_previousAngle = angle;
128 m_previousWheelPositions = wheelPositions;
129 m_pose = {newPose.Translation(), angle};
138 WheelPositions m_previousWheelPositions;
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel s...
Definition Kinematics.h:25
Class for odometry.
Definition Odometry.h:30
Odometry(const Kinematics< WheelSpeeds, WheelPositions > &kinematics, const Rotation2d &gyroAngle, const WheelPositions &wheelPositions, const Pose2d &initialPose=Pose2d{})
Constructs an Odometry object.
Definition Odometry.h:40
void ResetRotation(const Rotation2d &rotation)
Resets the rotation of the pose.
Definition Odometry.h:94
const Pose2d & Update(const Rotation2d &gyroAngle, const WheelPositions &wheelPositions)
Updates the robot's position on the field using forward kinematics and integration of the pose over t...
Definition Odometry.h:117
void ResetPosition(const Rotation2d &gyroAngle, const WheelPositions &wheelPositions, const Pose2d &pose)
Resets the robot's position on the field.
Definition Odometry.h:61
void ResetTranslation(const Translation2d &translation)
Resets the translation of the pose.
Definition Odometry.h:85
const Pose2d & GetPose() const
Returns the position of the robot on the field.
Definition Odometry.h:104
void ResetPose(const Pose2d &pose)
Resets the pose.
Definition Odometry.h:74
Represents a 2D pose containing translational and rotational elements.
Definition Pose2d.h:28
constexpr const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition Pose2d.h:128
A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Definition Rotation2d.h:31
Represents a translation in 2D space.
Definition Translation2d.h:29