Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add Reset methods to PoseEstimator (#6751)

Browse Source
This commit is contained in:
Jade
2024-07-09 09:26:08 +08:00
committed by GitHub
parent dc276b651b
commit cc02a948a0
3 changed files with 76 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
View File

@@ -15,6 +15,7 @@
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/Kinematics.h"
#include "frc/kinematics/Odometry.h"
@@ -87,6 +88,27 @@ class WPILIB_DLLEXPORT PoseEstimator {
void ResetPosition(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions, const Pose2d& pose);
/**
* Resets the robot's pose.
*
* @param pose The pose to reset to.
*/
void ResetPose(const Pose2d& pose);
/**
* Resets the robot's translation.
*
* @param translation The pose to translation to.
*/
void ResetTranslation(const Translation2d& translation);
/**
* Resets the robot's rotation.
*
* @param rotation The rotation to reset to.
*/
void ResetRotation(const Rotation2d& rotation);
/**
* Gets the estimated robot pose.
*

23
wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
View File

@@ -5,6 +5,9 @@
#pragma once
#include "frc/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/WheelPositions.h"
namespace frc {
@@ -53,6 +56,26 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
m_poseEstimate = m_odometry.GetPose();
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPose(const Pose2d& pose) {
m_odometry.ResetPose(pose);
m_odometryPoseBuffer.Clear();
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetTranslation(
const Translation2d& translation) {
m_odometry.ResetTranslation(translation);
m_odometryPoseBuffer.Clear();
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetRotation(
const Rotation2d& rotation) {
m_odometry.ResetTranslation(rotation);
m_odometryPoseBuffer.Clear();
}
template <typename WheelSpeeds, typename WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
const {