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

[wpimath] Fix ResetTranslation and ResetRotation in PoseEstimator and PoseEstimator3d causing the robot to teleport (#8285)

Browse Source 
Fixes https://github.com/wpilibsuite/allwpilib/issues/8284.

If we have vision updates at the time of the `Reset*` call, we can
correct the translation/rotation of the new odometry pose by adding a
new vision update where:
- `ResetTranslation`: the translation is hard-coded to the new
translation, and the rotation components are set to those of the latest
vision update (prior to clearing the map).
- `ResetRotation`: the rotation is hard-coded to the new rotation, and
the translation components are set to those of the latest vision update
(prior to clearing the map).
This commit is contained in:
Benjamin Hall
2025-12-13 18:53:53 -05:00
committed by GitHub
parent baa6379267
commit 3dc334c1ee
8 changed files with 275 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -139,24 +139,70 @@ class WPILIB_DLLEXPORT PoseEstimator {
*
* @param translation The pose to translation to.
*/
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif // defined(__GNUC__) && !defined(__clang__)
void ResetTranslation(const Translation2d& translation) {
m_odometry.ResetTranslation(translation);
const std::optional<std::pair<units::second_t, VisionUpdate>>
latestVisionUpdate =
m_visionUpdates.empty() ? std::nullopt
: std::optional{*m_visionUpdates.crbegin()};
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();
if (latestVisionUpdate) {
// apply vision compensation to the pose rotation
const VisionUpdate visionUpdate{
Pose2d{translation, latestVisionUpdate->second.visionPose.Rotation()},
Pose2d{translation,
latestVisionUpdate->second.odometryPose.Rotation()}};
m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
} else {
m_poseEstimate = m_odometry.GetPose();
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif // defined(__GNUC__) && !defined(__clang__)
/**
* Resets the robot's rotation.
*
* @param rotation The rotation to reset to.
*/
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif // defined(__GNUC__) && !defined(__clang__)
void ResetRotation(const Rotation2d& rotation) {
m_odometry.ResetRotation(rotation);
const std::optional<std::pair<units::second_t, VisionUpdate>>
latestVisionUpdate =
m_visionUpdates.empty() ? std::nullopt
: std::optional{*m_visionUpdates.crbegin()};
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();
if (latestVisionUpdate) {
// apply vision compensation to the pose translation
const VisionUpdate visionUpdate{
Pose2d{latestVisionUpdate->second.visionPose.Translation(), rotation},
Pose2d{latestVisionUpdate->second.odometryPose.Translation(),
rotation}};
m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
} else {
m_poseEstimate = m_odometry.GetPose();
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif // defined(__GNUC__) && !defined(__clang__)
/**
* Gets the estimated robot pose.
@@ -436,7 +482,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
TimeInterpolatableBuffer<Pose2d> m_odometryPoseBuffer{kBufferDuration};
// Maps timestamps to vision updates
// Always contains one entry before the oldest entry in m_odometryPoseBuffer,
// unless there have been no vision measurements after the last reset
// unless there have been no vision measurements after the last reset. May
// contain one entry while m_odometryPoseBuffer is empty to correct for
// translation/rotation after a call to ResetRotation/ResetTranslation.
std::map<units::second_t, VisionUpdate> m_visionUpdates;
Pose2d m_poseEstimate;

54
wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
View File

@@ -148,24 +148,70 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
*
* @param translation The pose to translation to.
*/
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif // defined(__GNUC__) && !defined(__clang__)
void ResetTranslation(const Translation3d& translation) {
m_odometry.ResetTranslation(translation);
const std::optional<std::pair<units::second_t, VisionUpdate>>
latestVisionUpdate =
m_visionUpdates.empty() ? std::nullopt
: std::optional{*m_visionUpdates.crbegin()};
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();
if (latestVisionUpdate) {
// apply vision compensation to the pose rotation
const VisionUpdate visionUpdate{
Pose3d{translation, latestVisionUpdate->second.visionPose.Rotation()},
Pose3d{translation,
latestVisionUpdate->second.odometryPose.Rotation()}};
m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
} else {
m_poseEstimate = m_odometry.GetPose();
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif // defined(__GNUC__) && !defined(__clang__)
/**
* Resets the robot's rotation.
*
* @param rotation The rotation to reset to.
*/
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif // defined(__GNUC__) && !defined(__clang__)
void ResetRotation(const Rotation3d& rotation) {
m_odometry.ResetRotation(rotation);
const std::optional<std::pair<units::second_t, VisionUpdate>>
latestVisionUpdate =
m_visionUpdates.empty() ? std::nullopt
: std::optional{*m_visionUpdates.crbegin()};
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();
if (latestVisionUpdate) {
// apply vision compensation to the pose translation
const VisionUpdate visionUpdate{
Pose3d{latestVisionUpdate->second.visionPose.Translation(), rotation},
Pose3d{latestVisionUpdate->second.odometryPose.Translation(),
rotation}};
m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
} else {
m_poseEstimate = m_odometry.GetPose();
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif // defined(__GNUC__) && !defined(__clang__)
/**
* Gets the estimated robot pose.
@@ -451,7 +497,9 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
TimeInterpolatableBuffer<Pose3d> m_odometryPoseBuffer{kBufferDuration};
// Maps timestamps to vision updates
// Always contains one entry before the oldest entry in m_odometryPoseBuffer,
// unless there have been no vision measurements after the last reset
// unless there have been no vision measurements after the last reset. May
// contain one entry while m_odometryPoseBuffer is empty to correct for
// translation/rotation after a call to ResetRotation/ResetTranslation.
std::map<units::second_t, VisionUpdate> m_visionUpdates;
Pose3d m_poseEstimate;