From c5d2ce36690fc12b71fe9405a90d0b1467b783c0 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Thu, 7 Nov 2024 22:51:01 -0600 Subject: [PATCH] extra logic to prevent nonsensical trig answers for object detection --- navigation/obstacleDetector.py | 7 ++-- wrappers/wrapperedObstaclePhotonCamera.py | 45 +++++++++++++---------- 2 files changed, 29 insertions(+), 23 deletions(-) diff --git a/navigation/obstacleDetector.py b/navigation/obstacleDetector.py index ea69270..ef22dad 100644 --- a/navigation/obstacleDetector.py +++ b/navigation/obstacleDetector.py @@ -1,7 +1,7 @@ from wrappers.wrapperedObstaclePhotonCamera import WrapperedObstaclePhotonCamera from drivetrain.drivetrainPhysical import ROBOT_TO_FRONT_CAM -from wpimath.geometry import Pose2d +from wpimath.geometry import Pose2d, Pose3d from navigation.forceGenerators import ForceGenerator, PointObstacle class ObstacleDetector(): @@ -19,8 +19,9 @@ def getObstacles(self, curPose:Pose2d) -> list['ForceGenerator']: self.frontCam.update() for obs in self.frontCam.getObstacles(): - obsPose = curPose.translation() + obs - retList.append(PointObstacle(location=obsPose, strength=0.7)) + camPose = Pose3d(curPose).transformBy(ROBOT_TO_FRONT_CAM).toPose2d() + obsTrans = camPose.transformBy(obs) + retList.append(PointObstacle(location=obsTrans.translation(), strength=0.7)) # TODO - add other cameras and their observations diff --git a/wrappers/wrapperedObstaclePhotonCamera.py b/wrappers/wrapperedObstaclePhotonCamera.py index fabc80c..2254324 100644 --- a/wrappers/wrapperedObstaclePhotonCamera.py +++ b/wrappers/wrapperedObstaclePhotonCamera.py @@ -1,5 +1,5 @@ import wpilib -from wpimath.geometry import Pose2d, Transform3d, Rotation2d, Translation2d +from wpimath.geometry import Pose2d, Transform3d, Rotation2d, Translation2d, Transform2d from wpimath.units import degreesToRadians from photonlibpy.photonCamera import PhotonCamera from photonlibpy.photonCamera import setVersionCheckEnabled @@ -13,7 +13,7 @@ def __init__(self, time:float, estFieldPose:Pose2d, trustworthiness=1.0): self.estFieldPose = estFieldPose self.trustworthiness = trustworthiness # TODO - not used yet -MIN_AREA=1.0 #idk tune this if we are reacting to small targets +MIN_AREA= 0.2 #idk tune this if we are reacting to small targets def _calculateDistanceToTargetMeters( cameraHeightMeters:float, @@ -25,15 +25,27 @@ def _calculateDistanceToTargetMeters( using right triangles, the assumption obstacles are on the ground, and the geometry of where the camera is mounted on the robot. """ - return (targetHeightMeters - cameraHeightMeters) / math.tan(cameraPitchRadians + targetPitchRadians) + netAngle = cameraPitchRadians + targetPitchRadians + + # Even if there's some error in measurement, be sure the angle puts the target + # in the right place. + netAngle = min(netAngle, -0.000001) + + return (targetHeightMeters - cameraHeightMeters) / math.tan(netAngle) def _estimateCameraToTargetTranslation(targetDistanceMeters:float, yaw:Rotation2d): """ Utility to generate a Translation2d based on """ - return Translation2d( - yaw.cos() * targetDistanceMeters, yaw.sin() * targetDistanceMeters) + xPos = yaw.cos() * targetDistanceMeters + yPos = -1.0 * yaw.sin() * targetDistanceMeters # Positive vision yaw is to the right + + # Ensure target is in front of the camera + if(xPos < 0.0): + xPos = 0.0 + + return Translation2d(xPos, yPos) class WrapperedObstaclePhotonCamera: """ @@ -49,7 +61,7 @@ def __init__(self, camName, robotToCam:Transform3d): self.disconFault = Fault(f"Camera {camName} not sending data") self.timeoutSec = 1.0 - self.obstacleEstimates:list[Translation2d] = [] + self.obstacleEstimates:list[Transform2d] = [] self.robotToCam = robotToCam def update(self): @@ -65,16 +77,9 @@ def update(self): # Note: Results simply report "I processed a frame". There may be 0 or more targets seen in a frame res = self.cam.getLatestResult() - # MiniHack - results also have a more accurate "getTimestamp()", but this is - # broken in photonvision 2.4.2. Hack with the non-broken latency calcualtion - latency = res.getLatencyMillis() - obsTime = wpilib.Timer.getFPGATimestamp() - # Update our disconnected fault since we have something from the camera self.disconFault.setNoFault() - print("got camera data") - # Process each target. # Each target has multiple solutions for where you could have been at on the field # when you observed it @@ -85,9 +90,8 @@ def update(self): # don't make sense. for target in res.getTargets(): # Transform both poses to on-field poses - print("saw target") if (target.getArea()>MIN_AREA): - print("big target") + print(f"Saw target at pitch {target.getPitch()} deg, yaw {target.getYaw()} deg") # Use algorithm described at # https://docs.limelightvision.io/docs/docs-limelight/tutorials/tutorial-estimating-distance # to estimate distance from the camera to the target. @@ -97,13 +101,14 @@ def update(self): self.robotToCam.rotation().Y(), # Pitch is rotation about the Y axis degreesToRadians(target.getPitch()) ) - camToObstacle = _estimateCameraToTargetTranslation( + camToObstacleTranslation = _estimateCameraToTargetTranslation( dist, Rotation2d.fromDegrees(target.getYaw()) ) - print(f"{camToObstacle}") - - self.obstacleEstimates.append(camToObstacle) + camToObstacleTransform = Transform2d(camToObstacleTranslation, Rotation2d()) + print(f"Saw big target at {camToObstacleTransform}") + + self.obstacleEstimates.append(camToObstacleTransform) - def getObstacles(self) -> list[Translation2d]: + def getObstacles(self) -> list[Transform2d]: return self.obstacleEstimates