This required changing the constant values (e.g. kSize) into functions
(e.g. GetSize()).
Fixed implementations of ForEachNested to be inline (as these are actually
templates).
Also added a ntcore Struct test.
This implements de/serialization for the types that aren't templated (SwerveDriveKinematics) in C++ or where there is no trivial way to go round-trip (Splines) for the messages.
Adds a reset method where teams can pass in module headings for the kinematics object to use if it gets an all-zero ChassisSpeeds while converting ChassisSpeeds to module states. Also removes internal states array, replacing it with an internal headings array.
This effectively replaces the Unscented Kalman Filter used for Pose Estimation with the Odometry model, and uses a recalculable Kalman gain matrix to update pose estimations whenever a vision measurement is added.
Notably, this change removes the need for the confusing generics used in Java, and the C++ implementation got quite a bit less complex as well.
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Now, implicit narrowing conversions are only used with wpi::Now(). This
also fixes clang-tidy warnings about C-style casts. For example:
```
== clang-tidy /__w/allwpilib/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc ==
/__w/allwpilib/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc:95:18: warning: C-style casts are discouraged; use static_cast/const_cast/reinterpret_cast [google-readability-casting]
auto curTime = units::second_t(m_timer.Get());
^
```
In that case at least, the cast was removed entirely since Get() already
returns a units::second_t.
* Calculated swerve module states now stored in a member variable
* If ChassisSpeeds(0, 0, 0) is converted to module speeds, the
previously calculated module angle will be conserved, with forward speed
set to 0
* New tests added
* Replace Matrix<> with Vector<> where vectors are explicitly intended.
I found these via `rg "Eigen::Matrix<double, \w+, 1>"`.
* Pass all Eigen matrices by const reference. I found these via `rg
"\(Eigen"` on main (the initializer list constructors make more false
positives).
* Replace MakeMatrix() and operator<< usage with initializer list
constructors. I found these via `rg MakeMatrix` and `rg "<<"`
respectively.
* Deprecate MakeMatrix()
The wpimath APIs use std::array, which doesn't do size checking. Passing
an array with the wrong size can result in uninitialized elements
instead of a compilation error.
This is a breaking change but is worthwhile to avoid hard-to-debug errors.
Pose and state estimators can filter latency-compensated global measurements and fuse them with state-space drivetrain model information to estimate robot position. They are drop-in replacements for the existing odometry classes.
Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
