From dbe008fd39878a05b3e2ad7cd0bfde7ee080e0a4 Mon Sep 17 00:00:00 2001 From: Justin Buist Date: Fri, 15 Mar 2024 17:10:53 -0400 Subject: [PATCH] Add robot pose visual to system --- robot.py | 62 +++++++++++++++++++++++++++++++++------- simgui-window.json | 43 +++++++++++++++++++--------- simgui.json | 47 +++++++++++++++++++++++++++++- subsystems/drivetrain.py | 2 +- 4 files changed, 129 insertions(+), 25 deletions(-) diff --git a/robot.py b/robot.py index 1b642ab..d698dd0 100644 --- a/robot.py +++ b/robot.py @@ -1,10 +1,11 @@ # All units of length will be in meters unless otherwise noted +from dataclasses import Field import json from time import time from math import radians -from wpilib import SmartDashboard, Joystick, DriverStation, Timer +from wpilib import SmartDashboard, Joystick, DriverStation, Timer, Field2d from commands2 import TimedCommandRobot, SequentialCommandGroup, InstantCommand from commands2.button import JoystickButton from wpimath.geometry import Rotation2d, Pose2d @@ -53,6 +54,12 @@ class MyRobot(TimedCommandRobot): def robotInit(self) -> None: self.vision_timer = Timer() + self.field = Field2d() + SmartDashboard.putData(self.field) + pn = SmartDashboard.putNumber + pn('visiontest/fakeX', 3) + pn('visiontest/fakeY', 4) + pn('visiontest/fakeRot', 0) """Robot initialization function""" if True: # Disable the joystick warnings in simulator mode; they're annoying @@ -187,8 +194,21 @@ def robotPeriodic(self) -> None: # Rough idea of how to incorporate vision into odometry if self.swerve.vision_stable is True: + from math import pi # Here's our method to pull data from LimeLight's network table - ll_poses, cameralag, computelag = self.get_poses_from_limelight() + if is_sim(): + gn = SmartDashboard.getNumber + fakex = gn('visiontest/fakeX', 0) + fakey = gn('visiontest/fakeY', 0) + fakerot = gn('visiontest/fakeRot', 0) + ll_poses = [Pose2d(fakex, fakey, Rotation2d(radians(fakerot)))] + certain_within = [(10, 10, 1/pi)] + cameralag = 0 + computelag = 0 + else: + ll_poses, certain_within, cameralag, computelag = ( + self.get_poses_from_limelight() + ) # Here we can make a determination to use the vision data or not # For example we might only want to accept posees that are already # within 1 meter of where we think we are. @@ -198,16 +218,21 @@ def robotPeriodic(self) -> None: # to get things performing right. Nothing is set in stone. if len(ll_poses) > 0: timelag = cameralag + computelag - for p in ll_poses: + for p, cw in zip(ll_poses, certain_within): x = p.X() y = p.Y() rot = p.rotation().degrees() - # print(f'vision heading: {x}, {y}, {rot}') # self.swerve.odometry.addVisionMeasurement(p, 0, (0.1, 0.1, 0.1)) - self.swerve.odometry.addVisionMeasurement(p, self.vision_timer.getFPGATimestamp(), (100, 100, 100)) + ts = self.vision_timer.getFPGATimestamp() - timelag + print(f'vision heading: {x}, {y}, {rot}, {ts}') + xdev, ydev, rotdev = cw + self.swerve.odometry.addVisionMeasurement( + p, ts, (xdev, ydev, rotdev) + ) else: # print('no vision data') pass + self.field.setRobotPose(self.swerve.getPose()) pass def auto_station_1(self): @@ -285,30 +310,47 @@ def teleopPeriodic(self) -> None: # https://docs.limelightvision.io/docs/docs-limelight/apis/json-dump-specification # We are going to be using 't6r_fs' aka "Robot Pose in field space as # computed by this fiducial (x,y,z,rx,ry,rz)" - def get_poses_from_limelight(self) -> tuple[list[Pose2d], float]: + def get_poses_from_limelight(self): t1 = time() data = self.swerve.ll_json_entry.get() obj = json.loads(data) poses = [] - camera_lag = 0 + certain_within = [] + camera_lag = 0 # Short circuit any JSON processing if we got back an empty list, which # is the default value for the limelight network table entry if len(obj) == 0: t2 = time() diff = t2 - t1 - return poses, camera_lag, diff + return poses, certain_within, camera_lag, diff result = obj['Results'] # camera_lag = result['tl'] for fid in result['Fiducial']: + target_area = fid['ta'] + # Use this, the total area the target is in pixels on the camera + # picture, to determine how certain we are of the robot's position. + + # start with a very uncertain value unless a condition is met to + # say otherwise. The format is undertainty in the x in meters, + # y in meters, and rotation in degrees + certainty = 10 + if target_area > 200: + certainty = (3, 3, 10) + if target_area > 300: + certainty = (2, 2, 5) + if target_area > 400: + certainty = (1, 1, 3) + robot_pose_raw = fid['t6r_fs'] # TODO: Verify that the rotation is the right value pose = Pose2d(robot_pose_raw[0], robot_pose_raw[1], - Rotation2d(radians(robot_pose_raw[3]))) + Rotation2d(radians(robot_pose_raw[3]))) poses.append(pose) + certain_within.append(certainty) t2 = time() diff = t2 - t1 - return poses, camera_lag, diff + return poses, certain_within, camera_lag, diff # TODO: Heading needs to be added to the return on this # and the overal processing could be a lot cleaner. diff --git a/simgui-window.json b/simgui-window.json index e63c5a5..efe34f0 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -8,31 +8,48 @@ "height": "900", "maximized": "0", "style": "0", - "userScale": "2", + "userScale": "4", "width": "1600", - "xpos": "-1", - "ypos": "-1" + "xpos": "160", + "ypos": "106" + } + }, + "Table": { + "0x542B5671,2": { + "Column 0 Width": "260", + "Column 1 Width": "228", + "RefScale": "19" } }, "Window": { + "###/SmartDashboard/Field": { + "Collapsed": "0", + "Pos": "729,-22", + "Size": "937,664" + }, + "###Addressable LEDs": { + "Collapsed": "0", + "Pos": "13,432", + "Size": "199,436" + }, "###FMS": { "Collapsed": "0", - "Pos": "6,675", - "Size": "336,158" + "Pos": "240,675", + "Size": "388,176" }, "###Joysticks": { "Collapsed": "0", "Pos": "312,581", - "Size": "976,82" + "Size": "1156,91" }, "###NetworkTables": { "Collapsed": "0", - "Pos": "312,346", - "Size": "937,231" + "Pos": "99,40", + "Size": "635,613" }, "###NetworkTables Info": { "Collapsed": "0", - "Pos": "312,162", + "Pos": "326,714", "Size": "937,181" }, "###Other Devices": { @@ -42,13 +59,13 @@ }, "###System Joysticks": { "Collapsed": "0", - "Pos": "6,437", - "Size": "232,254" + "Pos": "15,185", + "Size": "273,290" }, "###Timing": { "Collapsed": "0", "Pos": "6,187", - "Size": "162,168" + "Size": "188,186" }, "Debug##Default": { "Collapsed": "0", @@ -58,7 +75,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "109,134" + "Size": "126,152" } } } diff --git a/simgui.json b/simgui.json index 5f9d275..9e59ed0 100644 --- a/simgui.json +++ b/simgui.json @@ -1,7 +1,52 @@ { + "HALProvider": { + "Addressable LEDs": { + "window": { + "visible": true + } + }, + "Timing": { + "window": { + "visible": false + } + } + }, "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Field": "Field2d" + }, + "windows": { + "/SmartDashboard/Field": { + "bottom": 1476, + "height": 8.210550308227539, + "left": 150, + "right": 2961, + "top": 79, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "Shuffleboard": { + "open": true + }, + "SmartDashboard": { + "drivetrain": { + "odometry": { + "open": true + }, + "open": true + }, + "open": true, + "visiontest": { + "open": true + } + } } }, "NetworkTables Info": { diff --git a/subsystems/drivetrain.py b/subsystems/drivetrain.py index 1de6f19..37eacf5 100644 --- a/subsystems/drivetrain.py +++ b/subsystems/drivetrain.py @@ -123,7 +123,7 @@ def __init__(self, gyro: gyro.Gyro, driver_controller, photon: NoteTracker, self.backRight.getPosition(), ), Pose2d(), - (0, 0, 0), # wheel std devs for Kalman filters + (1, 1, 1), # wheel std devs for Kalman filters (0, 0, 0), # vision std devs for Kalman filters )