[Pathfinding] Add pathfinder

Pathfinding/build.gradle

@@ -0,0 +1,36 @@
+plugins {
+ id 'java-library'
+ id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3"
+}
+
+group 'com.swrobotics'
+version '2024'
+
+compileJava {
+ sourceCompatibility = '17'
+ targetCompatibility = '17'
+}
+
+dependencies {
+ api 'edu.wpi.first.wpimath:wpimath-java:2024.1.1-beta-3'
+ api 'edu.wpi.first.wpiutil:wpiutil-java:2024.1.1-beta-3'
+
+ implementation project(':Messenger:MessengerClient')
+
+ implementation 'com.google.code.gson:gson:2.9.0'
+
+ implementation wpi.java.deps.wpilib()
+ implementation wpi.java.vendor.java()
+}
+
+jar {
+ dependsOn ':Messenger:MessengerClient:jar'
+ manifest {
+ attributes 'Main-Class': 'com.swrobotics.pathfinding.task.PathfinderMain'
+ }
+
+ from {
+ configurations.compileClasspath.collect { it.isDirectory() ? it : zipTree(it) }
+ configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) }
+ }
+}

Pathfinding/config.json

@@ -0,0 +1,16 @@
+{
+ "messenger": {
+ "host": "localhost",
+ "port": 5805,
+ "name": "Pathfinder"
+ },
+ "field": {
+ "width": 16.4592,
+ "height": 8.2296,
+ "cellSize": 0.1524,
+ "originX": 0,
+ "originY": 1
+ },
+ "gridFile": "grids.json",
+ "finderType": "THETA_STAR"
+}

Pathfinding/grids.json

@@ -0,0 +1,25 @@
+{
+ "robot": {
+ "inverted": false,
+ "type": "CIRCLE",
+ "x": 0.0,
+ "y": 0.0,
+ "radius": 0.5
+ },
+ "grids": [
+ {
+ "type": "SHAPE",
+ "shapes": [
+ {
+ "type": "RECTANGLE",
+ "x": 8.2296,
+ "y": 4.1148,
+ "width": 16.4592,
+ "height": 8.2296,
+ "rotation": 0.0,
+ "inverted": true
+ }
+ ]
+ }
+ ]
+}

Pathfinding/src/main/java/com/swrobotics/pathfinding/core/FinderThread.java

@@ -0,0 +1,62 @@
+package com.swrobotics.pathfinding.core;
+
+import com.swrobotics.pathfinding.core.finder.Pathfinder;
+
+import java.util.List;
+
+public abstract class FinderThread<P> extends Thread {
+ private static final class Endpoints<P> {
+ private final P start, end;
+
+ public Endpoints(P start, P end) {
+ this.start = start;
+ this.end = end;
+ }
+ }
+
+ private final Pathfinder<P> finder;
+ private volatile Endpoints<P> endpoints;
+ private final Object lock;
+
+ public FinderThread(String name, Pathfinder<P> finder) {
+ super(name);
+ this.finder = finder;
+ lock = new Object();
+ }
+
+ public void setEndpoints(P start, P end) {
+ endpoints = new Endpoints<>(start, end);
+ synchronized (lock) {
+ lock.notifyAll();
+ }
+ }
+
+ protected abstract void reportResult(List<P> path);
+
+ @Override
+ public void run() {
+ while (!Thread.interrupted()) {
+ // Wait to be woken up when endpoints change
+ try {
+ synchronized (lock) {
+ lock.wait();
+ }
+ } catch (InterruptedException e) {
+ break;
+ }
+
+ if (endpoints == null) continue;
+
+ Endpoints<P> endpoints;
+ do {
+ endpoints = this.endpoints;
+
+ finder.setStart(endpoints.start);
+ finder.setGoal(endpoints.end);
+ reportResult(finder.findPath());
+
+ // Repeat if changed, since endpoints may have changed while solving
+ } while (endpoints != this.endpoints);
+ }
+ }
+}

Pathfinding/src/main/java/com/swrobotics/pathfinding/core/finder/AStarPathfinder.java

@@ -0,0 +1,110 @@
+package com.swrobotics.pathfinding.core.finder;
+
+import java.util.*;
+
+// TODO: Optimize if needed
+public class AStarPathfinder<P> implements Pathfinder<P> {
+ protected final Graph<P> graph;
+
+ private final Map<P, Node<P>> nodes = new HashMap<>();
+ private P startPoint, goalPoint;
+
+ public AStarPathfinder(Graph<P> graph) {
+ this.graph = graph;
+ }
+
+ @Override
+ public void setStart(P start) {
+ startPoint = start;
+ }
+
+ @Override
+ public void setGoal(P goal) {
+ goalPoint = goal;
+ }
+
+ protected static final class Node<P> implements Comparable<Node<P>> {
+ P position;
+ double priority;
+ double cost;
+ Node<P> parent;
+ boolean closed = false;
+
+ @Override
+ public int compareTo(Node<P> o) {
+ return Double.compare(priority, o.priority);
+ }
+ }
+
+ private List<P> extractPath(Node<P> node) {
+ List<P> out = new ArrayList<>();
+ while (node != null) {
+ out.add(0, node.position);
+ node = node.parent;
+ }
+ return out;
+ }
+
+ protected void computeCost(Node<P> current, Node<P> next) {
+ double cost = current.cost + graph.cost(current.position, next.position);
+ if (cost < next.cost) {
+ next.parent = current;
+ next.cost = cost;
+ }
+ }
+
+ private void updateVertex(PriorityQueue<Node<P>> open, Node<P> current, Node<P> next) {
+ double oldCost = next.cost;
+ computeCost(current, next);
+ if (next.cost < oldCost) {
+ open.remove(next);
+ next.priority = next.cost + graph.heuristic(next.position, goalPoint);
+ open.add(next);
+ }
+ }
+
+ private Node<P> getOrCreateNode(P point) {
+ Node<P> node = nodes.get(point);
+ if (node == null) {
+ node = new Node<>();
+ node.position = point;
+ nodes.put(point, node);
+ }
+ return node;
+ }
+
+ @Override
+ public List<P> findPath() {
+ nodes.clear();
+ Node<P> start = getOrCreateNode(startPoint);
+ Node<P> goal = getOrCreateNode(goalPoint);
+
+ PriorityQueue<Node<P>> open = new PriorityQueue<>();
+ start.priority = start.cost + graph.heuristic(start.position, goalPoint);
+ open.add(start);
+
+ int[] neighborCount = new int[1];
+ while (!open.isEmpty()) {
+ Node<P> current = open.remove();
+ if (current == goal) {
+ return extractPath(current);
+ }
+
+ current.closed = true;
+
+ P[] neighborPoints = graph.getNeighbors(current.position, neighborCount);
+ for (int i = 0; i < neighborCount[0]; i++) {
+ Node<P> next = getOrCreateNode(neighborPoints[i]);
+ if (!next.closed) {
+ if (!open.contains(next)) {
+ next.cost = Double.POSITIVE_INFINITY;
+ next.parent = null;
+ }
+ updateVertex(open, current, next);
+ }
+ }
+ }
+
+ return null;
+ }
+}

Pathfinding/src/main/java/com/swrobotics/pathfinding/core/finder/Graph.java

@@ -0,0 +1,11 @@
+package com.swrobotics.pathfinding.core.finder;
+
+public interface Graph<P> {
+ double heuristic(P point, P goal);
+
+ double cost(P current, P next);
+
+ // It is valid to return the same array again
+ // countOut[0] should be set to the number of neighbors
+ P[] getNeighbors(P current, int[] countOut);
+}

Pathfinding/src/main/java/com/swrobotics/pathfinding/core/finder/Pathfinder.java

@@ -0,0 +1,11 @@
+package com.swrobotics.pathfinding.core.finder;
+
+import java.util.List;
+
+public interface Pathfinder<P> {
+ void setStart(P start);
+
+ void setGoal(P goal);
+
+ List<P> findPath();
+}

Pathfinding/src/main/java/com/swrobotics/pathfinding/core/finder/SpatialGraph.java

@@ -0,0 +1,5 @@
+package com.swrobotics.pathfinding.core.finder;
+
+public interface SpatialGraph<P> extends Graph<P> {
+ boolean lineOfSight(P a, P b);
+}

Pathfinding/src/main/java/com/swrobotics/pathfinding/core/finder/ThetaStarPathfinder.java

@@ -0,0 +1,24 @@
+package com.swrobotics.pathfinding.core.finder;
+
+public final class ThetaStarPathfinder<P> extends AStarPathfinder<P> {
+ private final SpatialGraph<P> graph;
+
+ public ThetaStarPathfinder(SpatialGraph<P> graph) {
+ super(graph);
+ this.graph = graph;
+ }
+
+ @Override
+ protected void computeCost(Node<P> current, Node<P> next) {
+ if (current.parent != null && graph.lineOfSight(current.parent.position, next.position)) {
+ double newCost =
+ current.parent.cost + graph.cost(current.parent.position, next.position);
+ if (newCost < next.cost) {
+ next.parent = current.parent;
+ next.cost = newCost;
+ }
+ } else {
+ super.computeCost(current, next);
+ }
+ }
+}

Pathfinding/src/main/java/com/swrobotics/pathfinding/core/geom/Circle.java

@@ -0,0 +1,77 @@
+package com.swrobotics.pathfinding.core.geom;
+
+import com.google.gson.JsonDeserializationContext;
+import com.google.gson.JsonDeserializer;
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonParseException;
+import com.google.gson.JsonSerializationContext;
+import com.google.gson.JsonSerializer;
+import com.swrobotics.messenger.client.MessageBuilder;
+
+import java.lang.reflect.Type;
+
+public final class Circle extends RobotShape {
+ private final double x;
+ private final double y;
+ private final double radius;
+
+ public Circle(double x, double y, double radius, boolean inverted) {
+ super(inverted);
+ this.x = x;
+ this.y = y;
+ this.radius = radius;
+ }
+
+ public double getX() {
+ return x;
+ }
+
+ public double getY() {
+ return y;
+ }
+
+ public double getRadius() {
+ return radius;
+ }
+
+ @Override
+ public boolean collidesWith(RobotShape robot, double robotX, double robotY) {
+ return CollisionChecks.checkCircleVsCircleRobot(this, (Circle) robot, robotX, robotY);
+ }
+
+ @Override
+ public void writeToMessenger(MessageBuilder builder) {
+ super.writeToMessenger(builder);
+ builder.addByte(ShapeType.CIRCLE.getTypeId());
+ builder.addDouble(x);
+ builder.addDouble(y);
+ builder.addDouble(radius);
+ }
+
+ public static final class Serializer
+ implements JsonSerializer<Circle>, JsonDeserializer<Circle> {
+ @Override
+ public Circle deserialize(
+ JsonElement json, Type typeOfT, JsonDeserializationContext context)
+ throws JsonParseException {
+ JsonObject obj = json.getAsJsonObject();
+ boolean inverted = obj.has("inverted") && obj.get("inverted").getAsBoolean();
+ double x = obj.get("x").getAsDouble();
+ double y = obj.get("y").getAsDouble();
+ double radius = obj.get("radius").getAsDouble();
+ return new Circle(x, y, radius, inverted);
+ }
+
+ @Override
+ public JsonElement serialize(Circle src, Type typeOfSrc, JsonSerializationContext context) {
+ JsonObject obj = new JsonObject();
+ obj.addProperty("inverted", src.isInverted());
+ obj.addProperty("type", ShapeType.CIRCLE.toString());
+ obj.addProperty("x", src.x);
+ obj.addProperty("y", src.y);
+ obj.addProperty("radius", src.radius);
+ return obj;
+ }
+ }
+}