diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index e1e1d25..1614c51 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/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 \ No newline at end of file diff --git a/navigation/navMapTelemetry.py b/navigation/navMapTelemetry.py deleted file mode 100644 index 1598253..0000000 --- a/navigation/navMapTelemetry.py +++ /dev/null @@ -1,90 +0,0 @@ -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) - diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index 1728041..bf4dd1f 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/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) diff --git a/robot.py b/robot.py index b4e4842..bd92de9 100644 --- a/robot.py +++ b/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)