RemoteADStar pathfinder implementation (#477)

* add java RemoteADStar pathfinder

* add c++ RemoteADStar

pathplannerlib/.vscode/settings.json

@@ -86,6 +86,7 @@
     "unordered_set": "cpp",
     "forward_list": "cpp",
     "ranges": "cpp",
-    "valarray": "cpp"
+    "valarray": "cpp",
+    "*.mac": "cpp"
   }
 }

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

@@ -0,0 +1,162 @@
+package com.pathplanner.lib.pathfinding;
+
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPoint;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.networktables.*;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Filesystem;
+import java.io.BufferedReader;
+import java.io.File;
+import java.io.FileReader;
+import java.util.ArrayList;
+import java.util.EnumSet;
+import java.util.List;
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicReference;
+
+public class RemoteADStar implements Pathfinder {
+  private final StringPublisher navGridJsonPub;
+  private final DoubleArrayPublisher startPosPub;
+  private final DoubleArrayPublisher goalPosPub;
+  private final DoubleArrayPublisher dynamicObsPub;
+
+  private final DoubleArraySubscriber pathPointsSub;
+
+  private final AtomicReference<List<PathPoint>> currentPath =
+      new AtomicReference<>(new ArrayList<>());
+  private final AtomicBoolean newPathAvailable = new AtomicBoolean(false);
+
+  public RemoteADStar() {
+    var nt = NetworkTableInstance.getDefault();
+
+    navGridJsonPub = nt.getStringTopic("/PPLibCoprocessor/RemoteADStar/navGrid").publish();
+    startPosPub = nt.getDoubleArrayTopic("/PPLibCoprocessor/RemoteADStar/startPos").publish();
+    goalPosPub = nt.getDoubleArrayTopic("/PPLibCoprocessor/RemoteADStar/goalPos").publish();
+    dynamicObsPub =
+        nt.getDoubleArrayTopic("/PPLibCoprocessor/RemoteADStar/dynamicObstacles").publish();
+
+    pathPointsSub =
+        nt.getDoubleArrayTopic("/PPLibCoprocessor/RemoteADStar/pathPoints")
+            .subscribe(
+                new double[0], PubSubOption.keepDuplicates(true), PubSubOption.sendAll(true));
+
+    nt.addListener(
+        pathPointsSub,
+        EnumSet.of(NetworkTableEvent.Kind.kValueAll),
+        event -> {
+          double[] pathPointsArr = pathPointsSub.get();
+
+          List<PathPoint> pathPoints = new ArrayList<>();
+          for (int i = 0; i <= pathPointsArr.length - 2; i += 2) {
+            pathPoints.add(
+                new PathPoint(new Translation2d(pathPointsArr[i], pathPointsArr[i + 1]), null));
+          }
+
+          currentPath.set(pathPoints);
+          newPathAvailable.set(true);
+        });
+
+    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();
+        navGridJsonPub.set(fileContent);
+      } catch (Exception e) {
+        DriverStation.reportError(
+            "RemoteADStar failed to load navgrid. Pathfinding will not be functional.", false);
+      }
+    }
+  }
+
+  /**
+   * Get if a new path has been calculated since the last time a path was retrieved
+   *
+   * @return True if a new path is available
+   */
+  @Override
+  public boolean isNewPathAvailable() {
+    return newPathAvailable.get();
+  }
+
+  /**
+   * Get the most recently calculated path
+   *
+   * @param constraints The path constraints to use when creating the path
+   * @param goalEndState The goal end state to use when creating the path
+   * @return The PathPlannerPath created from the points calculated by the pathfinder
+   */
+  @Override
+  public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) {
+    List<PathPoint> pathPoints = new ArrayList<>(currentPath.get());
+
+    newPathAvailable.set(false);
+
+    if (pathPoints.size() < 2) {
+      return null;
+    }
+
+    return PathPlannerPath.fromPathPoints(pathPoints, constraints, goalEndState);
+  }
+
+  /**
+   * Set the start position to pathfind from
+   *
+   * @param startPosition Start position on the field. If this is within an obstacle it will be
+   *     moved to the nearest non-obstacle node.
+   */
+  @Override
+  public void setStartPosition(Translation2d startPosition) {
+    startPosPub.set(new double[] {startPosition.getX(), startPosition.getY()});
+  }
+
+  /**
+   * Set the goal position to pathfind to
+   *
+   * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved
+   *     to the nearest non-obstacle node.
+   */
+  @Override
+  public void setGoalPosition(Translation2d goalPosition) {
+    goalPosPub.set(new double[] {goalPosition.getX(), goalPosition.getY()});
+  }
+
+  /**
+   * Set the dynamic obstacles that should be avoided while pathfinding.
+   *
+   * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents
+   *     opposite corners of a bounding box.
+   * @param currentRobotPos The current position of the robot. This is needed to change the start
+   *     position of the path to properly avoid obstacles
+   */
+  @Override
+  public void setDynamicObstacles(
+      List<Pair<Translation2d, Translation2d>> obs, Translation2d currentRobotPos) {
+    double[] obsArr = new double[((obs.size() * 2) + 1) * 2];
+
+    // First two doubles represent current robot pos
+    obsArr[0] = currentRobotPos.getX();
+    obsArr[1] = currentRobotPos.getY();
+
+    int idx = 2;
+    for (var box : obs) {
+      obsArr[idx] = box.getFirst().getX();
+      obsArr[idx + 1] = box.getFirst().getY();
+      obsArr[idx + 2] = box.getSecond().getX();
+      obsArr[idx + 3] = box.getSecond().getY();
+
+      idx += 4;
+    }
+
+    dynamicObsPub.set(obsArr);
+  }
+}

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

@@ -33,8 +33,7 @@ LocalADStar::LocalADStar() : fieldLength(16.54), fieldWidth(8.02), nodeSize(
 
 	if (!error_code) {
 		try {
-			wpi::json json = wpi::json::parse(fileBuffer->begin(),
-					fileBuffer->end());
+			wpi::json json = wpi::json::parse(fileBuffer->GetCharBuffer());
 
 			nodeSize = json.at("nodeSizeMeters").get<double>();
 			wpi::json::const_reference grid = json.at("grid");

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

@@ -0,0 +1,109 @@
+#include "pathplanner/lib/pathfinding/RemoteADStar.h"
+#include <frc/Filesystem.h>
+#include <wpi/MemoryBuffer.h>
+#include <wpi/json.h>
+#include <frc/Errors.h>
+
+using namespace pathplanner;
+
+RemoteADStar::RemoteADStar() {
+	auto nt = nt::NetworkTableInstance::GetDefault();
+
+	m_navGridJsonPub = nt.GetStringTopic(
+			"/PPLibCoprocessor/RemoteADStar/navGrid").Publish();
+	m_startPosPub = nt.GetDoubleArrayTopic(
+			"/PPLibCoprocessor/RemoteADStar/startPos").Publish();
+	m_goalPosPub = nt.GetDoubleArrayTopic(
+			"/PPLibCoprocessor/RemoteADStar/goalPos").Publish();
+	m_dynamicObsPub = nt.GetDoubleArrayTopic(
+			"/PPLibCoprocessor/RemoteADStar/dynamicObstacles").Publish();
+
+	auto options = nt::PubSubOptions();
+	options.keepDuplicates = true;
+	options.sendAll = true;
+	m_pathPointsSub = nt.GetDoubleArrayTopic(
+			"/PPLibCoprocessor/RemoteADStar/pathPoints").Subscribe(
+			std::vector<double>(), options);
+
+	m_pathListenerHandle = nt.AddListener(m_pathPointsSub,
+			nt::EventFlags::kValueAll, [this](const nt::Event &event) {
+				std::scoped_lock lock { m_mutex };
+
+				auto pathPointsArr = event.GetValueEventData()->value.GetDoubleArray();
+
+				m_currentPath.clear();
+				for (size_t i = 0; i <= pathPointsArr.size() - 2; i += 2) {
+					units::meter_t x { pathPointsArr[i] };
+					units::meter_t y { pathPointsArr[i + 1] };
+
+					m_currentPath.emplace_back(frc::Translation2d(x, y),
+							std::nullopt, std::nullopt);
+				}
+
+				m_newPathAvailable = true;
+			}
+			);
+
+	const std::string filePath = frc::filesystem::GetDeployDirectory()
+			+ "/pathplanner/navgrid.json";
+
+	std::error_code error_code;
+	std::unique_ptr < wpi::MemoryBuffer > fileBuffer =
+			wpi::MemoryBuffer::GetFile(filePath, error_code);
+
+	if (error_code) {
+		FRC_ReportError(frc::err::Error,
+				"RemoteADStar failed to load navgrid. Pathfinding will not be functional.");
+	} else {
+		auto charBuffer = fileBuffer->GetCharBuffer();
+		m_navGridJsonPub.Set(std::string(charBuffer.begin(), charBuffer.end()));
+	}
+
+	m_newPathAvailable = false;
+}
+
+std::shared_ptr<PathPlannerPath> RemoteADStar::getCurrentPath(
+		PathConstraints constraints, GoalEndState goalEndState) {
+	std::vector < PathPoint > pathPointsCopy;
+
+	{
+		std::scoped_lock lock { m_mutex };
+		pathPointsCopy.insert(pathPointsCopy.begin(), m_currentPath.begin(),
+				m_currentPath.end());
+		m_newPathAvailable = false;
+	}
+
+	if (pathPointsCopy.size() < 2) {
+		return nullptr;
+	}
+
+	return PathPlannerPath::fromPathPoints(pathPointsCopy, constraints,
+			goalEndState);
+}
+
+void RemoteADStar::setStartPosition(const frc::Translation2d &start) {
+	m_startPosPub.Set(std::vector<double> { start.X()(), start.Y()() });
+}
+
+void RemoteADStar::setGoalPosition(const frc::Translation2d &goal) {
+	m_goalPosPub.Set(std::vector<double> { goal.X()(), goal.Y()() });
+}
+
+void RemoteADStar::setDynamicObstacles(
+		const std::vector<std::pair<frc::Translation2d, frc::Translation2d>> &obs,
+		const frc::Translation2d &currentRobotPos) {
+	std::vector<double> obsArr;
+
+	// First two doubles represent current robot pos
+	obsArr.emplace_back(currentRobotPos.X()());
+	obsArr.emplace_back(currentRobotPos.Y()());
+
+	for (auto box : obs) {
+		obsArr.emplace_back(box.first.X()());
+		obsArr.emplace_back(box.first.Y()());
+		obsArr.emplace_back(box.second.X()());
+		obsArr.emplace_back(box.second.Y()());
+	}
+
+	m_dynamicObsPub.Set(obsArr);
+}

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

@@ -0,0 +1,53 @@
+#pragma once
+
+#include "pathplanner/lib/pathfinding/Pathfinder.h"
+#include "pathplanner/lib/path/PathPoint.h"
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/DoubleArrayTopic.h>
+#include <networktables/StringTopic.h>
+#include <networktables/NetworkTableListener.h>
+#include <vector>
+#include <wpi/mutex.h>
+
+namespace pathplanner {
+class RemoteADStar: public Pathfinder {
+public:
+	RemoteADStar();
+
+	~RemoteADStar() {
+		nt::NetworkTableInstance::GetDefault().RemoveListener(
+				m_pathListenerHandle);
+	}
+
+	inline bool isNewPathAvailable() override {
+		std::scoped_lock lock { m_mutex };
+
+		return m_newPathAvailable;
+	}
+
+	std::shared_ptr<PathPlannerPath> getCurrentPath(PathConstraints constraints,
+			GoalEndState goalEndState) override;
+
+	void setStartPosition(const frc::Translation2d &start) override;
+
+	void setGoalPosition(const frc::Translation2d &goal) override;
+
+	void setDynamicObstacles(
+			const std::vector<std::pair<frc::Translation2d, frc::Translation2d>> &obs,
+			const frc::Translation2d &currentRobotPos) override;
+
+private:
+	nt::StringPublisher m_navGridJsonPub;
+	nt::DoubleArrayPublisher m_startPosPub;
+	nt::DoubleArrayPublisher m_goalPosPub;
+	nt::DoubleArrayPublisher m_dynamicObsPub;
+
+	nt::DoubleArraySubscriber m_pathPointsSub;
+	NT_Listener m_pathListenerHandle;
+
+	std::vector<PathPoint> m_currentPath;
+	bool m_newPathAvailable;
+
+	wpi::mutex m_mutex;
+};
+}

pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h

@@ -3,7 +3,6 @@
 #include <networktables/NetworkTableInstance.h>
 #include <networktables/DoubleArrayTopic.h>
 #include <networktables/DoubleTopic.h>
-#include <networktables/DoubleArrayTopic.h>
 #include <networktables/NetworkTableListener.h>
 #include <string>
 #include <unordered_map>