diff --git a/pathplannerlib/.vscode/settings.json b/pathplannerlib/.vscode/settings.json index 019bb9dc..2312a6d0 100644 --- a/pathplannerlib/.vscode/settings.json +++ b/pathplannerlib/.vscode/settings.json @@ -86,6 +86,7 @@ "unordered_set": "cpp", "forward_list": "cpp", "ranges": "cpp", - "valarray": "cpp" + "valarray": "cpp", + "*.mac": "cpp" } } \ No newline at end of file diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/RemoteADStar.java b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/RemoteADStar.java new file mode 100644 index 00000000..6e4b28c4 --- /dev/null +++ b/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> 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 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 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> 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); + } +} diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp index aac89b18..f3e8c353 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp +++ b/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(); wpi::json::const_reference grid = json.at("grid"); diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/RemoteADStar.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/RemoteADStar.cpp new file mode 100644 index 00000000..baa4e891 --- /dev/null +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/RemoteADStar.cpp @@ -0,0 +1,109 @@ +#include "pathplanner/lib/pathfinding/RemoteADStar.h" +#include +#include +#include +#include + +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(), 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 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 { start.X()(), start.Y()() }); +} + +void RemoteADStar::setGoalPosition(const frc::Translation2d &goal) { + m_goalPosPub.Set(std::vector { goal.X()(), goal.Y()() }); +} + +void RemoteADStar::setDynamicObstacles( + const std::vector> &obs, + const frc::Translation2d ¤tRobotPos) { + std::vector 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); +} diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/RemoteADStar.h b/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/RemoteADStar.h new file mode 100644 index 00000000..b1cda647 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +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 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> &obs, + const frc::Translation2d ¤tRobotPos) 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 m_currentPath; + bool m_newPathAvailable; + + wpi::mutex m_mutex; +}; +} diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h b/pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h index 040fd82d..bb69942d 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h @@ -3,7 +3,6 @@ #include #include #include -#include #include #include #include