Enable vision sim

components/vision.py

@@ -5,8 +5,9 @@
 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
 
@@ -32,7 +33,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,
@@ -51,45 +52,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 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)
@@ -109,13 +109,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:
@@ -134,24 +135,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
 

physics.py

@@ -12,7 +12,10 @@
 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
@@ -97,6 +100,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
@@ -120,3 +130,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())

robot.py

@@ -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
 
@@ -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([])
@@ -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()