From 5346cf21993477677c52459d14c270fdf7c7a9a8 Mon Sep 17 00:00:00 2001
From: Chris Gerth <chrisgerth010592@gmail.com>
Date: Thu, 26 Sep 2024 22:26:17 -0500
Subject: [PATCH] adding obstacle telemetry

---
 drivetrain/controlStrategies/autoDrive.py            |  2 +-
 drivetrain/poseEstimation/drivetrainPoseTelemetry.py | 10 ++++++++--
 navigation/repulsorFieldPlanner.py                   |  2 +-
 robot.py                                             |  2 +-
 simgui.json                                          | 12 ++++++++++++
 5 files changed, 23 insertions(+), 5 deletions(-)

diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py
index 1614c51..74378b6 100644
--- a/drivetrain/controlStrategies/autoDrive.py
+++ b/drivetrain/controlStrategies/autoDrive.py
@@ -49,7 +49,7 @@ def getWaypoints(self) -> list[Pose2d]:
         return self._telemTraj
     
     def getObstacles(self) -> list[Translation2d]:
-        return [x.translation() for x in self._rfp.getObstaclePoseList()]
+        return self._rfp.getObstacleTransList()
 
     def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand:
 
diff --git a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py
index 7851ae1..e13a7fd 100644
--- a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py
+++ b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py
@@ -3,7 +3,7 @@
 import wpilib
 from wpimath.units import metersToFeet
 from wpimath.trajectory import Trajectory
-from wpimath.geometry import Pose2d, Pose3d, Transform2d
+from wpimath.geometry import Pose2d, Pose3d, Transform2d, Rotation2d, Translation2d
 from ntcore import NetworkTableInstance
 
 from utils.signalLogging import log
@@ -23,6 +23,8 @@ def __init__(self):
         wpilib.SmartDashboard.putData("DT Pose 2D", self.field)
         self.curTraj = Trajectory()
         self.curTrajWaypoints = []
+        self.obstacles = []
+
         self.desPose = Pose2d()
 
         self.leftCamPosePublisher = (
@@ -42,7 +44,7 @@ def __init__(self):
     def setDesiredPose(self, desPose):
         self.desPose = desPose
 
-    def setCurTrajWaypoints(self, waypoints):
+    def setCurTrajWaypoints(self, waypoints:list[Pose2d]):
         self.curTrajWaypoints = waypoints
 
     def addVisionObservations(self, observations:list[CameraPoseObservation]):
@@ -50,6 +52,9 @@ def addVisionObservations(self, observations:list[CameraPoseObservation]):
             for obs in observations:
                 self.visionPoses.append(obs.estFieldPose)
 
+    def setCurObstacles(self, obstacles:list[Translation2d]):
+        self.obstacles = obstacles
+
     def clearVisionObservations(self):
         self.visionPoses = []
 
@@ -67,6 +72,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("curObstacles").setPoses([Pose2d(x, Rotation2d()) for x in self.obstacles])
 
         self.field.getObject("visionObservations").setPoses(self.visionPoses)
         self.visionPoses = []
diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py
index bf4dd1f..ad6b0e8 100644
--- a/navigation/repulsorFieldPlanner.py
+++ b/navigation/repulsorFieldPlanner.py
@@ -131,7 +131,7 @@ def decay_observations(self):
         # Only keep obstacles with positive strength
         self.transientObstcales = [x for x in self.transientObstcales if x.strength > 0.0]
 
-    def getObstaclePoseList(self) -> list[Pose2d]:
+    def getObstacleTransList(self) -> list[Translation2d]:
         retArr = []
         for obstacle in self.fixedObstacles:
             retArr.extend(obstacle.getTelemTrans())
diff --git a/robot.py b/robot.py
index bd92de9..4133a63 100644
--- a/robot.py
+++ b/robot.py
@@ -104,7 +104,7 @@ def teleopPeriodic(self):
         self.autodrive.updateTelemetry()
         self.driveTrain.poseEst.telemetry.setWPITrajectory(self.autodrive.getTrajectory())
         self.driveTrain.poseEst.telemetry.setCurTrajWaypoints(self.autodrive.getWaypoints())
-        self.driveTrain.poseEst.telemetry.setCurTrajWaypoints(self.autodrive.getWaypoints())
+        self.driveTrain.poseEst.telemetry.setCurObstacles(self.autodrive.getObstacles())
 
         # No trajectory in Teleop
         Trajectory().setCmd(None)
diff --git a/simgui.json b/simgui.json
index 73d9400..815693f 100644
--- a/simgui.json
+++ b/simgui.json
@@ -55,6 +55,18 @@
           "style": "Line (Closed)"
         },
         "bottom": 1476,
+        "curObstacles": {
+          "arrows": false,
+          "color": [
+            0.5098038911819458,
+            0.26989617943763733,
+            0.02998846210539341,
+            255.0
+          ],
+          "selectable": false,
+          "style": "Box/Image",
+          "weight": 16.0
+        },
         "desTraj": {
           "arrowColor": [
             187.2151641845703,