WIP telemetry on obstacles

drivetrain/controlStrategies/autoDrive.py

@@ -1,7 +1,8 @@
-from wpimath.geometry import Pose2d, Rotation2d
+from wpimath.geometry import Pose2d, Rotation2d, Translation2d
 from wpimath.trajectory import Trajectory
 from drivetrain.controlStrategies.holonomicDriveController import HolonomicDriveController
 from drivetrain.drivetrainCommand import DrivetrainCommand
+from utils.signalLogging import log
 from utils.singleton import Singleton
 from navigation.repulsorFieldPlanner import RepulsorFieldPlanner
 from drivetrain.drivetrainPhysical import MAX_DT_LINEAR_SPEED
@@ -29,9 +30,7 @@ def setRequest(self, toSpeaker, toPickup) -> None:
     def getTrajectory(self) -> Trajectory|None:
         return None # TODO
 
-    def updateTelemetry(self) -> None:
-        self._rfp.updateTelemetry()
-
+    def updateTelemetry(self) -> None:        
         self._telemTraj = []
         if(self._rfp.goal is not None):
             cp = self._curPose
@@ -48,6 +47,9 @@ def updateTelemetry(self) -> None:
 
     def getWaypoints(self) -> list[Pose2d]:
         return self._telemTraj
+
+    def getObstacles(self) -> list[Translation2d]:
+        return [x.translation() for x in self._rfp.getObstaclePoseList()]
 
     def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand:
 
@@ -70,4 +72,6 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand
             if( olCmd.desPose is not None):
                 retCmd = self._trajCtrl.update2(olCmd.velX, olCmd.velY, olCmd.velT, olCmd.desPose, curPose)
 
+        log("AutoDrive_active", self._rfp.goal is not None)
+
         return retCmd

navigation/repulsorFieldPlanner.py

@@ -4,7 +4,6 @@
 from navigation.navConstants import FIELD_X_M, FIELD_Y_M
 
 from drivetrain.drivetrainCommand import DrivetrainCommand
-from navigation.navMapTelemetry import CostMapTelemetry
 
 @dataclass
 class Force:
@@ -32,6 +31,8 @@ def _distToForceMag(self, dist:float)->float:
         return forceMag
     def getDist(self, position:Translation2d)->float:
         return float('inf')
+    def getTelemTrans(self)->list[Translation2d]:
+        return []
 
 class PointObstacle(Obstacle):
     def __init__(self, location:Translation2d, strength:float=1.0, forceIsPositive:bool=True):
@@ -49,6 +50,8 @@ def getForceAtPosition(self, position: Translation2d) -> Force:
         return Force(-1.0*unitX*forceMag, -1.0*unitY*forceMag)
     def getDist(self, position:Translation2d)->float:
         return (position - self.location).norm()
+    def getTelemTrans(self) -> list[Translation2d]:
+        return [self.location]
 
 class HorizontalObstacle(Obstacle):
     def __init__(self, y:float, strength:float=1.0, forceIsPositive:bool=True):
@@ -61,6 +64,9 @@ def getForceAtPosition(self, position: Translation2d) -> Force:
 
     def getDist(self, position: Translation2d) -> float:
         return abs(position.y - self.y)
+
+    def getTelemTrans(self) -> list[Translation2d]:
+        return []
 
 class VerticalObstacle(Obstacle):
     def __init__(self, x:float, strength:float=1.0, forceIsPositive:bool=True):
@@ -98,7 +104,6 @@ def __init__(self):
        self.fixedObstacles.extend(FIELD_OBSTACLES)
        self.fixedObstacles.extend(WALLS)
        self.transientObstcales:list[Obstacle] = []
-       self.telem = CostMapTelemetry(name="PotentialField")
 
     def setGoal(self, goal:Pose2d|None):
         self.goal = goal
@@ -126,8 +131,12 @@ def decay_observations(self):
         # Only keep obstacles with positive strength
         self.transientObstcales = [x for x in self.transientObstcales if x.strength > 0.0]
 
-    def updateTelemetry(self):
-        self.telem.update(self)
+    def getObstaclePoseList(self) -> list[Pose2d]:
+        retArr = []
+        for obstacle in self.fixedObstacles:
+            retArr.extend(obstacle.getTelemTrans())
+
+        return retArr
 
     def getForceAtTrans(self, trans:Translation2d)->Force:
         goalForce = self.getGoalForce(trans)

robot.py

@@ -104,6 +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())
 
         # No trajectory in Teleop
         Trajectory().setCmd(None)