From 1e66d90e986decad6ba35184dc0010cf17e3800a Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Thu, 14 Nov 2024 20:01:09 -0600 Subject: [PATCH] loop time improved, added obstacle telemetry back --- navigation/repulsorFieldPlanner.py | 8 ++++++-- robot.py | 2 +- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index d03da71..320c9b2 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -168,10 +168,14 @@ def _decayObservations(self): 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 = [] + fullTransientObstacles =[] thirdTransientObstacles = [] almostGoneTransientObstacles = [] + # Fixed obstacles are full strength + for x in self.fixedObstacles: + fullTransientObstacles.extend(x.getTelemTrans()) + for x in self.transientObstcales: if x.strength > .5: fullTransientObstacles.extend(x.getTelemTrans()) @@ -252,7 +256,7 @@ 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) + # self._doLookahead(curCmd) return nextCmd def _getCmd(self, curCmd:DrivetrainCommand, stepSize_m:float) -> DrivetrainCommand: diff --git a/robot.py b/robot.py index ee51ff9..9a4fc61 100644 --- a/robot.py +++ b/robot.py @@ -71,7 +71,7 @@ def robotPeriodic(self): self.autodrive.updateTelemetry() self.driveTrain.poseEst._telemetry.setCurAutoDriveWaypoints(self.autodrive.getWaypoints()) - #self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.rfp.getObstacleStrengths()) + self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.rfp.getObstacleStrengths()) self.stt.mark("Telemetry") self.ledCtrl.setAutoDrive(self.autodrive.isRunning())