diff --git a/cscore/src/main/native/windows/UsbCameraImpl.cpp b/cscore/src/main/native/windows/UsbCameraImpl.cpp index d9d77bfeae..60ed6b84bb 100644 --- a/cscore/src/main/native/windows/UsbCameraImpl.cpp +++ b/cscore/src/main/native/windows/UsbCameraImpl.cpp @@ -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 diff --git a/docs/build.gradle b/docs/build.gradle index 773059b6f7..42badb1f43 100644 --- a/docs/build.gradle +++ b/docs/build.gradle @@ -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 { diff --git a/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java b/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java index 00220a5b88..f1967f7a81 100644 --- a/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java @@ -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 { 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) diff --git a/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java index 15722b00e9..6406d5dc5e 100644 --- a/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java +++ b/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java @@ -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 { 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) diff --git a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp index 9ac8ed792e..3e7e98367a 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp @@ -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) diff --git a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp index 1c6dc8ac35..29bd3f3be7 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp @@ -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)