Return a straight line path when pathfinding to same navgrid node

mjansen4857 · Jan 1, 2024 · d5aa10a · d5aa10a
1 parent 48bc7e9
commit d5aa10a
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 3 deletions.
diff --git 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
@@ -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
@@ -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;