From f1cc8feae6d37e2e354d23b59c9057b407096105 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Fri, 15 Nov 2024 10:44:24 -0600 Subject: [PATCH] Lookahead runs in simulation, but not on real robot --- drivetrain/poseEstimation/drivetrainPoseTelemetry.py | 4 ---- navigation/repulsorFieldPlanner.py | 9 ++++++++- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py index f23b8a2..79aa0da 100644 --- a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py +++ b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py @@ -82,10 +82,6 @@ def update(self, estPose:Pose2d, moduleAngles): 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 9eeffd6..707b8ab 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -1,3 +1,4 @@ +from wpilib import TimedRobot from wpimath.geometry import Pose2d, Translation2d, Rotation2d from utils.mapLookup2d import MapLookup2D @@ -261,7 +262,13 @@ def update(self, curCmd:DrivetrainCommand, stepSize_m:float) -> DrivetrainComman self.startSlowFactor = min(self.startSlowFactor, 1.0) nextCmd = self._getCmd(curCmd, stepSize_m) - # self._doLookahead(curCmd) + + if(TimedRobot.isSimulation()): + # Lookahead is for telemetry and debug only, and is very + # time expensive. We'll do it in simulation, but not on the + # real robot. + self._doLookahead(curCmd) + return nextCmd def _getCmd(self, curCmd:DrivetrainCommand, stepSize_m:float) -> DrivetrainCommand: