Skip to content

Commit

Permalink
Add camera simulation (#39)
Browse files Browse the repository at this point in the history
  • Loading branch information
@doinkythederp
doinkythederp authored Jan 16, 2025
2 parents 232c996 + de7b392 commit f26ba8b
Show file tree
Hide file tree
Showing 4 changed files with 194 additions and 75 deletions.
80 changes: 49 additions & 31 deletions src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/AbsolutePose.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,7 @@ import com.frcteam3636.frc2025.utils.QuestNav
import edu.wpi.first.math.Matrix
import edu.wpi.first.math.VecBuilder
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator
import edu.wpi.first.math.geometry.Pose2d
import edu.wpi.first.math.geometry.Pose3d
import edu.wpi.first.math.geometry.Rotation2d
import edu.wpi.first.math.geometry.Transform2d
import edu.wpi.first.math.geometry.*
import edu.wpi.first.math.numbers.N1
import edu.wpi.first.math.numbers.N3
import edu.wpi.first.units.Units.*
Expand All @@ -24,6 +21,10 @@ import edu.wpi.first.wpilibj.Alert.AlertType
import edu.wpi.first.wpilibj.Timer
import org.littletonrobotics.junction.LogTable
import org.littletonrobotics.junction.inputs.LoggableInputs
import org.photonvision.PhotonCamera
import org.photonvision.PhotonPoseEstimator
import org.photonvision.simulation.PhotonCameraSim
import org.photonvision.simulation.SimCameraProperties
import org.team9432.annotation.Logged
import java.nio.ByteBuffer

Expand All @@ -38,16 +39,20 @@ class AbsolutePoseProviderInputs : LoggableInputs {
*/
var connected = false

var observedTags: IntArray = intArrayOf()

override fun toLog(table: LogTable) {
if (measurement != null) {
table.put("Measurement", measurement)
}
table.put("Connected", connected)
table.put("ObservedTags", observedTags)
}

override fun fromLog(table: LogTable) {
measurement = table.get("Measurement", measurement)[0]
connected = table.get("Connected", connected)
observedTags = table.get("ObservedTags", observedTags)
}
}

Expand Down Expand Up @@ -161,6 +166,44 @@ class LimelightPoseProvider(
}
}

@Suppress("unused")
class CameraSimPoseProvider(name: String, val chassisToCamera: Transform3d) : AbsolutePoseProvider {
private val camera = PhotonCamera(name)
private val simProperties = SimCameraProperties().apply {
setCalibration(1280, 800, Rotation2d(LIMELIGHT_FOV))
fps = 20.0
avgLatencyMs = 51.0
latencyStdDevMs = 5.0
}
val sim = PhotonCameraSim(camera, simProperties)

private val estimator =
PhotonPoseEstimator(
APRIL_TAGS,
PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
chassisToCamera
)

override fun updateInputs(inputs: AbsolutePoseProviderInputs) {
inputs.connected = true
inputs.measurement = null
val unreadResults = camera.allUnreadResults
val latestResult = unreadResults.lastOrNull()
if (latestResult != null) {
estimator.update(latestResult).ifPresent {
inputs.measurement = AbsolutePoseMeasurement(
it.estimatedPose.toPose2d(),
Seconds.of(it.timestampSeconds),
VecBuilder.fill(0.7, 0.7, 9999999.0)
)
}
inputs.observedTags = latestResult.targets.map {
it.fiducialId
}.toIntArray()
}
}
}

@Logged
open class QuestNavInputs {
/**
Expand Down Expand Up @@ -197,32 +240,6 @@ class QuestNavLocalizer(
}
}

//@Suppress("unused")
//class PhotonVisionPoseIOReal(name: String, chassisToCamera: Transform3d) : AbsolutePoseIO {
// private val camera = PhotonCamera(name).apply { driverMode = false }
// private val estimator =
// PhotonPoseEstimator(
// APRIL_TAG_FIELD_LAYOUT,
// PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
// camera,
// chassisToCamera
// )
//
// override fun updateInputs(inputs: AbsolutePoseIO.Inputs) {
// inputs.measurement = null
// estimator.update().ifPresent {
// inputs.measurement = AbsolutePoseMeasurement(
// it.estimatedPose,
// it.timestampSeconds,
// APRIL_TAG_STD_DEV(it.estimatedPose.translation.norm, it.targetsUsed.size)
// )
// }
// }
//
// override val cameraConnected
// get() = camera.isConnected
//}

data class AbsolutePoseMeasurement(
val pose: Pose2d,
val timestamp: Time,
Expand Down Expand Up @@ -272,7 +289,6 @@ class AbsolutePoseMeasurementStruct : Struct<AbsolutePoseMeasurement> {
}
}

//internal val APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile)

//internal const val APRIL_TAG_AMBIGUITY_FILTER = 0.3
//internal val APRIL_TAG_STD_DEV = { distance: Double, count: Int ->
Expand All @@ -283,3 +299,5 @@ class AbsolutePoseMeasurementStruct : Struct<AbsolutePoseMeasurement> {
// translationalStdDev, translationalStdDev, rotationalStdDev
// )
//}

val LIMELIGHT_FOV = Degrees.of(75.76079874010732)
98 changes: 54 additions & 44 deletions src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,7 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController
import com.pathplanner.lib.pathfinding.Pathfinding
import edu.wpi.first.math.VecBuilder
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator
import edu.wpi.first.math.geometry.Pose2d
import edu.wpi.first.math.geometry.Rotation2d
import edu.wpi.first.math.geometry.Transform2d
import edu.wpi.first.math.geometry.Translation2d
import edu.wpi.first.math.geometry.*
import edu.wpi.first.math.kinematics.ChassisSpeeds
import edu.wpi.first.math.kinematics.SwerveDriveKinematics
import edu.wpi.first.math.kinematics.SwerveModuleState
Expand Down Expand Up @@ -62,12 +59,18 @@ object Drivetrain : Subsystem, Sendable {
private val questNavInputs = LoggedQuestNavInputs()
private var questNavCalibrated = false

private val absolutePoseIOs = mapOf(
"Limelight" to LimelightPoseProvider(
"limelight",
algorithm = LimelightAlgorithm.MegaTag2 { inputs }
),
).mapValues { Pair(it.value, AbsolutePoseProviderInputs()) }
private val absolutePoseIOs = when (Robot.model) {
Robot.Model.SIMULATION -> mapOf(
"Limelight" to CameraSimPoseProvider("limelight", Transform3d()),
)

else -> mapOf(
"Limelight" to LimelightPoseProvider(
"limelight",
algorithm = LimelightAlgorithm.MegaTag2 { inputs },
),
)
}.mapValues { Pair(it.value, AbsolutePoseProviderInputs()) }

/** Helper for converting a desired drivetrain velocity into the speeds and angles for each swerve module */
private val kinematics =
Expand Down Expand Up @@ -113,7 +116,7 @@ object Drivetrain : Subsystem, Sendable {
this::measuredChassisSpeeds,
this::desiredChassisSpeeds::set,
PPHolonomicDriveController(
when(Robot.model) {
when (Robot.model) {
Robot.Model.SIMULATION -> DRIVING_PID_GAINS_TALON
Robot.Model.COMPETITION -> DRIVING_PID_GAINS_TALON
Robot.Model.PROTOTYPE -> DRIVING_PID_GAINS_NEO
Expand All @@ -126,10 +129,10 @@ object Drivetrain : Subsystem, Sendable {
this
)

if (io is DrivetrainIOSim){
if (io is DrivetrainIOSim) {
poseEstimator.resetPose(io.swerveDriveSimulation.simulatedDriveTrainPose)
io.registerPoseProviders(absolutePoseIOs.values.map { it.first })
}

}

override fun periodic() {
Expand Down Expand Up @@ -168,6 +171,14 @@ object Drivetrain : Subsystem, Sendable {
Logger.recordOutput("Drivetrain/Chassis Speeds", measuredChassisSpeeds)
Logger.recordOutput("Drivetrain/Localizer", localizer.name)
questNavInactiveAlert.set(localizer != Localizer.QuestNav)

Logger.recordOutput(
"Drivetrain/TagPoses", *APRIL_TAGS.tags
.filter { tag ->
absolutePoseIOs.values.any { it.second.observedTags.contains(tag.ID) }
}
.map { it.pose }
.toTypedArray())
}

/** The desired speeds and angles of the swerve modules. */
Expand Down Expand Up @@ -351,25 +362,25 @@ object Drivetrain : Subsystem, Sendable {
val MODULE_POSITIONS =
PerCorner(
frontLeft =
Pose2d(
Translation2d(WHEEL_BASE, TRACK_WIDTH) / 2.0,
Rotation2d.fromDegrees(0.0)
),
Pose2d(
Translation2d(WHEEL_BASE, TRACK_WIDTH) / 2.0,
Rotation2d.fromDegrees(0.0)
),
frontRight =
Pose2d(
Translation2d(WHEEL_BASE, -TRACK_WIDTH) / 2.0,
Rotation2d.fromDegrees(270.0)
),
Pose2d(
Translation2d(WHEEL_BASE, -TRACK_WIDTH) / 2.0,
Rotation2d.fromDegrees(270.0)
),
backLeft =
Pose2d(
Translation2d(-WHEEL_BASE, TRACK_WIDTH) / 2.0,
Rotation2d.fromDegrees(90.0)
),
Pose2d(
Translation2d(-WHEEL_BASE, TRACK_WIDTH) / 2.0,
Rotation2d.fromDegrees(90.0)
),
backRight =
Pose2d(
Translation2d(-WHEEL_BASE, -TRACK_WIDTH) / 2.0,
Rotation2d.fromDegrees(180.0)
),
Pose2d(
Translation2d(-WHEEL_BASE, -TRACK_WIDTH) / 2.0,
Rotation2d.fromDegrees(180.0)
),
)

// Chassis Control
Expand All @@ -378,7 +389,7 @@ object Drivetrain : Subsystem, Sendable {

val ROTATION_PID_GAINS = PIDGains(3.0, 0.0, 0.4)

// // Pathing
// // Pathing
// val DEFAULT_PATHING_CONSTRAINTS =
// PathConstraints(FREE_SPEED.baseUnitMagnitude(), 3.879, ROTATION_SPEED.baseUnitMagnitude(), 24.961)
// FIXME: Update for 2025
Expand Down Expand Up @@ -414,14 +425,13 @@ object Drivetrain : Subsystem, Sendable {
}.toTypedArray()
)

val PP_ROBOT_CONFIG = when(Robot.model) {
val PP_ROBOT_CONFIG = when (Robot.model) {
Robot.Model.SIMULATION -> PP_ROBOT_CONFIG_COMP
Robot.Model.COMPETITION -> PP_ROBOT_CONFIG_COMP
Robot.Model.PROTOTYPE -> PP_ROBOT_CONFIG_PROTOTYPE
}



// CAN IDs
val KRAKEN_MODULE_CAN_IDS =
PerCorner(
Expand Down Expand Up @@ -450,25 +460,25 @@ object Drivetrain : Subsystem, Sendable {
internal val MODULE_CAN_IDS_PRACTICE =
PerCorner(
frontLeft =
Pair(
REVMotorControllerId.FrontLeftDrivingMotor,
REVMotorControllerId.FrontLeftTurningMotor
),
Pair(
REVMotorControllerId.FrontLeftDrivingMotor,
REVMotorControllerId.FrontLeftTurningMotor
),
frontRight =
Pair(
REVMotorControllerId.FrontRightDrivingMotor,
REVMotorControllerId.FrontRightTurningMotor
),
Pair(
REVMotorControllerId.FrontRightDrivingMotor,
REVMotorControllerId.FrontRightTurningMotor
),
backLeft =
Pair(
REVMotorControllerId.BackLeftDrivingMotor,
REVMotorControllerId.BackLeftTurningMotor
),
backRight =
Pair(
REVMotorControllerId.BackRightDrivingMotor,
REVMotorControllerId.BackRightTurningMotor
),
Pair(
REVMotorControllerId.BackRightDrivingMotor,
REVMotorControllerId.BackRightTurningMotor
),
)

/** A position with the modules radiating outwards from the center of the robot, preventing movement. */
Expand Down
20 changes: 20 additions & 0 deletions src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/DrivetrainIO.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,8 @@ import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.WHEEL_
import com.frcteam3636.frc2025.utils.math.TAU
import com.frcteam3636.frc2025.utils.swerve.PerCorner
import com.studica.frc.AHRS
import edu.wpi.first.apriltag.AprilTagFieldLayout
import edu.wpi.first.apriltag.AprilTagFields
import edu.wpi.first.math.geometry.Pose2d
import edu.wpi.first.math.geometry.Rotation2d
import edu.wpi.first.math.kinematics.SwerveModulePosition
Expand All @@ -24,6 +26,7 @@ import org.ironmaple.simulation.drivesims.SwerveDriveSimulation
import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig
import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig
import org.littletonrobotics.junction.Logger
import org.photonvision.simulation.VisionSystemSim
import org.team9432.annotation.Logged

@Logged
Expand Down Expand Up @@ -138,6 +141,10 @@ class DrivetrainIOSim : DrivetrainIO() {
Pose2d(3.0, 3.0, Rotation2d())
)

val vision = VisionSystemSim("main").apply {
addAprilTags(APRIL_TAGS)
}

override val modules = PerCorner.generate { SimSwerveModule(swerveDriveSimulation.modules[it.ordinal]) }
override val gyro = GyroMapleSim(swerveDriveSimulation.gyroSimulation)

Expand All @@ -147,8 +154,21 @@ class DrivetrainIOSim : DrivetrainIO() {

override fun updateInputs(inputs: DrivetrainInputs) {
super.updateInputs(inputs)
vision.update(swerveDriveSimulation.simulatedDriveTrainPose)
Logger.recordOutput("FieldSimulation/RobotPosition", swerveDriveSimulation.simulatedDriveTrainPose)
}

fun registerPoseProviders(providers: Iterable<AbsolutePoseProvider>) {
for (provider in providers) {
if (provider is CameraSimPoseProvider) {
vision.addCamera(provider.sim, provider.chassisToCamera)
}
}
}

// Register the drivetrain simulation to the default simulation world
}

val APRIL_TAGS = AprilTagFieldLayout.loadFromResource(
AprilTagFields.k2025Reefscape.m_resourceFile
)!!
Loading

0 comments on commit f26ba8b

Please sign in to comment.