Fix various python issues (#815)

pathplannerlib-python/pathplannerlib/auto.py

@@ -201,7 +201,7 @@ def configure(pose_supplier: Callable[[], Pose2d], reset_pose: Callable[[Pose2d]
         AutoBuilder._isHolonomic = robot_config.isHolonomic
 
         AutoBuilder._pathfindToPoseCommandBuilder = \
-            lambda pose, constraints, goal_end_vel, rotation_delay_distance: PathfindingCommand(
+            lambda pose, constraints, goal_end_vel: PathfindingCommand(
                 constraints,
                 pose_supplier,
                 robot_relative_speeds_supplier,
@@ -214,7 +214,7 @@ def configure(pose_supplier: Callable[[], Pose2d], reset_pose: Callable[[Pose2d]
                 goal_end_vel=goal_end_vel
             )
         AutoBuilder._pathfindThenFollowPathCommandBuilder = \
-            lambda path, constraints, rotation_delay_distance: PathfindThenFollowPath(
+            lambda path, constraints: PathfindThenFollowPath(
                 path,
                 constraints,
                 pose_supplier,

pathplannerlib-python/pathplannerlib/events.py

@@ -167,7 +167,7 @@ def __init__(self):
         self._eventCommands = {}
         self._upcomingEvents = []
 
-    def initialize(self, trajectory: PathPlannerTrajectory) -> None:
+    def initialize(self, trajectory: 'PathPlannerTrajectory') -> None:
         """
         Initialize the EventScheduler for the given trajectory. This should be called from the initialize
         method of the command running this scheduler.
@@ -194,7 +194,7 @@ def execute(self, time: float) -> None:
             if not self._eventCommands[command]:
                 continue
 
-            command.execute(self)
+            command.execute()
 
             if command.isFinished():
                 command.end(False)
@@ -220,7 +220,7 @@ def end(self) -> None:
         self._upcomingEvents.clear()
 
     @staticmethod
-    def getSchedulerRequirements(path: PathPlannerPath) -> set[Subsystem]:
+    def getSchedulerRequirements(path: 'PathPlannerPath') -> set[Subsystem]:
         """
         Get the event requirements for the given path
 

pathplannerlib-python/pathplannerlib/path.py

@@ -1,7 +1,7 @@
 from __future__ import annotations
 
 import math
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 from typing import Final, List, Union
 from wpimath.geometry import Rotation2d, Translation2d, Pose2d
 from wpimath.kinematics import ChassisSpeeds
@@ -177,13 +177,13 @@ class PointTowardsZone:
     targetPosition: Translation2d
     minWaypointRelativePos: float
     maxWaypointRelativePos: float
-    rotationOffset: Rotation2d = Rotation2d()
+    rotationOffset: Rotation2d = field(default_factory=Rotation2d)
 
     @staticmethod
     def fromJson(json_dict: dict) -> PointTowardsZone:
         targetPos = PointTowardsZone._translationFromJson(json_dict['fieldPosition'])
         minPos = float(json_dict['minWaypointRelativePos'])
-        maxPos = float(json_dict['minWaypointRelativePos'])
+        maxPos = float(json_dict['maxWaypointRelativePos'])
         deg = float(json_dict['rotationOffset'])
 
         return PointTowardsZone(targetPos, minPos, maxPos, Rotation2d.fromDegrees(deg))
@@ -388,9 +388,9 @@ def __init__(self, waypoints: List[Waypoint], constraints: PathConstraints,
             self._rotationTargets = holonomic_rotations
             self._rotationTargets.sort(key=lambda x: x.waypointRelativePosition)
         if point_towards_zones is None:
-            self.point_towards_zones = []
+            self._pointTowardsZones = []
         else:
-            self.point_towards_zones = point_towards_zones
+            self._pointTowardsZones = point_towards_zones
         if constraint_zones is None:
             self._constraintZones = []
         else:
@@ -399,7 +399,7 @@ def __init__(self, waypoints: List[Waypoint], constraints: PathConstraints,
             self._eventMarkers = []
         else:
             self._eventMarkers = event_markers
-            self._eventMarkers.sort(key=lambda x: x.waypointRelativePosition)
+            self._eventMarkers.sort(key=lambda x: x.waypointRelativePos)
         self._globalConstraints = constraints
         self._idealStartingState = ideal_starting_state
         self._goalEndState = goal_end_state

pathplannerlib-python/pathplannerlib/telemetry.py

@@ -6,11 +6,11 @@
 class PPLibTelemetry:
     _velPub: DoubleArrayPublisher = NetworkTableInstance.getDefault().getDoubleArrayTopic('/PathPlanner/vel').publish()
     _posePub: StructPublisher = NetworkTableInstance.getDefault().getStructTopic(
-        '/PathPlanner/currentPose', Pose2d.WPIStruct).publish()
+        '/PathPlanner/currentPose', Pose2d).publish()
     _pathPub: StructArrayPublisher = NetworkTableInstance.getDefault().getStructArrayTopic(
-        '/PathPlanner/activePath', Pose2d.WPIStruct).publish()
+        '/PathPlanner/activePath', Pose2d).publish()
     _targetPosePub: StructPublisher = NetworkTableInstance.getDefault().getStructTopic(
-        '/PathPlanner/targetPose', Pose2d.WPIStruct).publish()
+        '/PathPlanner/targetPose', Pose2d).publish()
 
     @staticmethod
     def setVelocities(actual_vel: float, commanded_vel: float, actual_ang_vel: float, commanded_ang_vel: float) -> None:

pathplannerlib-python/pathplannerlib/trajectory.py

@@ -15,23 +15,26 @@
 
 @dataclass
 class SwerveModuleTrajectoryState(SwerveModuleState):
-    fieldAngle: Rotation2d = Rotation2d()
-    fieldPos: Translation2d = Translation2d()
+    fieldAngle: Rotation2d = field(default_factory=Rotation2d)
+    fieldPos: Translation2d = field(default_factory=Translation2d)
 
     deltaPos: float = 0.0
 
+    def __init__(self):
+        super().__init__()
+
 
 @dataclass
 class PathPlannerTrajectoryState:
     timeSeconds: float = 0.0
     fieldSpeeds: ChassisSpeeds = ChassisSpeeds()
-    pose: Pose2d = Pose2d()
+    pose: Pose2d = field(default_factory=Pose2d)
     linearVelocity: float = 0.0
     feedforwards: List[DriveFeedforward] = field(default_factory=list)
 
-    heading: Rotation2d = Rotation2d()
+    heading: Rotation2d = field(default_factory=Rotation2d)
     deltaPos: float = 0.0
-    deltaRot: Rotation2d = Rotation2d()
+    deltaRot: Rotation2d = field(default_factory=Rotation2d)
     moduleStates: List[SwerveModuleTrajectoryState] = field(default_factory=list)
     constraints: PathConstraints = None
     waypointRelativePos: float = 0.0
@@ -226,7 +229,7 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch
                     for m in range(config.numModules):
                         accel = (state.moduleStates[m].speed - prevState.moduleStates[m].speed) / dt
                         appliedForce = wheelForces[m].norm() * (
-                                    wheelForces[m].angle() - state.moduleStates[m].angle).cos()
+                                wheelForces[m].angle() - state.moduleStates[m].angle).cos()
                         wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters
                         torqueCurrent = wheelTorque / config.moduleConfig.driveMotor.Kt
 
@@ -237,13 +240,13 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch
                     while len(unaddedEvents) > 0 and abs(
                             unaddedEvents[0].getTimestamp() - prevState.waypointRelativePos) <= abs(
                         unaddedEvents[0].getTimestamp() - state.waypointRelativePos):
-                        events.append(unaddedEvents.pop(0))
-                        events[-1].setTimestamp(prevState.timeSeconds)
+                        self._events.append(unaddedEvents.pop(0))
+                        self._events[-1].setTimestamp(prevState.timeSeconds)
 
                 while len(unaddedEvents) != 0:
                     # There are events that need to be added to the last state
-                    events.append(unaddedEvents.pop(0))
-                    events[-1].setTimestamp(self._states[-1].timeSeconds)
+                    self._events.append(unaddedEvents.pop(0))
+                    self._events[-1].setTimestamp(self._states[-1].timeSeconds)
 
                 # Create feedforwards for the end state
                 self._states[-1].feedforwards = [DriveFeedforward(0, 0, 0)] * config.numModules
@@ -490,7 +493,7 @@ def _forwardAccelPass(states: List[PathPlannerTrajectoryState], config: RobotCon
         # Even though kinematics is usually used for velocities, it can still
         # convert chassis accelerations to module accelerations
         maxAngAccel = state.constraints.maxAngularAccelerationRpsSq
-        angularAccel = max(min(totalTorque / config.MOI, -maxAngAccel), maxAngAccel)
+        angularAccel = min(max(totalTorque / config.MOI, -maxAngAccel), maxAngAccel)
 
         accelVec = linearForceVec / config.massKG
         maxAccel = state.constraints.maxAccelerationMpsSq
@@ -589,7 +592,7 @@ def _reverseAccelPass(states: List[PathPlannerTrajectoryState], config: RobotCon
         # Even though kinematics is usually used for velocities, it can still
         # convert chassis accelerations to module accelerations
         maxAngAccel = state.constraints.maxAngularAccelerationRpsSq
-        angularAccel = max(min(totalTorque / config.MOI, -maxAngAccel), maxAngAccel)
+        angularAccel = min(max(totalTorque / config.MOI, -maxAngAccel), maxAngAccel)
 
         accelVec = linearForceVec / config.massKG
         maxAccel = state.constraints.maxAccelerationMpsSq
@@ -607,7 +610,7 @@ def _reverseAccelPass(states: List[PathPlannerTrajectoryState], config: RobotCon
             # vf^2 = v0^2 + 2ad
             maxVel = math.sqrt(abs(math.pow(nextState.moduleStates[m].speed, 2) + (
                     2 * moduleAcceleration * nextState.moduleStates[m].deltaPos)))
-            state.moduleStates[m].speed = min(maxVel, nextState.moduleStates[m].speed)
+            state.moduleStates[m].speed = min(maxVel, state.moduleStates[m].speed)
 
         # Go over the modules again to make sure they take the same amount of time to reach the next state
         maxDT = 0.0