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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-11-08 00:03:50 -08:00
parent 2109161534 b8d6bc2eb1
commit 161f8d107b
6 changed files with 90 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
cscore/src/main/native/windows/UsbCameraImpl.cpp
View File

@@ -255,8 +255,9 @@ bool UsbCameraImpl::CheckDeviceChange(WPARAM wParam, DEV_BROADCAST_HDR* pHdr,
}
void UsbCameraImpl::DeviceDisconnect() {
if (m_connectVerbose)
if (m_connectVerbose) {
SINFO("Disconnected from {}", m_path);
}
m_sourceReader.Reset();
m_mediaSource.Reset();
if (m_imageCallback) {
@@ -655,8 +656,9 @@ void UsbCameraImpl::DeviceCacheProperty(
rawProp->valueStr = perProp->valueStr; // copy
} else {
// Read current raw value and set percentage from it
if (!rawProp->DeviceGet(lock, pProcAmp))
if (!rawProp->DeviceGet(lock, pProcAmp)) {
SWARNING("failed to get property {}", rawProp->name);
}
if (perProp) {
perProp->SetValue(RawToPercentage(*rawProp, rawProp->value));
@@ -666,8 +668,9 @@ void UsbCameraImpl::DeviceCacheProperty(
// Set value on device if user-configured
if (rawProp->valueSet) {
if (!rawProp->DeviceSet(lock, pProcAmp))
if (!rawProp->DeviceSet(lock, pProcAmp)) {
SWARNING("failed to set property {}", rawProp->name);
}
}
// Update pointers since we released the lock

4
docs/build.gradle
View File

@@ -55,6 +55,10 @@ doxygen {
// https://frcmaven.wpi.edu/ui/native/generic-release-mirror/doxygen/, which
// is a mirror of binaries from https://doxygen.nl/download.html.
//
// To mirror a new Doxygen version, retrigger the GitHub Actions workflow in
// https://github.com/wpilibsuite/doxygen-mirror with the desired version
// number as an input.
//
// Ensure theme.css (from https://github.com/jothepro/doxygen-awesome-css)
// is compatible with Doxygen version when updating.
executables {

25
wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java
View File

@@ -9,8 +9,8 @@ import java.util.Optional;
import java.util.TreeMap;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Twist2d;
import org.wpilib.math.interpolation.TimeInterpolatableBuffer;
import org.wpilib.math.kinematics.Kinematics;
import org.wpilib.math.kinematics.Odometry;
@@ -270,20 +270,27 @@ public class PoseEstimator<T> {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision pose.
var twist = visionRobotPose.minus(visionSample.get()).log();
// Step 4: Measure the transform between the old pose estimate and the vision pose.
var transform = visionRobotPose.minus(visionSample.get());
// Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// Step 5: We should not trust the transform entirely, so instead we scale this transform by a
// Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
var k_times_transform =
m_visionK.times(
VecBuilder.fill(
transform.getX(), transform.getY(), transform.getRotation().getRadians()));
// Step 6: Convert back to Twist2d.
var scaledTwist =
new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
// Step 6: Convert back to Transform2d.
var scaledTransform =
new Transform2d(
k_times_transform.get(0, 0),
k_times_transform.get(1, 0),
Rotation2d.fromRadians(k_times_transform.get(2, 0)));
// Step 7: Calculate and record the vision update.
var visionUpdate =
new VisionUpdate(visionSample.get().plus(scaledTwist.exp()), odometrySample.get());
new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get());
m_visionUpdates.put(timestamp, visionUpdate);
// Step 8: Remove later vision measurements. (Matches previous behavior)

40
wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java
View File

@@ -11,9 +11,9 @@ import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.geometry.Twist3d;
import org.wpilib.math.interpolation.TimeInterpolatableBuffer;
import org.wpilib.math.kinematics.Kinematics;
import org.wpilib.math.kinematics.Odometry3d;
@@ -282,28 +282,36 @@ public class PoseEstimator3d<T> {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision pose.
var twist = visionRobotPose.minus(visionSample.get()).log();
// Step 4: Measure the transform between the old pose estimate and the vision pose.
var transform = visionRobotPose.minus(visionSample.get());
// Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// Step 5: We should not trust the transform entirely, so instead we scale this transform by a
// Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
var k_times_twist =
var k_times_transform =
m_visionK.times(
VecBuilder.fill(twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz));
VecBuilder.fill(
transform.getX(),
transform.getY(),
transform.getZ(),
transform.getRotation().getX(),
transform.getRotation().getY(),
transform.getRotation().getZ()));
// Step 6: Convert back to Twist3d.
var scaledTwist =
new Twist3d(
k_times_twist.get(0, 0),
k_times_twist.get(1, 0),
k_times_twist.get(2, 0),
k_times_twist.get(3, 0),
k_times_twist.get(4, 0),
k_times_twist.get(5, 0));
// Step 6: Convert back to Transform3d.
var scaledTransform =
new Transform3d(
k_times_transform.get(0, 0),
k_times_transform.get(1, 0),
k_times_transform.get(2, 0),
new Rotation3d(
k_times_transform.get(3, 0),
k_times_transform.get(4, 0),
k_times_transform.get(5, 0)));
// Step 7: Calculate and record the vision update.
var visionUpdate =
new VisionUpdate(visionSample.get().plus(scaledTwist.exp()), odometrySample.get());
new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get());
m_visionUpdates.put(timestamp, visionUpdate);
// Step 8: Remove later vision measurements. (Matches previous behavior)

32
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp
View File

@@ -12,6 +12,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/math/geometry/Transform2d.hpp"
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/math/interpolation/TimeInterpolatableBuffer.hpp"
#include "wpi/math/kinematics/Kinematics.hpp"
@@ -261,25 +262,26 @@ class WPILIB_DLLEXPORT PoseEstimator {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision
// pose.
auto twist = (visionRobotPose - visionSample.value()).Log();
// Step 4: Measure the transform between the old pose estimate and the
// vision transform.
auto transform = visionRobotPose - visionSample.value();
// Step 5: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
Eigen::Vector3d k_times_twist =
m_visionK * Eigen::Vector3d{twist.dx.value(), twist.dy.value(),
twist.dtheta.value()};
// Step 5: We should not trust the transform entirely, so instead we scale
// this transform by a Kalman gain matrix representing how much we trust
// vision measurements compared to our current pose.
Eigen::Vector3d k_times_transform =
m_visionK * Eigen::Vector3d{transform.X().value(),
transform.Y().value(),
transform.Rotation().Radians().value()};
// Step 6: Convert back to Twist2d.
Twist2d scaledTwist{wpi::units::meter_t{k_times_twist(0)},
wpi::units::meter_t{k_times_twist(1)},
wpi::units::radian_t{k_times_twist(2)}};
// Step 6: Convert back to Transform2d.
Transform2d scaledTransform{
wpi::units::meter_t{k_times_transform(0)},
wpi::units::meter_t{k_times_transform(1)},
Rotation2d{wpi::units::radian_t{k_times_transform(2)}}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),
*odometrySample};
VisionUpdate visionUpdate{*visionSample + scaledTransform, *odometrySample};
m_visionUpdates[timestamp] = visionUpdate;
// Step 8: Remove later vision measurements. (Matches previous behavior)

42
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp
View File

@@ -13,6 +13,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/math/geometry/Transform3d.hpp"
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/math/interpolation/TimeInterpolatableBuffer.hpp"
#include "wpi/math/kinematics/Kinematics.hpp"
@@ -270,29 +271,32 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision
// pose.
auto twist = (visionRobotPose - visionSample.value()).Log();
// Step 4: Measure the transform between the old pose estimate and the
// vision pose.
auto transform = visionRobotPose - visionSample.value();
// Step 5: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
wpi::math::Vectord<6> k_times_twist =
m_visionK * wpi::math::Vectord<6>{twist.dx.value(), twist.dy.value(),
twist.dz.value(), twist.rx.value(),
twist.ry.value(), twist.rz.value()};
// Step 5: We should not trust the transform entirely, so instead we scale
// this transform by a Kalman gain matrix representing how much we trust
// vision measurements compared to our current pose.
wpi::math::Vectord<6> k_times_transform =
m_visionK * wpi::math::Vectord<6>{transform.X().value(),
transform.Y().value(),
transform.Z().value(),
transform.Rotation().X().value(),
transform.Rotation().Y().value(),
transform.Rotation().Z().value()};
// Step 6: Convert back to Twist3d.
Twist3d scaledTwist{wpi::units::meter_t{k_times_twist(0)},
wpi::units::meter_t{k_times_twist(1)},
wpi::units::meter_t{k_times_twist(2)},
wpi::units::radian_t{k_times_twist(3)},
wpi::units::radian_t{k_times_twist(4)},
wpi::units::radian_t{k_times_twist(5)}};
// Step 6: Convert back to Transform3d.
Transform3d scaledTransform{
wpi::units::meter_t{k_times_transform(0)},
wpi::units::meter_t{k_times_transform(1)},
wpi::units::meter_t{k_times_transform(2)},
Rotation3d{wpi::units::radian_t{k_times_transform(3)},
wpi::units::radian_t{k_times_transform(4)},
wpi::units::radian_t{k_times_transform(5)}}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),
*odometrySample};
VisionUpdate visionUpdate{*visionSample + scaledTransform, *odometrySample};
m_visionUpdates[timestamp] = visionUpdate;
// Step 8: Remove later vision measurements. (Matches previous behavior)