fixups

drivetrain/controlStrategies/autoDrive.py

@@ -20,6 +20,7 @@ def __init__(self):
         self._rfp = RepulsorFieldPlanner()
         self._trajCtrl = HolonomicDriveController()
         self._curPose = Pose2d()
+        self._telemTraj = []
 
     def setRequest(self, toSpeaker, toPickup) -> None:
         self._toSpeaker = toSpeaker
@@ -28,22 +29,25 @@ def setRequest(self, toSpeaker, toPickup) -> None:
     def getTrajectory(self) -> Trajectory|None:
         return None # TODO
 
-    def getWaypoints(self) -> list[Pose2d]:
-        retArr = []
-
+    def updateTelemetry(self) -> None:
+        self._rfp.updateTelemetry()
+
+        self._telemTraj = []
         if(self._rfp.goal is not None):
             cp = self._curPose
             for _ in range(0,15):
-                retArr.append(cp)
+                self._telemTraj.append(cp)
                 tmp = self._rfp.getCmd(cp, TELEM_STEP_M)
                 if(tmp.desPose is not None):
                     cp = tmp.desPose
                 else:
                     break
 
-            retArr.append(self._rfp.goal)
+            self._telemTraj.append(self._rfp.goal)
 
-        return retArr
+
+    def getWaypoints(self) -> list[Pose2d]:
+        return self._telemTraj
 
     def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand:
 

navigation/navMapTelemetry.py

@@ -0,0 +1,90 @@
+from cscore import CameraServer, VideoMode
+from typing import TYPE_CHECKING
+import numpy as np
+from navigation.navConstants import FIELD_X_M, FIELD_Y_M
+from wpimath.geometry import Translation2d
+
+if TYPE_CHECKING:
+    from navigation.repulsorFieldPlanner import RepulsorFieldPlanner 
+
+
+_M_PER_GRID = 1
+_PX_PER_GRID = 20
+GRID_SIZE_X = int(FIELD_X_M / _M_PER_GRID)
+GRID_SIZE_Y = int(FIELD_Y_M / _M_PER_GRID)
+
+class CostMapTelemetry:
+    def __init__(self, name):
+        self.imgWidth = GRID_SIZE_X*_PX_PER_GRID
+        self.imgHeight = GRID_SIZE_Y*_PX_PER_GRID
+        self.outputStream = CameraServer.putVideo(f"CostMap_{name}", self.imgWidth, self.imgHeight)
+        self.outputStream.setPixelFormat(VideoMode.PixelFormat.kBGR)
+        self.outImgMat =  np.zeros((self.imgHeight, self.imgWidth, 3), dtype=np.uint8)
+
+    @staticmethod
+    def viridis_color(value: float, maxVal:float = 100) -> tuple[int, int, int]:
+        """
+        Convert a value in the range 0-100 to an RGB color on the Viridis colormap.
+        """
+        # Ensure the value is within the range [0, 50]
+        value = max(0, min(maxVal, value))
+
+        # Viridis colormap data: [r, g, b] values scaled to [0, 1] (excerpt of 6 points)
+        # Full colormap is typically more detailed, here is a scaled-down approximation
+        viridis_data = [
+            [0.267004, 0.004874, 0.329415],
+            [0.229739, 0.322361, 0.545706],
+            [0.127568, 0.566949, 0.550556],
+            [0.369214, 0.788888, 0.382914],
+            [0.678489, 0.863742, 0.189503],
+            [0.993248, 0.906157, 0.143936]
+        ]
+
+        # Normalize value to select within the viridis data
+        # Since we have 6 points, we divide by 20 to get an index range [0-5]
+        num_colors = len(viridis_data) - 1
+        index = value * num_colors / maxVal
+
+        # Find the indices for interpolation
+        lower_index = int(np.floor(index))
+        upper_index = min(lower_index + 1, num_colors)
+
+        # Interpolation factor
+        factor = index - lower_index
+
+        # Linearly interpolate between the two colors
+        color = [
+            viridis_data[lower_index][i] * (1 - factor) + viridis_data[upper_index][i] * factor
+            for i in range(3)
+        ]
+
+        # Convert color to 8-bit RGB values
+        r, g, b = [int(c * 255) for c in color]
+
+        return (b, g, r)  # Return as (Blue, Green, Red) since BGR is needed
+
+
+    def update(self, rpp:'RepulsorFieldPlanner'):
+        return
+        # This is way too slow need to get good
+        if(rpp.goal is not None):
+            for x_grid in range(GRID_SIZE_X):
+                for y_grid in range(GRID_SIZE_Y):
+
+                    x_center = (x_grid+0.5) * _M_PER_GRID
+                    y_center = (y_grid+0.5) * _M_PER_GRID
+
+                    val = rpp.getForceAtTrans(Translation2d(x_center, y_center)).mag()
+
+                    # Correct for graphics conventions
+                    x = x_grid
+                    y = GRID_SIZE_Y - 1 - y_grid
+
+                    x0 = x*_PX_PER_GRID
+                    x1 = (x+1)*_PX_PER_GRID
+                    y0 = y*_PX_PER_GRID
+                    y1 = (y+1)*_PX_PER_GRID
+                    self.outImgMat[y0:y1, x0:x1] = self.viridis_color(val)
+
+        self.outputStream.putFrame(self.outImgMat)
+

navigation/repulsorFieldPlanner.py

@@ -4,6 +4,7 @@
 from navigation.navConstants import FIELD_X_M, FIELD_Y_M
 
 from drivetrain.drivetrainCommand import DrivetrainCommand
+from navigation.navMapTelemetry import CostMapTelemetry
 
 @dataclass
 class Force:
@@ -97,6 +98,7 @@ 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
@@ -107,6 +109,8 @@ def add_obstcale_observaton(self, obstacle: Obstacle):
     def getGoalForce(self, curLocation:Translation2d) -> Force:
         if(self.goal is not None):
             displacement = self.goal.translation() - curLocation
+            if(displacement == 0.0):
+                return Force() # literally at goal, no force
             direction = displacement/displacement.norm()
             mag = GOAL_STRENGTH * (1 + 1.0/(0.0001 + displacement.norm()))
             return Force(direction.x*mag, direction.y*mag)
@@ -122,6 +126,24 @@ 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 getForceAtTrans(self, trans:Translation2d)->Force:
+        goalForce = self.getGoalForce(trans)
+
+        repusliveForces = []
+        for obstacle in self.fixedObstacles:
+            repusliveForces.append(obstacle.getForceAtPosition(trans))
+        for obstacle in self.transientObstcales:
+            repusliveForces.append(obstacle.getForceAtPosition(trans))
+
+        # Calcualte sum of forces
+        netForce = goalForce
+        for force in repusliveForces:
+            netForce += force
+
+        return netForce
 
     def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand:
         retVal = DrivetrainCommand() # Default, no command
@@ -136,18 +158,7 @@ def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand:
                 retVal.velY = 0.0
                 retVal.desPose = self.goal
             else:
-                goalForce = self.getGoalForce(curTrans)
-
-                repusliveForces = []
-                for obstacle in self.fixedObstacles:
-                    repusliveForces.append(obstacle.getForceAtPosition(curTrans))
-                for obstacle in self.transientObstcales:
-                    repusliveForces.append(obstacle.getForceAtPosition(curTrans))
-
-                # Calcualte sum of forces
-                netForce = goalForce
-                for force in repusliveForces:
-                    netForce += force
+                netForce = self.getForceAtTrans(curPose.translation())
 
                 # Take a step in the direction of the force
                 step = Translation2d(stepSize_m*netForce.unitX(), stepSize_m*netForce.unitY()) 

robot.py

@@ -101,6 +101,7 @@ def teleopPeriodic(self):
             self.driveTrain.resetGyro()
 
         self.autodrive.setRequest(self.dInt.getNavToSpeaker(), self.dInt.getNavToPickup())
+        self.autodrive.updateTelemetry()
         self.driveTrain.poseEst.telemetry.setWPITrajectory(self.autodrive.getTrajectory())
         self.driveTrain.poseEst.telemetry.setCurTrajWaypoints(self.autodrive.getWaypoints())