diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp index bc0cb455..e8c97545 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp @@ -1,5 +1,5 @@ #include "pathplanner/lib/commands/PathfindingCommand.h" -#include "pathplanner/lib/pathfinding/ADStar.h" +#include "pathplanner/lib/pathfinding/Pathfinding.h" #include "pathplanner/lib/util/GeometryUtil.h" #include @@ -18,7 +18,7 @@ PathfindingCommand::PathfindingCommand( rotationDelayDistance) { AddRequirements(requirements); - ADStar::ensureInitialized(); + Pathfinding::ensureInitialized(); frc::Rotation2d targetRotation; for (PathPoint p : m_targetPath->getAllPathPoints()) { @@ -48,7 +48,7 @@ PathfindingCommand::PathfindingCommand(frc::Pose2d targetPose, rotationDelayDistance) { AddRequirements(requirements); - ADStar::ensureInitialized(); + Pathfinding::ensureInitialized(); } void PathfindingCommand::Initialize() { @@ -64,12 +64,12 @@ void PathfindingCommand::Initialize() { m_goalEndState.getRotation()); } - if (ADStar::getGridPos(currentPose.Translation()) - == ADStar::getGridPos(m_targetPose.Translation())) { + if (currentPose.Translation().Distance(m_targetPose.Translation()) + < 0.25_m) { Cancel(); } else { - ADStar::setStartPos(currentPose.Translation()); - ADStar::setGoalPos(m_targetPose.Translation()); + Pathfinding::setStartPosition(currentPose.Translation()); + Pathfinding::setGoalPosition(m_targetPose.Translation()); } m_startingPose = currentPose; @@ -82,16 +82,10 @@ void PathfindingCommand::Execute() { PathPlannerLogging::logCurrentPose(currentPose); PPLibTelemetry::setCurrentPose(currentPose); - if (ADStar::isNewPathAvailable()) { - std::vector < frc::Translation2d > bezierPoints = - ADStar::getCurrentPath(); - - if (bezierPoints.size() >= 4) { - auto path = - std::make_shared < PathPlannerPath - > (bezierPoints, std::vector(), std::vector< - ConstraintsZone>(), std::vector(), m_constraints, m_goalEndState, false); + if (Pathfinding::isNewPathAvailable()) { + auto path = Pathfinding::getCurrentPath(m_constraints, m_goalEndState); + if (path) { if (currentPose.Translation().Distance(path->getPoint(0).position) <= 0.25_m) { m_currentTrajectory = PathPlannerTrajectory(path, diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/ADStar.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp similarity index 66% rename from pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/ADStar.cpp rename to pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp index 696c3f63..e927162c 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/ADStar.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp @@ -1,4 +1,4 @@ -#include "pathplanner/lib/pathfinding/ADStar.h" +#include "pathplanner/lib/pathfinding/LocalADStar.h" #include #include #include @@ -9,111 +9,72 @@ using namespace pathplanner; -const double ADStar::SMOOTHING_ANCHOR_PCT = 0.8; -const double ADStar::SMOOTHING_CONTROL_PCT = 0.33; - -double ADStar::FIELD_LENGTH = 16.54; -double ADStar::FIELD_WIDTH = 8.02; - -double ADStar::NODE_SIZE = 0.2; - -int ADStar::NODE_X = static_cast(std::ceil( - ADStar::FIELD_LENGTH / ADStar::NODE_SIZE)); -int ADStar::NODE_Y = static_cast(std::ceil( - ADStar::FIELD_WIDTH / ADStar::NODE_SIZE)); - -const double ADStar::EPS = 2.5; - -std::unordered_map ADStar::g; -std::unordered_map ADStar::rhs; -std::unordered_map> ADStar::open; -std::unordered_map> ADStar::incons; -std::unordered_set ADStar::closed; -std::unordered_set ADStar::staticObstacles; -std::unordered_set ADStar::dynamicObstacles; -std::unordered_set ADStar::obstacles; - -ADStar::GridPosition ADStar::sStart; -frc::Translation2d ADStar::realStartPos; -ADStar::GridPosition ADStar::sGoal; -frc::Translation2d ADStar::realGoalPos; - -double ADStar::eps = ADStar::EPS; - -std::thread ADStar::planningThread; -std::mutex ADStar::mutex; - -bool ADStar::doMinor = true; -bool ADStar::doMajor = true; -bool ADStar::needsReset = true; -bool ADStar::needsExtract = false; -bool ADStar::running = false; -bool ADStar::newPathAvailable = false; - -std::vector ADStar::currentPath; -void ADStar::ensureInitialized() { - if (!running) { - running = true; - sStart = GridPosition(0, 0); - realStartPos = frc::Translation2d(0_m, 0_m); - sGoal = GridPosition(0, 0); - realGoalPos = frc::Translation2d(0_m, 0_m); - - staticObstacles.clear(); - dynamicObstacles.clear(); +LocalADStar::LocalADStar() : fieldLength(16.54), fieldWidth(8.02), nodeSize( + 0.2), nodesX(static_cast(std::ceil(fieldLength / nodeSize))), nodesY( + static_cast(std::ceil(fieldWidth / nodeSize))), g(), rhs(), open(), incons(), closed(), staticObstacles(), dynamicObstacles(), obstacles(), sStart(), realStartPos(), sGoal(), realGoalPos(), eps( + EPS), planningThread(), mutex(), doMinor(true), doMajor(true), needsReset( + true), needsExtract(false), newPathAvailable(false), currentPath() { + sStart = GridPosition(0, 0); + realStartPos = frc::Translation2d(0_m, 0_m); + sGoal = GridPosition(0, 0); + realGoalPos = frc::Translation2d(0_m, 0_m); - const std::string filePath = frc::filesystem::GetDeployDirectory() - + "/pathplanner/navgrid.json"; + staticObstacles.clear(); + dynamicObstacles.clear(); - std::error_code error_code; - wpi::raw_fd_istream input { filePath, error_code }; + const std::string filePath = frc::filesystem::GetDeployDirectory() + + "/pathplanner/navgrid.json"; - if (!error_code) { - try { - wpi::json json; - input >> json; + std::error_code error_code; + wpi::raw_fd_istream input { filePath, error_code }; - NODE_SIZE = json.at("nodeSizeMeters").get(); - wpi::json::const_reference grid = json.at("grid"); - NODE_Y = grid.size(); - for (size_t row = 0; row < grid.size(); row++) { - wpi::json::const_reference rowArr = grid[row]; - if (row == 0) { - NODE_X = rowArr.size(); - } - for (size_t col = 0; col < rowArr.size(); col++) { - bool isObstacle = rowArr[col].get(); - if (isObstacle) { - staticObstacles.emplace(col, row); - } + if (!error_code) { + try { + wpi::json json; + input >> json; + + nodeSize = json.at("nodeSizeMeters").get(); + wpi::json::const_reference grid = json.at("grid"); + nodesY = grid.size(); + for (size_t row = 0; row < grid.size(); row++) { + wpi::json::const_reference rowArr = grid[row]; + if (row == 0) { + nodesX = rowArr.size(); + } + for (size_t col = 0; col < rowArr.size(); col++) { + bool isObstacle = rowArr[col].get(); + if (isObstacle) { + staticObstacles.emplace(col, row); } } - - wpi::json::const_reference fieldSize = json.at("field_size"); - FIELD_LENGTH = fieldSize.at("x").get(); - FIELD_WIDTH = fieldSize.at("y").get(); - } catch (...) { - // Ignore, just use defaults } + + wpi::json::const_reference fieldSize = json.at("field_size"); + fieldLength = fieldSize.at("x").get(); + fieldWidth = fieldSize.at("y").get(); + } catch (...) { + // Ignore, just use defaults } + } - obstacles.clear(); - obstacles.insert(staticObstacles.begin(), staticObstacles.end()); - obstacles.insert(dynamicObstacles.begin(), dynamicObstacles.end()); + obstacles.clear(); + obstacles.insert(staticObstacles.begin(), staticObstacles.end()); + obstacles.insert(dynamicObstacles.begin(), dynamicObstacles.end()); - needsReset = true; - doMajor = true; - doMinor = true; + needsReset = true; + doMajor = true; + doMinor = true; - newPathAvailable = false; + newPathAvailable = false; - planningThread = std::thread(ADStar::runThread); - planningThread.detach(); - } + planningThread = std::thread([this]() { + runThread(); + }); + planningThread.detach(); } -void ADStar::runThread() { - while (running) { +void LocalADStar::runThread() { + while (true) { try { std::lock_guard < std::mutex > lock(mutex); @@ -134,7 +95,7 @@ void ADStar::runThread() { } } -void ADStar::doWork() { +void LocalADStar::doWork() { if (needsReset) { reset(); needsReset = false; @@ -165,17 +126,27 @@ void ADStar::doWork() { } } -std::vector ADStar::getCurrentPath() { - if (!running) { - FRC_ReportError(frc::warn::Warning, - "ADStar path was retrieved before it was initialized"); +std::shared_ptr LocalADStar::getCurrentPath( + PathConstraints constraints, GoalEndState goalEndState) { + std::vector < frc::Translation2d > bezierPoints; + + { + std::lock_guard < std::mutex > lock(mutex); + + bezierPoints = currentPath; + newPathAvailable = false; } - newPathAvailable = false; - return currentPath; + if (bezierPoints.size() < 4) { + // Not enough points to make a path + return nullptr; + } + + return std::make_shared < PathPlannerPath + > (bezierPoints, constraints, goalEndState); } -void ADStar::setStartPos(const frc::Translation2d &start) { +void LocalADStar::setStartPosition(const frc::Translation2d &start) { std::lock_guard < std::mutex > lock(mutex); GridPosition startPos = findClosestNonObstacle(getGridPos(start)); @@ -188,7 +159,7 @@ void ADStar::setStartPos(const frc::Translation2d &start) { } } -void ADStar::setGoalPos(const frc::Translation2d &goal) { +void LocalADStar::setGoalPosition(const frc::Translation2d &goal) { std::lock_guard < std::mutex > lock(mutex); GridPosition gridPos = findClosestNonObstacle(getGridPos(goal)); @@ -203,7 +174,7 @@ void ADStar::setGoalPos(const frc::Translation2d &goal) { } } -ADStar::GridPosition ADStar::findClosestNonObstacle(const GridPosition &pos) { +GridPosition LocalADStar::findClosestNonObstacle(const GridPosition &pos) { if (!obstacles.contains(pos)) { return pos; } @@ -234,7 +205,7 @@ ADStar::GridPosition ADStar::findClosestNonObstacle(const GridPosition &pos) { return pos; } -void ADStar::setDynamicObstacles( +void LocalADStar::setDynamicObstacles( const std::vector> &obs, const frc::Translation2d ¤tRobotPos) { std::unordered_set < GridPosition > newObs; @@ -269,11 +240,11 @@ void ADStar::setDynamicObstacles( doMajor = true; } - setStartPos(currentRobotPos); - setGoalPos (realGoalPos); + setStartPosition(currentRobotPos); + setGoalPosition (realGoalPos); } -std::vector ADStar::extractPath() { +std::vector LocalADStar::extractPath() { if (sGoal == sStart) { return std::vector { realGoalPos }; } @@ -369,8 +340,7 @@ std::vector ADStar::extractPath() { return bezierPoints; } -bool ADStar::walkable(const ADStar::GridPosition &s1, - const ADStar::GridPosition &s2) { +bool LocalADStar::walkable(const GridPosition &s1, const GridPosition &s2) { int x0 = s1.x; int y0 = s1.y; int x1 = s2.x; @@ -410,15 +380,15 @@ bool ADStar::walkable(const ADStar::GridPosition &s1, return true; } -void ADStar::reset() { +void LocalADStar::reset() { g.clear(); rhs.clear(); open.clear(); incons.clear(); closed.clear(); - for (int x = 0; x < NODE_X; x++) { - for (int y = 0; y < NODE_Y; y++) { + for (int x = 0; x < nodesX; x++) { + for (int y = 0; y < nodesY; y++) { g[GridPosition(x, y)] = std::numeric_limits::infinity(); rhs[GridPosition(x, y)] = std::numeric_limits::infinity(); } @@ -431,7 +401,7 @@ void ADStar::reset() { open[sGoal] = key(sGoal); } -void ADStar::computeOrImprovePath() { +void LocalADStar::computeOrImprovePath() { while (true) { auto svOpt = topKey(); if (!svOpt.has_value()) { @@ -465,7 +435,7 @@ void ADStar::computeOrImprovePath() { } } -void ADStar::updateState(const ADStar::GridPosition &s) { +void LocalADStar::updateState(const GridPosition &s) { if (s != sGoal) { rhs[s] = std::numeric_limits::infinity(); @@ -485,8 +455,8 @@ void ADStar::updateState(const ADStar::GridPosition &s) { } } -bool ADStar::isCollision(const ADStar::GridPosition &sStart, - const ADStar::GridPosition &sEnd) { +bool LocalADStar::isCollision(const GridPosition &sStart, + const GridPosition &sEnd) { if (obstacles.contains(sStart) || obstacles.contains(sEnd)) { return true; } @@ -513,15 +483,15 @@ bool ADStar::isCollision(const ADStar::GridPosition &sStart, return false; } -std::unordered_set ADStar::getOpenNeighbors( - const ADStar::GridPosition &s) { +std::unordered_set LocalADStar::getOpenNeighbors( + const GridPosition &s) { std::unordered_set < GridPosition > ret; for (int xMove = -1; xMove <= 1; xMove++) { for (int yMove = -1; yMove <= 1; yMove++) { GridPosition sNext = GridPosition(s.x + xMove, s.y + yMove); - if (!obstacles.contains(sNext) && sNext.x >= 0 && sNext.x < NODE_X - && sNext.y >= 0 && sNext.y < NODE_Y) { + if (!obstacles.contains(sNext) && sNext.x >= 0 && sNext.x < nodesX + && sNext.y >= 0 && sNext.y < nodesY) { ret.emplace(sNext); } } @@ -529,15 +499,15 @@ std::unordered_set ADStar::getOpenNeighbors( return ret; } -std::unordered_set ADStar::getAllNeighbors( - const ADStar::GridPosition &s) { +std::unordered_set LocalADStar::getAllNeighbors( + const GridPosition &s) { std::unordered_set < GridPosition > ret; for (int xMove = -1; xMove <= 1; xMove++) { for (int yMove = -1; yMove <= 1; yMove++) { GridPosition sNext = GridPosition(s.x + xMove, s.y + yMove); - if (sNext.x >= 0 && sNext.x < NODE_X && sNext.y >= 0 - && sNext.y < NODE_Y) { + if (sNext.x >= 0 && sNext.x < nodesX && sNext.y >= 0 + && sNext.y < nodesY) { ret.emplace(sNext); } } @@ -545,7 +515,7 @@ std::unordered_set ADStar::getAllNeighbors( return ret; } -std::pair ADStar::key(const ADStar::GridPosition &s) { +std::pair LocalADStar::key(const GridPosition &s) { if (g.at(s) > rhs.at(s)) { return std::pair(rhs.at(s) + eps * heuristic(sStart, s), rhs.at(s)); @@ -555,9 +525,9 @@ std::pair ADStar::key(const ADStar::GridPosition &s) { } } -std::optional>> ADStar::topKey() { - std::optional < std::pair> - > min = std::nullopt; +std::optional>> LocalADStar::topKey() { + std::optional < std::pair> > min = + std::nullopt; for (auto entry : open) { if (!min || comparePair(entry.second, min.value().second) < 0) { diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/Pathfinding.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/Pathfinding.cpp new file mode 100644 index 00000000..c8d201ad --- /dev/null +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/Pathfinding.cpp @@ -0,0 +1,13 @@ +#include "pathplanner/lib/pathfinding/Pathfinding.h" +#include "pathplanner/lib/pathfinding/Pathfinder.h" +#include "pathplanner/lib/pathfinding/LocalADStar.h" + +using namespace pathplanner; + +std::unique_ptr Pathfinding::pathfinder; + +void Pathfinding::ensureInitialized() { + if (!pathfinder) { + pathfinder = std::make_unique(); + } +} diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/ADStar.h b/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/ADStar.h deleted file mode 100644 index b2b5f56a..00000000 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/ADStar.h +++ /dev/null @@ -1,172 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace pathplanner { -class ADStar { -public: - static void ensureInitialized(); - - static inline bool isNewPathAvailable() { - return newPathAvailable; - } - - static std::vector getCurrentPath(); - - static void setStartPos(const frc::Translation2d &start); - - static void setGoalPos(const frc::Translation2d &goal); - - static void setDynamicObstacles( - const std::vector> &obs, - const frc::Translation2d ¤tRobotPos); - - class GridPosition { - public: - int x; - int y; - - constexpr GridPosition() : x(0), y(0) { - } - - constexpr GridPosition(const int xPos, const int yPos) : x(xPos), y( - yPos) { - } - - constexpr bool operator==(const GridPosition &other) const { - return x == other.x && y == other.y; - } - }; - - static inline GridPosition getGridPos(const frc::Translation2d &pos) { - return GridPosition(static_cast(std::floor(pos.X()() / NODE_SIZE)), - static_cast(std::floor(pos.Y()() / NODE_SIZE))); - } - -private: - static const double SMOOTHING_ANCHOR_PCT; - static const double SMOOTHING_CONTROL_PCT; - - static double FIELD_LENGTH; - static double FIELD_WIDTH; - - static double NODE_SIZE; - - static int NODE_X; - static int NODE_Y; - - static const double EPS; - - static std::unordered_map g; - static std::unordered_map rhs; - static std::unordered_map> open; - static std::unordered_map> incons; - static std::unordered_set closed; - static std::unordered_set staticObstacles; - static std::unordered_set dynamicObstacles; - static std::unordered_set obstacles; - - static GridPosition sStart; - static frc::Translation2d realStartPos; - static GridPosition sGoal; - static frc::Translation2d realGoalPos; - - static double eps; - - static std::thread planningThread; - static std::mutex mutex; - - static bool doMinor; - static bool doMajor; - static bool needsReset; - static bool needsExtract; - static bool running; - static bool newPathAvailable; - - static std::vector currentPath; - - static void runThread(); - - static void doWork(); - - static GridPosition findClosestNonObstacle(const GridPosition &pos); - - static std::vector extractPath(); - - static bool walkable(const GridPosition &s1, const GridPosition &s2); - - static void reset(); - - static void computeOrImprovePath(); - - static void updateState(const GridPosition &s); - - static inline double cost(const GridPosition &sStart, - const GridPosition &sGoal) { - if (isCollision(sStart, sGoal)) { - return std::numeric_limits::infinity(); - } - return heuristic(sStart, sGoal); - } - - static bool isCollision(const GridPosition &sStart, - const GridPosition &sEnd); - - static std::unordered_set getOpenNeighbors( - const GridPosition &s); - - static std::unordered_set getAllNeighbors( - const GridPosition &s); - - static std::pair key(const GridPosition &s); - - static std::optional>> topKey(); - - static inline double heuristic(const GridPosition &sStart, - const GridPosition &sGoal) { - return std::hypot(sGoal.x - sStart.x, sGoal.y - sStart.y); - } - - static constexpr int comparePair(const std::pair &a, - const std::pair &b) { - int first = compareDouble(a.first, b.first); - if (first == 0) { - return compareDouble(a.second, b.second); - } else { - return first; - } - } - - static constexpr int compareDouble(const double a, const double b) { - if (a < b) { - return -1; - } else if (a > b) { - return 1; - } - return 0; - } - - static inline frc::Translation2d gridPosToTranslation2d( - const GridPosition &pos) { - return frc::Translation2d( - units::meter_t { (pos.x * NODE_SIZE) + (NODE_SIZE / 2.0) }, - units::meter_t { (pos.y * NODE_SIZE) + (NODE_SIZE / 2.0) }); - } -}; -} - -namespace std { -template<> -struct hash { - size_t operator()(const pathplanner::ADStar::GridPosition &gridPos) const { - return ((hash()(gridPos.x) ^ ((hash()(gridPos.y) << 1) >> 1))); - } -}; -} diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/LocalADStar.h b/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/LocalADStar.h new file mode 100644 index 00000000..b0359012 --- /dev/null +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/LocalADStar.h @@ -0,0 +1,171 @@ +#pragma once + +#include "pathplanner/lib/pathfinding/Pathfinder.h" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pathplanner { +class GridPosition { +public: + int x; + int y; + + constexpr GridPosition() : x(0), y(0) { + } + + constexpr GridPosition(const int xPos, const int yPos) : x(xPos), y(yPos) { + } + + constexpr bool operator==(const GridPosition &other) const { + return x == other.x && y == other.y; + } +}; +} + +namespace std { +template<> +struct hash { + size_t operator()(const pathplanner::GridPosition &gridPos) const { + return ((hash()(gridPos.x) ^ ((hash()(gridPos.y) << 1) >> 1))); + } +}; +} + +namespace pathplanner { +class LocalADStar: public Pathfinder { +public: + LocalADStar(); + + ~LocalADStar() { + } + + inline bool isNewPathAvailable() override { + return 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; + + inline GridPosition getGridPos(const frc::Translation2d &pos) { + return GridPosition(static_cast(std::floor(pos.X()() / nodeSize)), + static_cast(std::floor(pos.Y()() / nodeSize))); + } + +private: + const double SMOOTHING_ANCHOR_PCT = 0.8; + const double SMOOTHING_CONTROL_PCT = 0.33; + const double EPS = 2.5; + + double fieldLength; + double fieldWidth; + + double nodeSize; + + int nodesX; + int nodesY; + + std::unordered_map g; + std::unordered_map rhs; + std::unordered_map> open; + std::unordered_map> incons; + std::unordered_set closed; + std::unordered_set staticObstacles; + std::unordered_set dynamicObstacles; + std::unordered_set obstacles; + + GridPosition sStart; + frc::Translation2d realStartPos; + GridPosition sGoal; + frc::Translation2d realGoalPos; + + double eps; + + std::thread planningThread; + std::mutex mutex; + + bool doMinor; + bool doMajor; + bool needsReset; + bool needsExtract; + bool newPathAvailable; + + std::vector currentPath; + + void runThread(); + + void doWork(); + + GridPosition findClosestNonObstacle(const GridPosition &pos); + + std::vector extractPath(); + + bool walkable(const GridPosition &s1, const GridPosition &s2); + + void reset(); + + void computeOrImprovePath(); + + void updateState(const GridPosition &s); + + inline double cost(const GridPosition &sStart, const GridPosition &sGoal) { + if (isCollision(sStart, sGoal)) { + return std::numeric_limits::infinity(); + } + return heuristic(sStart, sGoal); + } + + bool isCollision(const GridPosition &sStart, const GridPosition &sEnd); + + std::unordered_set getOpenNeighbors(const GridPosition &s); + + std::unordered_set getAllNeighbors(const GridPosition &s); + + std::pair key(const GridPosition &s); + + std::optional>> topKey(); + + inline double heuristic(const GridPosition &sStart, + const GridPosition &sGoal) { + return std::hypot(sGoal.x - sStart.x, sGoal.y - sStart.y); + } + + constexpr int comparePair(const std::pair &a, + const std::pair &b) { + int first = compareDouble(a.first, b.first); + if (first == 0) { + return compareDouble(a.second, b.second); + } else { + return first; + } + } + + constexpr int compareDouble(const double a, const double b) { + if (a < b) { + return -1; + } else if (a > b) { + return 1; + } + return 0; + } + + inline frc::Translation2d gridPosToTranslation2d(const GridPosition &pos) { + return frc::Translation2d( + units::meter_t { (pos.x * nodeSize) + (nodeSize / 2.0) }, + units::meter_t { (pos.y * nodeSize) + (nodeSize / 2.0) }); + } +}; +} diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/Pathfinder.h b/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/Pathfinder.h new file mode 100644 index 00000000..29c7969b --- /dev/null +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/Pathfinder.h @@ -0,0 +1,62 @@ +#pragma once + +#include "pathplanner/lib/path/GoalEndState.h" +#include "pathplanner/lib/path/PathConstraints.h" +#include "pathplanner/lib/path/PathPlannerPath.h" +#include +#include +#include +#include + +namespace pathplanner { +class Pathfinder { +public: + virtual ~Pathfinder() { + } + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + virtual bool isNewPathAvailable() = 0; + + /** + * 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 + */ + virtual std::shared_ptr getCurrentPath( + PathConstraints constraints, GoalEndState goalEndState) = 0; + + /** + * 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. + */ + virtual void setStartPosition(const frc::Translation2d &startPosition) = 0; + + /** + * 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. + */ + virtual void setGoalPosition(const frc::Translation2d &goalPosition) = 0; + + /** + * 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 + */ + virtual void setDynamicObstacles( + const std::vector> &obs, + const frc::Translation2d ¤tRobotPos) = 0; +}; +} diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/Pathfinding.h b/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/Pathfinding.h new file mode 100644 index 00000000..0e5ba5d3 --- /dev/null +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/Pathfinding.h @@ -0,0 +1,80 @@ +#pragma once + +#include "pathplanner/lib/pathfinding/Pathfinder.h" +#include + +namespace pathplanner { +class Pathfinding { +public: + /** + * Set the pathfinder that should be used by the path following commands + * + * @param pathfinder The pathfinder to use + */ + static inline void setPathfinder(std::unique_ptr pathfinder) { + Pathfinding::pathfinder = std::move(pathfinder); + } + + /** Ensure that a pathfinding implementation has been chosen. If not, set it to the default. */ + static void ensureInitialized(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + static inline bool isNewPathAvailable() { + return pathfinder->isNewPathAvailable(); + } + + /** + * 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 + */ + static inline std::shared_ptr getCurrentPath( + PathConstraints constraints, GoalEndState goalEndState) { + return pathfinder->getCurrentPath(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. + */ + static inline void setStartPosition( + const frc::Translation2d &startPosition) { + pathfinder->setStartPosition(startPosition); + } + + /** + * 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. + */ + static inline void setGoalPosition(const frc::Translation2d &goalPosition) { + pathfinder->setGoalPosition(goalPosition); + } + + /** + * 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 if the robot is now within an obstacle. + */ + static inline void setDynamicObstacles( + const std::vector> &obs, + const frc::Translation2d ¤tRobotPos) { + pathfinder->setDynamicObstacles(obs, currentRobotPos); + } + +private: + static std::unique_ptr pathfinder; +}; +}