From e632fbcbd20661d55de920aa4774f5882951d26f Mon Sep 17 00:00:00 2001
From: Chris Gerth <chrisgerth010592@gmail.com>
Date: Fri, 15 Nov 2024 08:50:42 -0600
Subject: [PATCH] Adding fixed obstacles back to telemetry

---
 .../poseEstimation/drivetrainPoseTelemetry.py |  4 +++-
 navigation/repulsorFieldPlanner.py            | 24 ++++++++++---------
 2 files changed, 16 insertions(+), 12 deletions(-)

diff --git a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py
index 7285f6e..f23b8a2 100644
--- a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py
+++ b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py
@@ -21,6 +21,7 @@ def __init__(self):
         wpilib.SmartDashboard.putData("DT Pose 2D", self.field)
         self.curTraj = Trajectory()
         self.curTrajWaypoints = []
+        self.fixedObstacles = []
         self.fullObstacles = []
         self.thirdObstacles = []
         self.almostGoneObstacles = []
@@ -58,7 +59,7 @@ def addVisionObservations(self, observations:list[CameraPoseObservation]):
                 self.visionPoses.append(obs.estFieldPose)
 
     def setCurObstacles(self, obstacles):
-        self.fullObstacles, self.thirdObstacles, self.almostGoneObstacles = obstacles
+        self.fixedObstacles, self.fullObstacles, self.thirdObstacles, self.almostGoneObstacles = obstacles
 
     def clearVisionObservations(self):
         self.visionPoses = []
@@ -77,6 +78,7 @@ def update(self, estPose:Pose2d, moduleAngles):
         self.field.getObject("desPose").setPose(self.desPose)
         self.field.getObject("desTraj").setTrajectory(self.curTraj)
         self.field.getObject("desTrajWaypoints").setPoses(self.curTrajWaypoints)
+        self.field.getObject("curObstaclesFixed").setPoses([Pose2d(x, Rotation2d()) for x in self.fixedObstacles])
         self.field.getObject("curObstaclesFull").setPoses([Pose2d(x, Rotation2d()) for x in self.fullObstacles])
         self.field.getObject("curObstaclesThird").setPoses([Pose2d(x, Rotation2d()) for x in self.thirdObstacles])
         self.field.getObject("curObstaclesAlmostGone").setPoses([Pose2d(x, Rotation2d()) for x in self.almostGoneObstacles])
diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py
index 320c9b2..9c9be08 100644
--- a/navigation/repulsorFieldPlanner.py
+++ b/navigation/repulsorFieldPlanner.py
@@ -166,25 +166,27 @@ def _decayObservations(self):
         # Fully decayed obstacles have zero or negative strength.
         self.transientObstcales = [x for x in self.transientObstcales if x.strength > 0.0]
 
-    def getObstacleStrengths(self):
-        #these are all Translation 2ds, or should be, but we can't say that if we want to return all 3. 
-        fullTransientObstacles =[]
-        thirdTransientObstacles = []
-        almostGoneTransientObstacles = []
+    def getObstacleStrengths(self) -> tuple[list[Translation2d], list[Translation2d], list[Translation2d], list[Translation2d]]:
+        #these are all Translation 2ds, or should be, but we can't say that if we want to return all 4. 
+        fixedObstacleTranslations = []
+        fullTransientObstacleTranslations =[]
+        thirdTransientObstacleTranslations = []
+        almostGoneTransientObstacleTranslations = []
         
-        # Fixed obstacles are full strength
+        # Fixed obstacles
         for x in self.fixedObstacles:
-            fullTransientObstacles.extend(x.getTelemTrans())
+            fixedObstacleTranslations.extend(x.getTelemTrans())
 
+        # Transient Obstacles
         for x in self.transientObstcales:
             if x.strength > .5:
-                fullTransientObstacles.extend(x.getTelemTrans())
+                fullTransientObstacleTranslations.extend(x.getTelemTrans())
             elif x.strength > .33:
-                thirdTransientObstacles.extend(x.getTelemTrans())
+                thirdTransientObstacleTranslations.extend(x.getTelemTrans())
             else:
-                almostGoneTransientObstacles.extend(x.getTelemTrans())
+                almostGoneTransientObstacleTranslations.extend(x.getTelemTrans())
 
-        return fullTransientObstacles, thirdTransientObstacles, almostGoneTransientObstacles
+        return fixedObstacleTranslations, fullTransientObstacleTranslations, thirdTransientObstacleTranslations, almostGoneTransientObstacleTranslations
 
     def getObstacleTransList(self) -> list[Translation2d]:
         """