comments, renames, cleanup

drivetrain/controlStrategies/autoDrive.py

@@ -5,7 +5,8 @@
 from navigation.obstacleDetector import ObstacleDetector
 from utils.signalLogging import log
 from utils.singleton import Singleton
-from navigation.repulsorFieldPlanner import GOAL_PICKUP, GOAL_SPEAKER, RepulsorFieldPlanner
+from navigation.repulsorFieldPlanner import RepulsorFieldPlanner
+from navigation.navConstants import GOAL_PICKUP, GOAL_SPEAKER
 from drivetrain.drivetrainPhysical import MAX_DT_LINEAR_SPEED
 from utils.allianceTransformUtils import transform
 
@@ -47,9 +48,9 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand
         retCmd = cmdIn # default - no auto driving
 
         for obs in self._obsDet.getObstacles(curPose):
-            self.rfp.add_obstcale_observaton(obs)
+            self.rfp.addObstacleObservation(obs)
 
-        self.rfp._decay_observations()
+        self.rfp._decayObservations()
 
         # Handle command changes
         if(self._toPickup):

navigation/obstacles.py → navigation/forceGenerators.py

@@ -1,10 +1,13 @@
 import math
 from wpimath.geometry import Translation2d
 
-from navigation.navForce import Force, _logistic_func
+from navigation.navForce import Force, logisticFunc
 
-# Generic and specifc types of obstacles that repel a robot
-class Obstacle:
+class ForceGenerator:
+    """
+    Generic, common class for all objects that generate forces for pathplanning on the field.
+    Usually you should be using a specific type of force object, not this generic one
+    """
     def __init__(self, strength:float=1.0, radius:float = 0.25):
         self.strength = strength
         self.forceIsPositive = True
@@ -18,27 +21,47 @@ def __init__(self, strength:float=1.0, radius:float = 0.25):
         self.fieldSteepness = 3.5
 
     def setForceInverted(self, isInverted)->None:
+        """
+        Invert the force direction. 
+        """
         self.forceIsPositive = not isInverted
 
     def getForceAtPosition(self, position:Translation2d)->Force:
+        """
+        Abstract implementation that assumes the object has no force. 
+        Specific object types should override this
+        """
         return Force()
 
     def _distToForceMag(self, dist:float)->float:
+        """
+        Common method for converting a distance from the force object into a force strength
+        We're using the logistic function here, which (while not physically plaussable) has some
+        nice properties about getting bigger near the obstacle, while further objects do not have much
+        (or any) influence on the robot.
+        """
         dist = abs(dist)
-        #Sigmoid shape
-        forceMag = _logistic_func(-1.0 * dist, self.strength, self.fieldSteepness, -self.radius)
+        forceMag = logisticFunc(-1.0 * dist, self.strength, self.fieldSteepness, -self.radius)
         if(not self.forceIsPositive):
             forceMag *= -1.0
         return forceMag
     def getDist(self, position:Translation2d)->float:
+        """
+        Returns the distance (in meters) between the object and a given position
+        """
         return float('inf')
     def getTelemTrans(self)->list[Translation2d]:
+        """
+        return all x/y positions associated with this field force generating object
+        """
         return []
 
 
-# A point obstacle is defined as round, centered at a specific point, with a specific radius
-# It pushes the robot away radially outward from its center
-class PointObstacle(Obstacle):
+class PointObstacle(ForceGenerator):
+    """
+    A point obstacle is defined as round, centered at a specific point, with a specific radius
+    It pushes the robot away radially outward from its center
+    """
     def __init__(self, location:Translation2d, strength:float=1.0, radius:float = 0.3):
         self.location = location
         super().__init__(strength,radius)
@@ -59,10 +82,13 @@ def getTelemTrans(self) -> list[Translation2d]:
         return [self.location]
 
 
-# Linear obstacles are infinite lines at a specific coordinate
-# They push the robot away along a perpendicular direction
-# with the specific direction determined by forceIsPositive
-class HorizontalObstacle(Obstacle):
+class HorizontalObstacle(ForceGenerator):
+    """
+    Linear obstacles are infinite lines at a specific coordinate
+    They push the robot away along a perpendicular direction
+    with the specific direction determined by the force inversion.
+    The Horizontal Obstacle exists parallel to the X axis, at a specific Y coordinate, and pushes parallel to the Y axis.
+    """
     def __init__(self, y:float, strength:float=1.0, radius:float = 0.5):
         self.y=y
         super().__init__(strength,radius)
@@ -77,7 +103,13 @@ def getDist(self, position: Translation2d) -> float:
     def getTelemTrans(self) -> list[Translation2d]:
         return []
 
-class VerticalObstacle(Obstacle):
+class VerticalObstacle(ForceGenerator):
+    """
+    Linear obstacles are infinite lines at a specific coordinate
+    They push the robot away along a perpendicular direction
+    with the specific direction determined by the force inversion.
+    The Vertical Obstacle exists parallel to the Y axis, at a specific X coordinate, and pushes parallel to the X axis.
+    """
     def __init__(self, x:float, strength:float=1.0, radius:float = 0.5):
         self.x=x
         super().__init__(strength,radius)
@@ -88,8 +120,10 @@ def getForceAtPosition(self, position: Translation2d) -> Force:
     def getDist(self, position: Translation2d) -> float:
         return abs(position.x - self.x)
 
-# Describes a field force that exists along a line segment with a start and end point
-class LinearObstacle(Obstacle):
+class LinearForceGenerator(ForceGenerator):
+    """
+    A linear force generator creates forces based on the relative position of the robot to a specific line segment
+    """
     def __init__(self, start:Translation2d, end:Translation2d, strength:float=0.75, radius:float = 0.4):
         self.start = start
         self.end = end
@@ -123,9 +157,11 @@ def _shortestTransToSegment(self, point: Translation2d) -> Translation2d:
     def getDist(self, position: Translation2d) -> float:
         return self._shortestTransToSegment(position).norm()
 
-# Field formation that pushes the robot toward and along a line between start/end
-class Lane(LinearObstacle):
-
+class Lane(LinearForceGenerator):
+    """
+    A lane is an attractor - it creates a field force that pulls robots toward and along it, "ejecting" them out the far end.
+    It helps as a "hint" when the robot needs to navigate in a specific way around obstacles, which is not necessarily straight toward the goal.
+    """
     def getForceAtPosition(self, position: Translation2d) -> Force:
         toSeg = self._shortestTransToSegment(position)
         toSegUnit = toSeg/toSeg.norm()
@@ -143,9 +179,12 @@ def getForceAtPosition(self, position: Translation2d) -> Force:
 
         return Force(unitX*forceMag, unitY*forceMag)
 
-# Field formation that pushes the robot uniformly away from the line
-class Wall(LinearObstacle):
 
+class Wall(LinearForceGenerator):
+    """
+    Walls obstacles are finite lines between specific coordinates
+    They push the robot away along a perpendicular direction.
+    """
     def getForceAtPosition(self, position: Translation2d) -> Force:
         toSeg = self._shortestTransToSegment(position)
         toSegUnit = toSeg/toSeg.norm()

navigation/navConstants.py

@@ -1,3 +1,16 @@
+from wpimath.geometry import Pose2d, Rotation2d
 
+"""
+Constants related to navigation
+"""
+
+"""
+"Official" field dimensions
+"""
 FIELD_X_M = 16.54
 FIELD_Y_M = 8.21
+
+
+# Happy Constants for the goal poses we may want to drive to
+GOAL_PICKUP = Pose2d.fromFeet(40,5,Rotation2d.fromDegrees(0.0))
+GOAL_SPEAKER = Pose2d.fromFeet(3,20,Rotation2d.fromDegrees(180.0))

navigation/navForce.py

@@ -5,13 +5,19 @@
 import math
 
 
-def _logistic_func(x, L, k, x0):
-    """Logistic function."""
+def logisticFunc(x, L, k, x0):
+    """
+    Implements the Logistic function.
+    https://en.wikipedia.org/wiki/Logistic_function
+    This function is nice due to being exactly 0 at one side, 1.0 at the other, and a smooth transition between the two
+    """
     return L / (1 + math.exp(-k * (x - x0)))
 
-# Utility class for representing a force vector
 @dataclass
 class Force:
+    """
+    Simple class to represent a force in a 2d plane
+    """
     x:float=0
     y:float=0
     def __add__(self, other):

navigation/obstacleDetector.py

@@ -2,13 +2,19 @@
 from wrappers.wrapperedObstaclePhotonCamera import WrapperedObstaclePhotonCamera
 from drivetrain.drivetrainPhysical import ROBOT_TO_FRONT_CAM
 from wpimath.geometry import Pose2d
-from navigation.obstacles import Obstacle, PointObstacle
+from navigation.forceGenerators import ForceGenerator, PointObstacle
 
 class ObstacleDetector():
+    """
+    Class to use a PhotonVision-equipped coprocoessor and camera to deduce the location of obstacles on the field and report them
+    """
     def __init__(self):
         self.frontCam = WrapperedObstaclePhotonCamera("FRONT_CAM", ROBOT_TO_FRONT_CAM)
 
-    def getObstacles(self, curPose:Pose2d) -> list['Obstacle']:
+    def getObstacles(self, curPose:Pose2d) -> list['ForceGenerator']:
+        """
+        Returns the currently observed obstacles
+        """
         retList = []
 
         self.frontCam.update()

navigation/repulsorFieldPlanner.py

@@ -6,7 +6,7 @@
 from drivetrain.drivetrainCommand import DrivetrainCommand
 
 from navigation.navForce import Force
-from navigation.obstacles import HorizontalObstacle, Obstacle, PointObstacle, VerticalObstacle
+from navigation.forceGenerators import HorizontalObstacle, ForceGenerator, PointObstacle, VerticalObstacle
 from navigation.navConstants import FIELD_X_M, FIELD_Y_M
 
 # Relative strength of how hard the goal pulls the robot toward it
@@ -19,7 +19,7 @@
 
 # Rate at which transient obstacles decay in strength
 # Bigger numbers here make transient obstacles disappear faster
-TRANSIENT_OBS_DECAY_PER_LOOP = 0.1
+TRANSIENT_OBS_DECAY_PER_LOOP = 0.01
 
 # Obstacles come in two main flavors: Fixed and Transient.
 # Transient obstacles decay and disappear over time. They are things like other robots, or maybe gamepieces we don't want to drive through.
@@ -46,9 +46,6 @@
 
 WALLS = [B_WALL, T_WALL, L_WALL, R_WALL]
 
-# Happy Constants for the goal poses we may want to drive to
-GOAL_PICKUP = Pose2d.fromFeet(40,5,Rotation2d.fromDegrees(0.0))
-GOAL_SPEAKER = Pose2d.fromFeet(3,20,Rotation2d.fromDegrees(180.0))
 
 # Usually, the path planner assumes we traverse the path at a fixed velocity
 # However, near the goal, we'd like to slow down. This map defines how we ramp down
@@ -89,10 +86,10 @@ class RepulsorFieldPlanner:
     Finally, the planner will recommend a step of fixed size, in the direction the net force pushes on.
     """
     def __init__(self):
-        self.fixedObstacles:list[Obstacle] = []
+        self.fixedObstacles:list[ForceGenerator] = []
         self.fixedObstacles.extend(FIELD_OBSTACLES_2024)
         self.fixedObstacles.extend(WALLS)
-        self.transientObstcales:list[Obstacle] = []
+        self.transientObstcales:list[ForceGenerator] = []
         self.distToGo:float = 0.0
         self.goal:Pose2d|None = None
         self.lookaheadTraj:list[Pose2d] = []
@@ -105,7 +102,7 @@ def setGoal(self, goal:Pose2d|None):
         """
         self.goal = goal
 
-    def add_obstcale_observaton(self, obs:Obstacle):
+    def addObstacleObservation(self, obs:ForceGenerator):
         """
         Call this to report a new, transient obstacle observed on the field.
         """
@@ -127,7 +124,7 @@ def getGoalForce(self, curLocation:Translation2d) -> Force:
             # no goal, no force
             return Force()
 
-    def _decay_observations(self):
+    def _decayObservations(self):
         """
         Transient obstacles decay in strength over time. 
         This function decays the obstacle's strength, and removes obstacles which have zero strength.

robot.py

@@ -7,7 +7,7 @@
 from drivetrain.drivetrainControl import DrivetrainControl
 from humanInterface.driverInterface import DriverInterface
 from humanInterface.ledControl import LEDControl
-from navigation.obstacles import PointObstacle
+from navigation.forceGenerators import PointObstacle
 from utils.segmentTimeTracker import SegmentTimeTracker
 from utils.signalLogging import SignalWrangler
 from utils.calibration import CalibrationWrangler
@@ -127,7 +127,7 @@ def teleopPeriodic(self):
             ]
             for tf in tfs:
                 obs = PointObstacle(location=(ct+tf), strength=0.7)
-                self.autodrive.rfp.add_obstcale_observaton(obs)
+                self.autodrive.rfp.addObstacleObservation(obs)
 
         self.autodrive.setRequest(self.dInt.getNavToSpeaker(), self.dInt.getNavToPickup())
 

vectorPlotter.py

@@ -2,7 +2,8 @@
 import math
 import matplotlib.pyplot as plt
 import numpy as np
-from navigation.repulsorFieldPlanner import RepulsorFieldPlanner, GOAL_PICKUP, GOAL_SPEAKER
+from navigation.repulsorFieldPlanner import RepulsorFieldPlanner
+from navigation.navConstants import GOAL_PICKUP, GOAL_SPEAKER
 from wpimath.geometry import Translation2d
 import matplotlib.pyplot as plt
 import numpy as np

wrappers/wrapperedObstaclePhotonCamera.py

@@ -15,23 +15,33 @@ def __init__(self, time:float, estFieldPose:Pose2d, trustworthiness=1.0):
 
 MIN_AREA=10.0 #idk tune this if we are reacting to small targets
 
-def calculateDistanceToTargetMeters(
+def _calculateDistanceToTargetMeters(
         cameraHeightMeters:float,
         targetHeightMeters:float,
         cameraPitchRadians:float,
         targetPitchRadians:float):
+    """
+    Utility method to calculate the distance to the target from 2d targeting information
+    using right triangles, the assumption obstacles are on the ground, and the geometry
+    of where the camera is mounted on the robot.
+    """
     return (targetHeightMeters - cameraHeightMeters) / math.tan(cameraPitchRadians + targetPitchRadians)
 
 
-def estimateCameraToTargetTranslation(targetDistanceMeters:float, yaw:Rotation2d):
+def _estimateCameraToTargetTranslation(targetDistanceMeters:float, yaw:Rotation2d):
+        """
+        Utility to generate a Translation2d based on 
+        """
         return Translation2d(
                 yaw.cos() * targetDistanceMeters, yaw.sin() * targetDistanceMeters)
 
-# Wrappers photonvision to:
-# 1 - resolve issues with target ambiguity (two possible poses for each observation)
-# 2 - Convert pose estimates to the field
-# 3 - Handle recording latency of when the image was actually seen
 class WrapperedObstaclePhotonCamera:
+    """
+    Wrappers photonvision to:
+    1 - resolve issues with target ambiguity (two possible poses for each observation)
+    2 - Convert pose estimates to the field
+    3 - Handle recording latency of when the image was actually seen
+    """
     def __init__(self, camName, robotToCam:Transform3d):
         setVersionCheckEnabled(False)
 
@@ -77,13 +87,13 @@ def update(self):
                 # Use algorithm described at 
                 # https://docs.limelightvision.io/docs/docs-limelight/tutorials/tutorial-estimating-distance
                 # to estimate distance from the camera to the target.
-                dist = calculateDistanceToTargetMeters(
+                dist = _calculateDistanceToTargetMeters(
                     self.robotToCam.translation().Z(),
                     0.05, # Assume the average bumper starts 5cm off the ground
                     self.robotToCam.rotation().Y(), # Pitch is rotation about the Y axis
                     degreesToRadians(target.getPitch())
                 )
-                camToObstacle = estimateCameraToTargetTranslation(
+                camToObstacle = _estimateCameraToTargetTranslation(
                     dist,
                     Rotation2d.fromDegrees(target.getYaw())
                 )