Skip to content

Commit

Permalink
Merge pull request #16 from thedropbears/james/vision-sim
Browse files Browse the repository at this point in the history
Enable vision sim
  • Loading branch information
james-ward authored Nov 21, 2024
2 parents f22ae65 + e086540 commit 4b55927
Show file tree
Hide file tree
Showing 4 changed files with 52 additions and 56 deletions.
67 changes: 32 additions & 35 deletions components/vision.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@
import wpilib
import wpiutil.log
from magicbot import tunable, feedback
from photonlibpy.photonCamera import PhotonCamera
from photonlibpy.targeting.photonTrackedTarget import PhotonTrackedTarget
from photonlibpy import PhotonCamera
from photonlibpy.targeting import PhotonTrackedTarget
from wpimath import objectToRobotPose
from wpimath.geometry import Pose2d, Rotation3d, Transform3d, Translation3d, Pose3d

Expand All @@ -32,7 +32,7 @@ class VisualLocalizer:
last_pose_z = tunable(0.0, writeDefault=False)
linear_vision_uncertainty = tunable(0.3)
rotation_vision_uncertainty = tunable(0.08)
reproj_error_threshold = 1
reproj_error_threshold = 0.1

def __init__(
self,
Expand All @@ -51,45 +51,44 @@ def __init__(
self.camera_to_robot = self.robot_to_camera.inverse()
self.last_timestamp = -1
self.last_recieved_timestep = -1.0

self.single_best_log = field.getObject(name + "single_best_log")
self.single_alt_log = field.getObject(name + "single_alt_log")
self.multi_best_log = field.getObject(name + "multi_best_log")
self.multi_alt_log = field.getObject(name + "multi_alt_log")
self.field_pos_obj = field.getObject(name + "vision_pose")
self.best_log = field.getObject(name + "_best_log")
self.alt_log = field.getObject(name + "_alt_log")
self.field_pos_obj = field.getObject(name + "_vision_pose")
self.pose_log_entry = wpiutil.log.FloatArrayLogEntry(
data_log, name + "vision_pose"
data_log, name + "_vision_pose"
)

self.chassis = chassis
self.current_reproj = 0.0
self.has_multitag = False

@feedback
def reproj(self) -> float:
return self.current_reproj

def execute(self) -> None:
# stop warnings in simulation
if wpilib.RobotBase.isSimulation():
return
@feedback
def using_multitag(self) -> bool:
return self.has_multitag

def execute(self) -> None:
results = self.camera.getLatestResult()
# if results didn't see any targets
if not results.getTargets():
return

# if we have already processed these results
timestamp = results.getTimestamp()
timestamp = results.getTimestampSeconds()

if timestamp == self.last_timestamp:
return
self.last_recieved_timestep = time.monotonic()
self.last_timestamp = timestamp

if results.multiTagResult.estimatedPose.isPresent:
p = results.multiTagResult.estimatedPose
if results.multitagResult:
self.has_multitag = True
p = results.multitagResult.estimatedPose
pose = (Pose3d() + p.best + self.camera_to_robot).toPose2d()
reprojectionErr = p.bestReprojError
reprojectionErr = p.bestReprojErr
self.current_reproj = reprojectionErr

self.field_pos_obj.setPose(pose)
Expand All @@ -109,13 +108,14 @@ def execute(self) -> None:
)

if self.should_log:
self.multi_best_log.setPose(
self.best_log.setPose(
Pose2d(p.best.x, p.best.y, p.best.rotation().toRotation2d())
)
self.multi_alt_log.setPose(
self.alt_log.setPose(
Pose2d(p.alt.x, p.alt.y, p.alt.rotation().toRotation2d())
)
else:
self.has_multitag = False
for target in results.getTargets():
# filter out likely bad targets
if target.getPoseAmbiguity() > 0.25:
Expand All @@ -134,24 +134,21 @@ def execute(self) -> None:
)

self.field_pos_obj.setPose(pose)
self.chassis.estimator.addVisionMeasurement(pose, timestamp)
self.chassis.estimator.addVisionMeasurement(
pose,
timestamp,
(
self.linear_vision_uncertainty,
self.linear_vision_uncertainty,
self.rotation_vision_uncertainty,
),
)

if self.should_log:
self.single_best_log.setPose(
Pose2d(
target.bestCameraToTarget.x,
target.bestCameraToTarget.y,
target.bestCameraToTarget.rotation().toRotation2d(),
)
)
self.single_alt_log.setPose(
Pose2d(
target.altCameraToTarget.x,
target.altCameraToTarget.y,
target.altCameraToTarget.rotation().toRotation2d(),
)
)
self.best_log.setPose(best)
self.alt_log.setPose(alt)

@feedback
def sees_target(self):
return time.monotonic() - self.last_recieved_timestep < self.TIMEOUT

Expand Down
11 changes: 11 additions & 0 deletions physics.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,10 @@
from wpimath.kinematics import SwerveDrive4Kinematics
from wpimath.system.plant import DCMotor, LinearSystemId
from wpimath.units import kilogram_square_meters
from photonlibpy.simulation import PhotonCameraSim, SimCameraProperties, VisionSystemSim

from components.chassis import SwerveModule
from utilities import game

if typing.TYPE_CHECKING:
from robot import MyRobot
Expand Down Expand Up @@ -97,6 +99,13 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot):
self.imu = SimDeviceSim("navX-Sensor", 4)
self.imu_yaw = self.imu.getDouble("Yaw")

self.vision = VisionSystemSim("main")
self.vision.addAprilTags(game.apriltag_layout)
properties = SimCameraProperties.OV9281_1280_720()
self.camera = PhotonCameraSim(robot.vision.camera, properties)
self.camera.setMaxSightRange(5.0)
self.vision.addCamera(self.camera, robot.vision.robot_to_camera)

def update_sim(self, now: float, tm_diff: float) -> None:
# Enable the Phoenix6 simulated devices
# TODO: delete when phoenix6 integrates with wpilib
Expand All @@ -120,3 +129,5 @@ def update_sim(self, now: float, tm_diff: float) -> None:
self.imu_yaw.set(self.imu_yaw.get() - math.degrees(speeds.omega * tm_diff))

self.physics_controller.drive(speeds, tm_diff)

self.vision.update(self.physics_controller.get_pose())
7 changes: 3 additions & 4 deletions pyproject.toml
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
[tool.coverage.run]
branch = true
omit = ["config.py", "config-3.py"]

[tool.coverage.report]
exclude_lines = [
Expand Down Expand Up @@ -66,11 +67,9 @@ prerelease = "allow"
name = "pyreefscape"
version = "0.0.0"
description = "The Drop Bears' FRC 2025 robot code"
authors = [
{name = "The Drop Bears", email = "[email protected]"},
]
authors = [{ name = "The Drop Bears", email = "[email protected]" }]
readme = "README.md"
license = {text = "MIT"}
license = { text = "MIT" }
requires-python = ">=3.10,<3.13"

dependencies = [
Expand Down
23 changes: 6 additions & 17 deletions robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,7 @@ class MyRobot(magicbot.MagicRobot):
max_spin_rate = magicbot.tunable(4) # m/s
lower_max_spin_rate = magicbot.tunable(2) # m/s
inclination_angle = tunable(0.0)
vision_port: VisualLocalizer
vision_starboard: VisualLocalizer
vision: VisualLocalizer

START_POS_TOLERANCE = 1

Expand All @@ -40,17 +39,9 @@ def createObjects(self) -> None:
# side: (28*3)*2 + front: (30*3) - 2 (R.I.P)
self.status_lights_strip_length = (28 * 3) * 2 + (30 * 3) - 2

self.vision_port_name = "ardu_cam_port"
self.vision_port_pos = Translation3d(0.005, 0.221, 0.503)
self.vision_port_rot = Rotation3d(
0, -math.radians(20), math.radians(180) - math.radians(90 - 71.252763)
)

self.vision_starboard_name = "ardu_cam_starboard"
self.vision_starboard_pos = Translation3d(0.005, 0.161, 0.503)
self.vision_starboard_rot = Rotation3d(
0, -math.radians(20), math.radians(180) + math.radians(90 - 71.252763)
)
self.vision_name = "ardu_cam"
self.vision_pos = Translation3d(0.25, 0.0, 0.20)
self.vision_rot = Rotation3d(0, -math.radians(20), 0)

def teleopInit(self) -> None:
self.field.getObject("Intended start pos").setPoses([])
Expand Down Expand Up @@ -116,12 +107,10 @@ def testPeriodic(self) -> None:

self.chassis.update_odometry()

self.vision_port.execute()
self.vision_starboard.execute()
self.vision.execute()

def disabledPeriodic(self) -> None:
self.chassis.update_alliance()
self.chassis.update_odometry()

self.vision_port.execute()
self.vision_starboard.execute()
self.vision.execute()

0 comments on commit 4b55927

Please sign in to comment.