From d5aa10a6bfa288add75a01f5588348ae3d0911d4 Mon Sep 17 00:00:00 2001
From: Michael Jansen <mjansen4857@gmail.com>
Date: Mon, 1 Jan 2024 13:59:23 -0500
Subject: [PATCH] Return a straight line path when pathfinding to same navgrid
 node

---
 pathplannerlib-python/pathplannerlib/pathfinders.py          | 2 +-
 .../java/com/pathplanner/lib/pathfinding/LocalADStar.java    | 2 +-
 .../native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp   | 5 ++++-
 3 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/pathplannerlib-python/pathplannerlib/pathfinders.py b/pathplannerlib-python/pathplannerlib/pathfinders.py
index 3ab4c397..f725c483 100644
--- a/pathplannerlib-python/pathplannerlib/pathfinders.py
+++ b/pathplannerlib-python/pathplannerlib/pathfinders.py
@@ -456,7 +456,7 @@ def _doWork(self, needs_reset: bool, do_minor: bool, do_major: bool, s_start: Gr
     def _extractPath(self, s_start: GridPosition, s_goal: GridPosition, obstacles: Set[GridPosition]) -> List[
         GridPosition]:
         if s_goal == s_start:
-            return []
+            return [s_start, s_goal]
 
         path = [s_start]
 
diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/LocalADStar.java b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/LocalADStar.java
index 772ff6f4..0092beaf 100644
--- a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/LocalADStar.java
+++ b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/LocalADStar.java
@@ -352,7 +352,7 @@ private void doWork(
   private List<GridPosition> extractPath(
       GridPosition sStart, GridPosition sGoal, Set<GridPosition> obstacles) {
     if (sGoal.equals(sStart)) {
-      return new ArrayList<>();
+      return List.of(sStart, sGoal);
     }
 
     List<GridPosition> path = new ArrayList<>();
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 c591deb3..f0857d63 100644
--- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp
+++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp
@@ -312,7 +312,10 @@ std::vector<GridPosition> LocalADStar::extractPath(const GridPosition &sStart,
 		const GridPosition &sGoal,
 		const std::unordered_set<GridPosition> &obstacles) {
 	if (sGoal == sStart) {
-		return std::vector<GridPosition>();
+		std::vector < GridPosition > ret;
+		ret.emplace_back(sStart);
+		ret.emplace_back(sGoal);
+		return ret;
 	}
 
 	std::vector < GridPosition > path;