prevent possible pathfinding crash (#557)

mjansen4857 · Jan 16, 2024 · 78917be · 78917be
1 parent 07ff0d6
commit 78917be
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 6 deletions.
diff --git a/pathplannerlib-python/pathplannerlib/commands.py b/pathplannerlib-python/pathplannerlib/commands.py
@@ -354,7 +354,8 @@ def initialize(self):
             if self._shouldFlipPath():
                 self._targetPose = flipFieldPose(self._targetPose)
 
-        if currentPose.translation().distance(self._targetPose.translation()) < 0.35:
+        if currentPose.translation().distance(self._targetPose.translation()) < 0.5:
+            self._output(ChassisSpeeds())
             self.cancel()
         else:
             Pathfinding.setStartPosition(currentPose.translation())
@@ -383,7 +384,7 @@ def execute(self):
                 # Find the two closest states in front of and behind robot
                 closestState1Idx = 0
                 closestState2Idx = 1
-                while True:
+                while closestState2Idx < len(self._currentTrajectory.getStates()) - 1:
                     closest2Dist = self._currentTrajectory.getState(closestState2Idx).positionMeters.distance(
                         currentPose.translation())
                     nextDist = self._currentTrajectory.getState(closestState2Idx + 1).positionMeters.distance(

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java
@@ -171,7 +171,8 @@ public void initialize() {
       }
     }
 
-    if (currentPose.getTranslation().getDistance(targetPose.getTranslation()) < 0.35) {
+    if (currentPose.getTranslation().getDistance(targetPose.getTranslation()) < 0.5) {
+      output.accept(new ChassisSpeeds());
       this.cancel();
     } else {
       Pathfinding.setStartPosition(currentPose.getTranslation());
@@ -207,7 +208,7 @@ public void execute() {
         // Find the two closest states in front of and behind robot
         int closestState1Idx = 0;
         int closestState2Idx = 1;
-        while (true) {
+        while (closestState2Idx < currentTrajectory.getStates().size() - 1) {
           double closest2Dist =
               currentTrajectory
                   .getState(closestState2Idx)

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp
@@ -94,7 +94,8 @@ void PathfindingCommand::Initialize() {
 	}
 
 	if (currentPose.Translation().Distance(m_targetPose.Translation())
-			< 0.35_m) {
+			< 0.5_m) {
+		m_output(frc::ChassisSpeeds());
 		Cancel();
 	} else {
 		Pathfinding::setStartPosition(currentPose.Translation());
@@ -126,7 +127,7 @@ void PathfindingCommand::Execute() {
 			// Find the two closest states in front of and behind robot
 			size_t closestState1Idx = 0;
 			size_t closestState2Idx = 1;
-			while (true) {
+			while (closestState2Idx < m_currentTrajectory.getStates().size() - 1) {
 				auto closest2Dist = m_currentTrajectory.getState(
 						closestState2Idx).position.Distance(
 						currentPose.Translation());