This PR updates the existing differentialdriveposeestimator example to include computer vision pose estimation and latency compensation.
The example generates a simulated cameraToTarget transformation, which is then fed into ComputerVisionUtil.objectToRobotPose() to compute the robot's field-relative position exclusively from vision measurements. The vision measurements are applied through DifferentialDrivePoseEstimator.addVisionMeasurement().
The updated example constructs an AprilTagFieldLayout from JSON. This requires a deploy directory, something which isn't currently supported in wpilibjExamples and wpilibcExamples.
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>
Refactor some examples to use newer features, such as HID factories, library-provided command factories, CommandPtr (C++), as well as new idioms such as static/instance command factories.
Motivation
Feedback from 2022 showed that the Trigger API is rather confusing, mostly due to the following:
- duplicate Trigger and Button APIs were available; users were confused searching for a nonexistent difference between them.
- the when terminology was ambiguous and unclear whether it refers to the high state or specifically the rising edge.
- the Active terminology didn't unambiguously refer to the high state; it wasn't unintuitive to understand it as "when the binding is active/polled".
- whileHeld vs whenHeld was very confusing, and the difference between them wasn't obvious. The parallel Trigger verbs, whileActiveContinuously and whileActiveOnce are much less confusing.
Solution
Deprecating Button and its binding methods. The rationale for deprecating Button (and not Trigger) is because Button uses terminology that is needlessly more specific and restricting to the button use case, making the use case of arbitrary trigger conditions unintuitive.
After consideration, deprecation of Button's subclasses was decided against:
- NetworkButton (a trigger condition based on a boolean NT entry/topic) is a use case that is not necessarily intuitive for teams to implement themselves, so it is an abstraction that should be provided in the library. A parallel class for the BooleanEvent level, NetworkBooleanEvent, was also added as part of NT4. NT listeners were considered as a alternative solution, but they require attention to thread safety, and aren't interoperable with the EventLoop API.
- JoystickButton/POVButton provide abstractions around HID buttons. The new Trigger-returning factories on the HID classes are an equal (if not more concise) alternative, but there is no reason not to keep them for those who find their use preferable.
At a later date in the deprecation cycle (perhaps for 2024), when Button is removed, these subclasses should be changed to inherit directly from Trigger.
Trigger's bindings are changed to use True/False terminology, as it should be unambiguous. Each binding type has both True and False variants; for brevity, only the True variants are listed here:
- onTrue (replaces whenActive): schedule on rising edge.
- whileTrue (replaces whileActiveOnce): schedule on rising edge, cancel on falling edge.
- toggleOnTrue (replaces toggleWhenActive): on rising edge, schedule if unscheduled and cancel if scheduled.
Two binding types are completely deprecated:
- cancelWhenActive: this is a fairly niche use case which is better described as having the trigger's rising edge (Trigger.rising()) as an end condition for the command (using Command.until()).
- whileActiveContinuously: however common, this relied on the no-op behavior of scheduling an already-scheduled command. The more correct way to repeat the command if it ends before the falling edge is using Command.repeatedly/RepeatCommand or a RunCommand -- the only difference is if the command is interrupted, but that is more likely to result in two commands perpetually canceling each other than achieve the desired behavior. Manually implementing a blindly-scheduling binding like whileActiveContinuously is still possible, though might not be intuitive.
Notes
It was considered to share BooleanEvent's digital signal terminology; however, once it was decided that Trigger should not inherit from BooleanEvent (due to overload incompatibility) the common terminology was not worth the unintuitiveness stemming from users' unfamiliarity with the signal processing terms.
All trigonometric functions and vector classes assume North-West-Up axes
convention, so using North-East-Down convention with them is really
error-prone. We've broken something every time we touched the drive
classes.
We originally used North-East-Down to match the joystick convention, but
the volume of long-lived bugs has made this not worth it in retrospect.
The rest of WPILib also uses North-West-Up, so this makes things
consistent.
KilloughDrive was removed since no one uses it.
The current DS thread model has some pretty major issues. It makes it difficult to know if all data is from the same remote packet, and if the data changes while the robot loop is running. Additionally, the DS thread is used for a few other things (MotorSafety and State Tracking for EducationalRobot). This also makes sim difficult, as user code has to wait for the thread to know it has new data.
This change completely rethinks how threading works in the driver station model.
First, the DS HAL system receives a new data callback, either from Netcomm or DriverStationSim. Inside the context of this callback, all the low latency data is read and put into a cache. Doing some investigation on the robot side, this is perfectly safe to do, and also ensures a ds packet will not be parsed before we finish reading the current packet data.
After all data is read, the cache is swapped with a 2nd buffer. This buffer just stores the data, none of the HAL DS calls read from this buffer. An event is then fired, stating there is new data ready to go.
Robot code calls HAL_UpdateDSData(). This swaps the 2nd buffer with a 3rd buffer, which always contains the current data. This data will not be updated until HAL_UpdateDSData is called again. Which solves the state problem.
The high level driver station classes have. an updateData() call, which calls HAL_UpdateDSData, and then update button state variables, then data log and update the NT FMS data table (Java also caches across the JNI boundary here, but that could trivially be removed). An extra event provider is provided, allowing other threads to know when this call has been completed.
IterativeRobotBase calls DS.updateData() at the beginning of each loop, and only once per loop. This means all commands will always have the same state.
All of this means there is no longer a DS thread. Everything happens synchronously. This means Sim and testing is easier, as you can just call DriverStationSim.NotifyNewData(), and then DriverStation.UpdateData(), and you can guarantee that all the DriverStation.*** data is up to date.
As for Motor Safety and Educational Robot State Handling, those can all be handled by their own threads. The Educational Thread only needs to run under EducationalRobot, and MotorSafety will only be started if there is a motor safety object enabled.
* Use explicit this capture required by C++20
* Use C++20 span
* Replace wpi::numbers with std::numbers
* Fix C++20 clang-tidy warning false positive in fmt
* Remove ciso646 include since C++20 removed that header
* Fix global-buffer-overflow asan warnings in ntcore tests
* Add DIOSetProxy constructor to HAL
* Upgrade MSVC compiler to 2022
* Bump native-utils to 2023.2.7 (changes to std=c++20)
Co-authored-by: Peter Johnson <johnson.peter@gmail.com>
Add a CommandPtr with an internal unique_ptr to enable not needing to move the underlying classes, which is error-prone due to the potential for lambda captures.
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.
This shows more real world usage then hardcoding the setpoint and PID
gains. There were no current examples using Preferences. This will also
be used to update frc-docs article for Preferences.
