adding obstacle telemetry

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:
 

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,14 +44,17 @@ 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]):
         if(len(observations) > 0):
             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 = []

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())

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)

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,