allwpilib
allwpilib copied to clipboard
wpilibsuite
`SwerveDrivePoseEstimator` returns infinity occasionally
Describe the bug
When using SwerveDrivePoseEstimator we will get infinite values returned occasionally.
To Reproduce I can't really provide good reproduction instructions for this as its being tested on our robot.
- Link to code: https://github.com/Superbro525Alt/2024-Crescendo/tree/isaac/auto
Expected behavior
SwerveDrivePoseEstimator should return normal/correct values.
Desktop (please complete the following information):
- OS: Linux Athena
- Project Information: WPILib Information: Project Version: 2024.2.1 VS Code Version: 1.85.1 WPILib Extension Version: 2024.1.1 C++ Extension Version: 1.19.1 Java Extension Version: 1.28.2024021408 Java Debug Extension Version: 0.56.2024020601 Java Dependencies Extension Version 0.23.2023120100 Java Version: 17 Java Location: /home/spacey_sooty/wpilib/2024/jdk Vendor Libraries: PathplannerLib (2024.2.3) CTRE-Phoenix (v5) (5.33.0) CTRE-Phoenix (v6) (24.1.0) REVLib (2024.2.0) WPILib-New-Commands (1.0.0) photonlib (v2024.1.1-beta-3.2)
Additional context I'll try do a reproduction project for this if I get time.
Can you log both the inputs and outputs to the estimator? That would enable us to reproduce the input sequence.
:+1: I can do that next time I've with the robot (submitting a bug report from something we noticed last night)
Haven't had an opportunity to add an logging for the inputs and outputs but it appears to return infinity when the values get very close to 0. This could be that they are rounding to 0 then getting divided?
I can reproduce. We had a similar issue with our robot code and ended up solving it through a combination of heavily trusting vision and throwing out any values outside of the field dimensions.
Could you please send the code that reproduced the issue? I'm glad that there's a workaround, but I'd like to see if there's a root cause that can/should be fixed.
