From e3898b05448d2f6f17d6e01f16d9b521127833c1 Mon Sep 17 00:00:00 2001 From: c-eid Date: Wed, 27 Aug 2025 13:03:38 -0500 Subject: [PATCH 1/2] Untested navgrid size based starting distance --- .../lib/commands/PathfindingCommand.java | 5 +++- .../lib/pathfinding/Pathfinding.java | 28 +++++++++++++++++++ 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java index 03e5a0240..b6e5b7956 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java @@ -262,7 +262,10 @@ public void initialize() { } } - if (currentPose.getTranslation().getDistance(targetPose.getTranslation()) < 0.5) { + if (currentPose.getTranslation().getDistance(targetPose.getTranslation()) + < (Pathfinding.getNavgridSize() * Math.sqrt(2))) { + // This finds the max distance in your current navgrid and makes sure that it cannot pathfind + // to itself. output.accept(new ChassisSpeeds(), DriveFeedforwards.zeros(robotConfig.numModules)); finish = true; } else { diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java index f5a2a74f2..4871acf5a 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java @@ -5,7 +5,13 @@ import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Filesystem; +import java.io.BufferedReader; +import java.io.File; +import java.io.FileReader; import java.util.List; +import org.json.simple.JSONObject; +import org.json.simple.parser.JSONParser; /** * Static class for interacting with the chosen pathfinding implementation from the pathfinding @@ -23,6 +29,28 @@ public static void setPathfinder(Pathfinder pathfinder) { Pathfinding.pathfinder = pathfinder; } + public static double getNavgridSize() { + File navGridFile = new File(Filesystem.getDeployDirectory(), "pathplanner/navgrid.json"); + if (navGridFile.exists()) { + try (BufferedReader br = new BufferedReader(new FileReader(navGridFile))) { + StringBuilder fileContentBuilder = new StringBuilder(); + String line; + while ((line = br.readLine()) != null) { + fileContentBuilder.append(line); + } + + String fileContent = fileContentBuilder.toString(); + JSONObject json = (JSONObject) new JSONParser().parse(fileContent); + + return ((Number) json.get("nodeSizeMeters")).doubleValue(); + + } catch (Exception e) { + return 0.3; + } + } + return 0.3; + } + /** Ensure that a pathfinding implementation has been chosen. If not, set it to the default. */ public static void ensureInitialized() { if (pathfinder == null) { From 6743bf1f385c4a1746c7c3da24010a3849821d09 Mon Sep 17 00:00:00 2001 From: c-eid Date: Sat, 30 Aug 2025 16:22:47 -0500 Subject: [PATCH 2/2] Tested and updated navgrid based minimum distance --- .../com/pathplanner/lib/commands/PathfindingCommand.java | 2 +- .../java/com/pathplanner/lib/pathfinding/Pathfinding.java | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java index b6e5b7956..be0025fff 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java @@ -292,7 +292,7 @@ public void execute() { && currentPose .getTranslation() .getDistance(currentTrajectory.getEndState().pose.getTranslation()) - < 2.0; + < (Pathfinding.getNavgridSize()*4); if (!skipUpdates && Pathfinding.isNewPathAvailable()) { currentPath = Pathfinding.getCurrentPath(constraints, goalEndState); diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java index 4871acf5a..85b5864ac 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java @@ -28,7 +28,11 @@ public class Pathfinding { public static void setPathfinder(Pathfinder pathfinder) { Pathfinding.pathfinder = pathfinder; } - + /** + * Get the current navgrid size from the navgrid.json file in deploy + * + * @return Navgrid size (double) + */ public static double getNavgridSize() { File navGridFile = new File(Filesystem.getDeployDirectory(), "pathplanner/navgrid.json"); if (navGridFile.exists()) {