Add method to get a list of poses from a path

pathplannerlib-python/pathplannerlib/logging.py

@@ -64,7 +64,6 @@ def logActivePath(path: Union[PathPlannerPath, None]) -> None:
         """
         if PathPlannerLogging._logActivePath is not None:
             if path is not None:
-                poses = [Pose2d(p.position, Rotation2d()) for p in path.getAllPathPoints()]
-                PathPlannerLogging._logActivePath(poses)
+                PathPlannerLogging._logActivePath(path.getPathPoses())
             else:
                 PathPlannerLogging._logActivePath([])

pathplannerlib-python/pathplannerlib/path.py

@@ -875,6 +875,15 @@ def flipPath(self) -> PathPlannerPath:
         return PathPlannerPath(newBezier, self._globalConstraints, newEndState, newRotTargets, self._constraintZones,
                                newMarkers, self._reversed, newPreviewRot)
 
+    def getPathPoses(self) -> List[Pose2d]:
+        """
+        Get a list of poses representing every point in this path.
+        This can be used to display a path on a field 2d widget, for example.
+
+        :return: List of poses for each point in this path
+        """
+        return [Pose2d(p.position, Rotation2d()) for p in self._allPoints]
+
     @staticmethod
     def _mapPct(pct: float, seg1_pct: float) -> float:
         if pct <= seg1_pct:

pathplannerlib/.vscode/settings.json

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

pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java

@@ -1117,6 +1117,18 @@ public PathPlannerPath flipPath() {
         newPreviewRot);
   }
 
+  /**
+   * Get a list of poses representing every point in this path. This can be used to display a path
+   * on a field 2d widget, for example.
+   *
+   * @return List of poses for each point in this path
+   */
+  public List<Pose2d> getPathPoses() {
+    return allPoints.stream()
+        .map(p -> new Pose2d(p.position, new Rotation2d()))
+        .collect(Collectors.toList());
+  }
+
   /**
    * Map a given percentage/waypoint relative position over 2 segments
    *

pathplannerlib/src/main/java/com/pathplanner/lib/util/PathPlannerLogging.java

@@ -2,11 +2,9 @@
 
 import com.pathplanner.lib.path.PathPlannerPath;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.function.Consumer;
-import java.util.stream.Collectors;
 
 /** Utility class for handling custom logging callbacks */
 public class PathPlannerLogging {
@@ -73,13 +71,8 @@ public static void logTargetPose(Pose2d targetPose) {
    */
   public static void logActivePath(PathPlannerPath path) {
     if (logActivePath != null) {
-
       if (path != null) {
-        List<Pose2d> poses =
-            path.getAllPathPoints().stream()
-                .map(p -> new Pose2d(p.position, new Rotation2d()))
-                .collect(Collectors.toList());
-        logActivePath.accept(poses);
+        logActivePath.accept(path.getPathPoses());
       } else {
         logActivePath.accept(new ArrayList<>());
       }

pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h

@@ -234,6 +234,20 @@ class PathPlannerPath: public std::enable_shared_from_this<PathPlannerPath> {
 	 */
 	std::shared_ptr<PathPlannerPath> flipPath();
 
+	/**
+	 * Get a list of poses representing every point in this path. This can be used to display a path
+	 * on a field 2d widget, for example.
+	 *
+	 * @return List of poses for each point in this path
+	 */
+	inline std::vector<frc::Pose2d> getPathPoses() {
+		std::vector < frc::Pose2d > poses;
+		for (const PathPoint &point : m_allPoints) {
+			poses.emplace_back(point.position, frc::Rotation2d());
+		}
+		return poses;
+	}
+
 private:
 	static std::shared_ptr<PathPlannerPath> fromJson(const wpi::json &json);
 

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

@@ -42,10 +42,7 @@ class PathPlannerLogging {
 			std::vector < frc::Pose2d > poses;
 
 			if (path) {
-				for (const PathPoint &point : path->getAllPathPoints()) {
-					poses.push_back(
-							frc::Pose2d(point.position, frc::Rotation2d()));
-				}
+				poses = path->getPathPoses();
 			}
 
 			m_logActivePath(poses);