Skip to content

Navgrid Based Start Tolerance for Pathfinding #1115

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
c-eid wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Navgrid Based Start Tolerance for Pathfinding #1115

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e3898b0
Untested navgrid size based starting distance
c-eid Aug 27, 2025
6743bf1
Tested and updated navgrid based minimum distance
c-eid Aug 30, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 5 additions & 2 deletions pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -289,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);
Expand Down
32 changes: 32 additions & 0 deletions pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -22,6 +28,32 @@ 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()) {
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() {
Expand Down