loop time improved, added obstacle telemetry back

RobotCasserole1736 · Nov 15, 2024 · 1e66d90 · 1e66d90
1 parent 147d94b
commit 1e66d90
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 3 deletions.
diff --git 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
@@ -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())