Merge remote-tracking branch 'origin/main_nav' into main_nav

Author: gerth2

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 = []

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

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())

simgui-ds.json

@@ -96,6 +96,10 @@
     }
   ],
   "robotJoysticks": [
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    },
     {
       "guid": "Keyboard0"
     }

simgui.json

@@ -67,19 +67,44 @@
           "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,
             0.26989617943763733,
             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
       }
     }