Skip to content

Commit

Permalink
Merge pull request #52 from CrossTheRoadElec/swerve-add-vision-measur…
Browse files Browse the repository at this point in the history
…ement

Add override for `SwerveDrivetrain::addVisionMeasurement` to convert timebase
bhall-ctre authored Jan 22, 2025
2 parents 40222f4 + c480443 commit 2e1a33d
Showing 7 changed files with 126 additions and 8 deletions.
2 changes: 1 addition & 1 deletion cpp/SwerveWithPathPlanner/src/main/cpp/Robot.cpp
Original file line number Diff line number Diff line change
@@ -28,7 +28,7 @@ void Robot::RobotPeriodic() {
LimelightHelpers::SetRobotOrientation("limelight", heading.value(), 0, 0, 0, 0, 0);
auto llMeasurement = LimelightHelpers::getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
if (llMeasurement && llMeasurement->tagCount > 0 && units::math::abs(omega) < 2_tps) {
m_container.drivetrain.AddVisionMeasurement(llMeasurement->pose, utils::FPGAToCurrentTime(llMeasurement->timestampSeconds));
m_container.drivetrain.AddVisionMeasurement(llMeasurement->pose, llMeasurement->timestampSeconds);
}
}
}
Original file line number Diff line number Diff line change
@@ -254,6 +254,38 @@ class CommandSwerveDrivetrain : public frc2::SubsystemBase, public TunerSwerveDr
return m_sysIdRoutineToApply->Dynamic(direction);
}

/**
* \brief Adds a vision measurement to the Kalman Filter. This will correct the
* odometry pose estimate while still accounting for measurement noise.
*
* \param visionRobotPose The pose of the robot as measured by the vision camera.
* \param timestamp The timestamp of the vision measurement in seconds.
*/
void AddVisionMeasurement(frc::Pose2d visionRobotPose, units::second_t timestamp) override
{
TunerSwerveDrivetrain::AddVisionMeasurement(std::move(visionRobotPose), utils::FPGAToCurrentTime(timestamp));
}

/**
* \brief Adds a vision measurement to the Kalman Filter. This will correct the
* odometry pose estimate while still accounting for measurement noise.
*
* Note that the vision measurement standard deviations passed into this method
* will continue to apply to future measurements until a subsequent call to
* #SetVisionMeasurementStdDevs or this method.
*
* \param visionRobotPose The pose of the robot as measured by the vision camera.
* \param timestamp The timestamp of the vision measurement in seconds.
* \param visionMeasurementStdDevs Standard deviations of the vision pose measurement.
*/
void AddVisionMeasurement(
frc::Pose2d visionRobotPose,
units::second_t timestamp,
std::array<double, 3> visionMeasurementStdDevs) override
{
TunerSwerveDrivetrain::AddVisionMeasurement(std::move(visionRobotPose), utils::FPGAToCurrentTime(timestamp), visionMeasurementStdDevs);
}

private:
void ConfigureAutoBuilder();
void StartSimThread();
4 changes: 1 addition & 3 deletions java/SwerveWithChoreo/src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
@@ -4,8 +4,6 @@

package frc.robot;

import com.ctre.phoenix6.Utils;

import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
@@ -42,7 +40,7 @@ public void robotPeriodic() {
LimelightHelpers.SetRobotOrientation("limelight", headingDeg, 0, 0, 0, 0, 0);
var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) {
m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, Utils.fpgaToCurrentTime(llMeasurement.timestampSeconds));
m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds);
}
}
}
Original file line number Diff line number Diff line change
@@ -15,6 +15,7 @@
import choreo.trajectory.SwerveSample;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
@@ -315,4 +316,38 @@ private void startSimThread() {
});
m_simNotifier.startPeriodic(kSimLoopPeriod);
}

/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds.
*/
@Override
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds));
}

/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
* <p>
* Note that the vision measurement standard deviations passed into this method
* will continue to apply to future measurements until a subsequent call to
* {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds.
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
* in the form [x, y, theta]ᵀ, with units in meters and radians.
*/
@Override
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs
) {
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs);
}
}
Original file line number Diff line number Diff line change
@@ -4,8 +4,6 @@

package frc.robot;

import com.ctre.phoenix6.Utils;

import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
@@ -42,7 +40,7 @@ public void robotPeriodic() {
LimelightHelpers.SetRobotOrientation("limelight", headingDeg, 0, 0, 0, 0, 0);
var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) {
m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, Utils.fpgaToCurrentTime(llMeasurement.timestampSeconds));
m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds);
}
}
}
Original file line number Diff line number Diff line change
@@ -16,6 +16,7 @@
import com.pathplanner.lib.controllers.PPHolonomicDriveController;

import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
@@ -290,4 +291,38 @@ private void startSimThread() {
});
m_simNotifier.startPeriodic(kSimLoopPeriod);
}

/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds.
*/
@Override
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds));
}

/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
* <p>
* Note that the vision measurement standard deviations passed into this method
* will continue to apply to future measurements until a subsequent call to
* {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds.
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
* in the form [x, y, theta]ᵀ, with units in meters and radians.
*/
@Override
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs
) {
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs);
}
}
Original file line number Diff line number Diff line change
@@ -7,7 +7,7 @@
from typing import Callable, overload
from wpilib import DriverStation, Notifier, RobotController
from wpilib.sysid import SysIdRoutineLog
from wpimath.geometry import Rotation2d
from wpimath.geometry import Pose2d, Rotation2d


class CommandSwerveDrivetrain(Subsystem, swerve.SwerveDrivetrain):
@@ -329,3 +329,23 @@ def _sim_periodic():
self._last_sim_time = utils.get_current_time_seconds()
self._sim_notifier = Notifier(_sim_periodic)
self._sim_notifier.startPeriodic(self._SIM_LOOP_PERIOD)

def add_vision_measurement(self, vision_robot_pose: Pose2d, timestamp: units.second, vision_measurement_std_devs: tuple[float, float, float] | None = None):
"""
Adds a vision measurement to the Kalman Filter. This will correct the
odometry pose estimate while still accounting for measurement noise.
Note that the vision measurement standard deviations passed into this method
will continue to apply to future measurements until a subsequent call to
set_vision_measurement_std_devs or this method.
:param vision_robot_pose: The pose of the robot as measured by the vision camera.
:type vision_robot_pose: Pose2d
:param timestamp: The timestamp of the vision measurement in seconds.
:type timestamp: second
:param vision_measurement_std_devs: Standard deviations of the vision pose measurement
in the form [x, y, theta]ᵀ, with units in meters
and radians.
:type vision_measurement_std_devs: tuple[float, float, float] | None
"""
swerve.SwerveDrivetrain.add_vision_measurement(self, vision_robot_pose, utils.fpga_to_current_time(timestamp), vision_measurement_std_devs)

0 comments on commit 2e1a33d

Please sign in to comment.