diff --git a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py index 305f20e..7285f6e 100644 --- a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py +++ b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py @@ -21,7 +21,9 @@ def __init__(self): wpilib.SmartDashboard.putData("DT Pose 2D", self.field) self.curTraj = Trajectory() self.curTrajWaypoints = [] - self.obstacles = [] + self.fullObstacles = [] + self.thirdObstacles = [] + self.almostGoneObstacles = [] self.desPose = Pose2d() @@ -55,8 +57,8 @@ 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 setCurObstacles(self, obstacles): + self.fullObstacles, self.thirdObstacles, self.almostGoneObstacles = obstacles def clearVisionObservations(self): self.visionPoses = [] @@ -75,7 +77,13 @@ 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("curObstaclesFull").setPoses([Pose2d(x, Rotation2d()) for x in self.fullObstacles]) + self.field.getObject("curObstaclesThird").setPoses([Pose2d(x, Rotation2d()) for x in self.thirdObstacles]) + self.field.getObject("curObstaclesAlmostGone").setPoses([Pose2d(x, Rotation2d()) for x in self.almostGoneObstacles]) + #print("Full: " + str(len(self.fullObstacles))) + #print("Third: " + str(len(self.thirdObstacles))) + #print("Almost gone: " + str(len(self.almostGoneObstacles))) + self.field.getObject("visionObservations").setPoses(self.visionPoses) self.visionPoses = [] diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index 49d18c8..d03da71 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -108,6 +108,11 @@ def __init__(self): # Track the "lookahead" - a series of steps predicting where we will go next self.lookaheadTraj:list[Pose2d] = [] + #these actually aren't going to have any forces attached to them, it's just going to be for the graphics + self.fullTransientObstacles = [] + self.thirdTransientObstacles = [] + self.almostGoneTransientObstacles = [] + # Keep things slow right when the goal changes self.startSlowFactor = 0.0 @@ -161,6 +166,22 @@ def _decayObservations(self): # Fully decayed obstacles have zero or negative strength. self.transientObstcales = [x for x in self.transientObstcales if x.strength > 0.0] + def getObstacleStrengths(self): + #these are all Translation 2ds, or should be, but we can't say that if we want to return all 3. + fullTransientObstacles = [] + thirdTransientObstacles = [] + almostGoneTransientObstacles = [] + + for x in self.transientObstcales: + if x.strength > .5: + fullTransientObstacles.extend(x.getTelemTrans()) + elif x.strength > .33: + thirdTransientObstacles.extend(x.getTelemTrans()) + else: + almostGoneTransientObstacles.extend(x.getTelemTrans()) + + return fullTransientObstacles, thirdTransientObstacles, almostGoneTransientObstacles + def getObstacleTransList(self) -> list[Translation2d]: """ Return a list of translations (x/y points), representing the current obstacles diff --git a/robot.py b/robot.py index 80a7d19..1b23af6 100644 --- a/robot.py +++ b/robot.py @@ -71,7 +71,8 @@ def robotPeriodic(self): self.autodrive.updateTelemetry() self.driveTrain.poseEst._telemetry.setCurAutoDriveWaypoints(self.autodrive.getWaypoints()) - self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.getObstacles()) + #self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.getObstacles()) + self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.rfp.getObstacleStrengths()) self.stt.mark("Telemetry") self.ledCtrl.setAutoDrive(self.autodrive.isRunning()) diff --git a/simgui-ds.json b/simgui-ds.json index cb1aa21..d631100 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -96,6 +96,10 @@ } ], "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + }, { "guid": "Keyboard0" } diff --git a/simgui.json b/simgui.json index a70647d..6a46bcf 100644 --- a/simgui.json +++ b/simgui.json @@ -67,7 +67,33 @@ "weight": 2.0 }, "bottom": 1476, - "curObstacles": { + "curObstaclesAlmostGone": { + "arrows": false, + "color": [ + 255.0, + 0.0, + 232.50010681152344, + 255.0 + ], + "length": 0.6859999895095825, + "selectable": false, + "style": "Box/Image", + "weight": 5.0 + }, + "curObstaclesFull": { + "arrows": false, + "color": [ + 0.18339098989963531, + 0.19835130870342255, + 0.3529411554336548, + 255.0 + ], + "length": 0.6859999895095825, + "selectable": false, + "style": "Box/Image", + "weight": 5.0 + }, + "curObstaclesThird": { "arrows": false, "color": [ 0.5098038911819458, @@ -75,11 +101,10 @@ 0.02998846210539341, 255.0 ], - "image": "obstacle.png", "length": 0.6859999895095825, "selectable": false, "style": "Box/Image", - "weight": 16.0 + "weight": 5.0 }, "desTraj": { "arrowColor": [ @@ -148,6 +173,9 @@ "open": true }, "SmartDashboard": { + "DT Pose 2D": { + "open": true + }, "open": true } }