Fix various PPLib issues

pathplannerlib-python/pathplannerlib/events.py

@@ -126,6 +126,7 @@ def cancelEvent(self, eventScheduler: 'EventScheduler') -> None:
 
 class OneShotTriggerEvent(Event):
     _name: str
+    _resetCommand: Command
 
     def __init__(self, timestamp: float, name: str):
         """
@@ -136,19 +137,15 @@ def __init__(self, timestamp: float, name: str):
         """
         super().__init__(timestamp)
         self._name = name
+        self._resetCommand = cmd.waitSeconds(0).andThen(
+            cmd.runOnce(lambda: EventScheduler.setCondition(self._name, False))).ignoringDisable(True)
 
     @override
     def handleEvent(self, eventScheduler: 'EventScheduler') -> None:
+        EventScheduler.setCondition(self._name, True)
         # We schedule this command with the main command scheduler so that it is guaranteed to be run
         # in its entirety, since the EventScheduler could cancel this command before it finishes
-        CommandScheduler.getInstance().schedule(
-            cmd.sequence(
-                cmd.runOnce(lambda: EventScheduler.setCondition(self._name, True)),
-                cmd.waitSeconds(0),  # Wait for 0 seconds to delay until next loop
-                cmd.runOnce(lambda: EventScheduler.setCondition(self._name, False))
-            ).ignoringDisable(True)
-        )
-        eventScheduler.setCondition(self._name, False)
+        CommandScheduler.getInstance().schedule(self._resetCommand)
 
     @override
     def cancelEvent(self, eventScheduler: 'EventScheduler') -> None:

pathplannerlib-python/pathplannerlib/path.py

@@ -886,6 +886,7 @@ def _createPath(self) -> List[PathPoint]:
             distance = points[-1].position.distance(position)
             if distance <= 0.01:
                 pos = min(pos + targetIncrement, numSegments)
+                continue
 
             prevWaypointPos = pos - targetIncrement
 

pathplannerlib-python/pathplannerlib/pathfinders.py

@@ -402,14 +402,14 @@ def _createWaypoints(self, path: List[GridPosition], real_start_pos: Translation
             anchor1 = ((current - last) * LocalADStar._SMOOTHING_ANCHOR_PCT) + last
             heading1 = (current - last).angle()
             anchor2 = ((current - next) * LocalADStar._SMOOTHING_ANCHOR_PCT) + next
-            heading2 = (anchor2 - next).angle()
+            heading2 = (next - anchor2).angle()
 
             pathPoses.append(Pose2d(anchor1, heading1))
             pathPoses.append(Pose2d(anchor2, heading2))
 
         pathPoses.append(Pose2d(
             fieldPosPath[-1],
-            (fieldPosPath[-2] - fieldPosPath[-1]).angle()
+            (fieldPosPath[-1] - fieldPosPath[-2]).angle()
         ))
 
         return PathPlannerPath.waypointsFromPoses(pathPoses)

pathplannerlib/src/main/java/com/pathplanner/lib/events/OneShotTriggerEvent.java

@@ -1,11 +1,13 @@
 package com.pathplanner.lib.events;
 
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 
 /** Event that will activate a trigger, then deactivate it the next loop */
 public class OneShotTriggerEvent extends Event {
   private final String name;
+  private final Command resetCommand;
 
   /**
    * Create an event for activating a trigger, then deactivating it the next loop
@@ -16,6 +18,10 @@ public class OneShotTriggerEvent extends Event {
   public OneShotTriggerEvent(double timestamp, String name) {
     super(timestamp);
     this.name = name;
+    this.resetCommand =
+        Commands.waitSeconds(0)
+            .andThen(Commands.runOnce(() -> EventScheduler.setCondition(name, false)))
+            .ignoringDisable(true);
   }
 
   /**
@@ -25,15 +31,10 @@ public OneShotTriggerEvent(double timestamp, String name) {
    */
   @Override
   public void handleEvent(EventScheduler eventScheduler) {
+    EventScheduler.setCondition(name, true);
     // We schedule this command with the main command scheduler so that it is guaranteed to be run
     // in its entirety, since the EventScheduler could cancel this command before it finishes
-    CommandScheduler.getInstance()
-        .schedule(
-            Commands.sequence(
-                    Commands.runOnce(() -> EventScheduler.setCondition(name, true)),
-                    Commands.waitSeconds(0), // Wait for 0 seconds to delay until next loop
-                    Commands.runOnce(() -> EventScheduler.setCondition(name, false)))
-                .ignoringDisable(true));
+    CommandScheduler.getInstance().schedule(resetCommand);
   }
 
   /**

pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java

@@ -663,6 +663,7 @@ private List<PathPoint> createPath() {
       double distance = points.get(points.size() - 1).position.getDistance(position);
       if (distance <= 0.01) {
         pos = Math.min(pos + targetIncrement, numSegments);
+        continue;
       }
 
       double prevWaypointPos = pos - targetIncrement;

pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/LocalADStar.java

@@ -24,7 +24,6 @@
  */
 public class LocalADStar implements Pathfinder {
   private static final double SMOOTHING_ANCHOR_PCT = 0.8;
-  private static final double SMOOTHING_CONTROL_PCT = 0.33;
   private static final double EPS = 2.5;
 
   private double fieldLength = 16.54;
@@ -427,7 +426,7 @@ private List<Waypoint> createWaypoints(
       Translation2d anchor1 = current.minus(last).times(SMOOTHING_ANCHOR_PCT).plus(last);
       Rotation2d heading1 = current.minus(last).getAngle();
       Translation2d anchor2 = current.minus(next).times(SMOOTHING_ANCHOR_PCT).plus(next);
-      Rotation2d heading2 = anchor2.minus(next).getAngle();
+      Rotation2d heading2 = next.minus(anchor2).getAngle();
 
       pathPoses.add(new Pose2d(anchor1, heading1));
       pathPoses.add(new Pose2d(anchor2, heading2));
@@ -436,8 +435,8 @@ private List<Waypoint> createWaypoints(
         new Pose2d(
             fieldPosPath.get(fieldPosPath.size() - 1),
             fieldPosPath
-                .get(fieldPosPath.size() - 2)
-                .minus(fieldPosPath.get(fieldPosPath.size() - 1))
+                .get(fieldPosPath.size() - 1)
+                .minus(fieldPosPath.get(fieldPosPath.size() - 2))
                 .getAngle()));
 
     return PathPlannerPath.waypointsFromPoses(pathPoses);

pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp

@@ -88,7 +88,7 @@ void FollowPathCommand::Initialize() {
 void FollowPathCommand::Execute() {
 	units::second_t currentTime = m_timer.Get();
 	PathPlannerTrajectoryState targetState = m_trajectory.sample(currentTime);
-	if (m_controller->isHolonomic()) {
+	if (!m_controller->isHolonomic() && m_path->isReversed()) {
 		targetState = targetState.reverse();
 	}
 

pathplannerlib/src/main/native/cpp/pathplanner/lib/controllers/PPHolonomicDriveController.cpp

@@ -9,11 +9,10 @@ std::function<units::radians_per_second_t()> PPHolonomicDriveController::rotatio
 
 PPHolonomicDriveController::PPHolonomicDriveController(
 		PIDConstants translationConstants, PIDConstants rotationConstants,
-		units::meters_per_second_t maxModuleSpeed,
-		units::meter_t driveBaseRadius, units::second_t period) : m_xController(
+		units::second_t period) : m_xController(translationConstants.kP,
+		translationConstants.kI, translationConstants.kD, period), m_yController(
 		translationConstants.kP, translationConstants.kI,
-		translationConstants.kD, period), m_yController(translationConstants.kP,
-		translationConstants.kI, translationConstants.kD, period), m_rotationController(
+		translationConstants.kD, period), m_rotationController(
 		rotationConstants.kP, rotationConstants.kI, rotationConstants.kD,
 		period) {
 	m_xController.SetIntegratorRange(-translationConstants.iZone,

pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp

@@ -317,6 +317,7 @@ std::vector<PathPoint> PathPlannerPath::createPath() {
 		if (distance <= 0.01_m) {
 			pos = std::min(pos + targetIncrement,
 					static_cast<double>(numSegments));
+			continue;
 		}
 
 		double prevWaypointPos = pos - targetIncrement;

pathplannerlib/src/main/native/cpp/pathplanner/lib/path/Waypoint.cpp

@@ -14,7 +14,7 @@ Waypoint Waypoint::autoControlPoints(frc::Translation2d anchor,
 		prevControl = anchor - frc::Translation2d(d, heading);
 	}
 	if (nextAnchor.has_value()) {
-		auto d = anchor.Distance(nextControl.value())
+		auto d = anchor.Distance(nextAnchor.value())
 				* AUTO_CONTROL_DISTANCE_FACTOR;
 		nextControl = anchor + frc::Translation2d(d, heading);
 	}

pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp

@@ -389,14 +389,14 @@ std::vector<Waypoint> LocalADStar::createWaypoints(
 		frc::Rotation2d heading1 = (current - last).Angle();
 		frc::Translation2d anchor2 = ((current - next) * SMOOTHING_ANCHOR_PCT)
 				+ next;
-		frc::Rotation2d heading2 = (anchor2 - next).Angle();
+		frc::Rotation2d heading2 = (next - anchor2).Angle();
 
 		pathPoses.emplace_back(anchor1, heading1);
 		pathPoses.emplace_back(anchor2, heading2);
 	}
 	pathPoses.emplace_back(fieldPosPath[fieldPosPath.size() - 1],
-			(fieldPosPath[fieldPosPath.size() - 2]
-					- fieldPosPath[fieldPosPath.size() - 1]).Angle());
+			(fieldPosPath[fieldPosPath.size() - 1]
+					- fieldPosPath[fieldPosPath.size() - 2]).Angle());
 
 	return PathPlannerPath::waypointsFromPoses(pathPoses);
 }

pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp

@@ -302,7 +302,7 @@ void PathPlannerTrajectory::generateStates(
 void PathPlannerTrajectory::forwardAccelPass(
 		std::vector<PathPlannerTrajectoryState> &states,
 		const RobotConfig &config) {
-	for (size_t i = 1; i < states.size(); i++) {
+	for (size_t i = 1; i < states.size() - 1; i++) {
 		PathPlannerTrajectoryState &prevState = states[i - 1];
 		PathPlannerTrajectoryState &state = states[i];
 		PathPlannerTrajectoryState &nextState = states[i + 1];

pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPHolonomicDriveController.h

@@ -25,16 +25,10 @@ class PPHolonomicDriveController: public PathFollowingController {
 	 *
 	 * @param translationConstants PID constants for the translation PID controllers
 	 * @param rotationConstants PID constants for the rotation controller
-	 * @param maxModuleSpeed The max speed of a drive module in meters/sec
-	 * @param driveBaseRadius The radius of the drive base in meters. For swerve drive, this is the
-	 *     distance from the center of the robot to the furthest module. For mecanum, this is the
-	 *     drive base width / 2
 	 * @param period Period of the control loop in seconds
 	 */
 	PPHolonomicDriveController(PIDConstants translationConstants,
-			PIDConstants rotationConstants,
-			units::meters_per_second_t maxModuleSpeed,
-			units::meter_t driveBaseRadius, units::second_t period = 0.02_s);
+			PIDConstants rotationConstants, units::second_t period = 0.02_s);
 
 	/**
 	 * Enables and disables the controller for troubleshooting. When calculate() is called on a

pathplannerlib/src/main/native/include/pathplanner/lib/events/ActivateTriggerEvent.h

@@ -18,7 +18,7 @@ class ActivateTriggerEvent: public Event {
 	}
 
 	inline void handleEvent(EventScheduler *eventScheduler) override {
-		eventScheduler->setCondition(m_name, true);
+		EventScheduler::setCondition(m_name, true);
 	}
 
 	inline void cancelEvent(EventScheduler *eventScheduler) override {

pathplannerlib/src/main/native/include/pathplanner/lib/events/DeactivateTriggerEvent.h

@@ -18,12 +18,12 @@ class DeactivateTriggerEvent: public Event {
 	}
 
 	inline void handleEvent(EventScheduler *eventScheduler) override {
-		eventScheduler->setCondition(m_name, false);
+		EventScheduler::setCondition(m_name, false);
 	}
 
 	inline void cancelEvent(EventScheduler *eventScheduler) override {
 		// Ensure the condition gets set to false
-		eventScheduler->setCondition(m_name, false);
+		EventScheduler::setCondition(m_name, false);
 	}
 
 private:

pathplannerlib/src/main/native/include/pathplanner/lib/events/EventScheduler.h

@@ -28,8 +28,9 @@ class EventScheduler {
 	inline void initialize(PathPlannerTrajectory trajectory) {
 		m_eventCommands.clear();
 		m_upcomingEvents.clear();
-		m_upcomingEvents.insert(m_upcomingEvents.end(),
-				trajectory.getEvents().begin(), trajectory.getEvents().end());
+		auto events = trajectory.getEvents();
+		m_upcomingEvents.insert(m_upcomingEvents.end(), events.begin(),
+				events.end());
 	}
 
 	/**
@@ -56,8 +57,8 @@ class EventScheduler {
 			std::shared_ptr<PathPlannerPath> path) {
 		wpi::SmallSet<frc2::Subsystem*, 4> allReqs;
 		for (auto m : path->getEventMarkers()) {
-			allReqs.insert(m.getCommand()->GetRequirements().begin(),
-					m.getCommand()->GetRequirements().end());
+			auto markerReqs = m.getCommand()->GetRequirements();
+			allReqs.insert(markerReqs.begin(), markerReqs.end());
 		}
 		return allReqs;
 	}

pathplannerlib/src/main/native/include/pathplanner/lib/events/OneShotTriggerEvent.h

@@ -16,23 +16,19 @@ class OneShotTriggerEvent: public Event {
 	 * @param name The name of the trigger to control
 	 */
 	OneShotTriggerEvent(units::second_t timestamp, std::string name) : Event(
-			timestamp), m_name(name) {
+			timestamp), m_name(name), m_resetCommand(
+			frc2::cmd::Wait(0_s).AndThen(frc2::cmd::RunOnce([this]() {
+				EventScheduler::setCondition(m_name, false);
+			}
+			)
+			).IgnoringDisable(true)) {
 	}
 
 	inline void handleEvent(EventScheduler *eventScheduler) override {
+		EventScheduler::setCondition(m_name, true);
 		// We schedule this command with the main command scheduler so that it is guaranteed to be run
 		// in its entirety, since the EventScheduler could cancel this command before it finishes
-		eventScheduler->setCondition(m_name, true);
-		frc2::CommandScheduler::GetInstance().Schedule(
-				frc2::cmd::Sequence(
-						frc2::cmd::RunOnce([this, eventScheduler]() {
-							eventScheduler->setCondition(m_name, true);
-						}),
-						frc2::cmd::Wait(0_s),
-						frc2::cmd::RunOnce([this, eventScheduler]() {
-							eventScheduler->setCondition(m_name, false);
-						})
-				).IgnoringDisable(true));
+		frc2::CommandScheduler::GetInstance().Schedule(m_resetCommand);
 	}
 
 	inline void cancelEvent(EventScheduler *eventScheduler) override {
@@ -41,5 +37,6 @@ class OneShotTriggerEvent: public Event {
 
 private:
 	std::string m_name;
+	frc2::CommandPtr m_resetCommand;
 };
 }

pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h

@@ -343,10 +343,10 @@ class PathPlannerPath: public std::enable_shared_from_this<PathPlannerPath> {
 	static std::shared_ptr<PathPlannerPath> fromJson(const wpi::json &json);
 
 	static inline std::vector<Waypoint> waypointsFromJson(
-			const wpi::json &json) {
+			const wpi::json &waypointsJson) {
 		std::vector < Waypoint > waypoints;
-		for (wpi::json::const_reference waypointJson : json.at("waypoints")) {
-			waypoints.emplace_back(Waypoint::fromJson(waypointJson));
+		for (wpi::json::const_reference waypoint : waypointsJson) {
+			waypoints.emplace_back(Waypoint::fromJson(waypoint));
 		}
 		return waypoints;
 	}

pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/LocalADStar.h

@@ -69,7 +69,6 @@ class LocalADStar: public Pathfinder {
 
 private:
 	const double SMOOTHING_ANCHOR_PCT = 0.8;
-	const double SMOOTHING_CONTROL_PCT = 0.33;
 	const double EPS = 2.5;
 
 	double fieldLength;