From a22e90f0dc200e9f31c7bb35f10bb732cb60c462 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Tue, 6 Feb 2024 11:54:28 -0500 Subject: [PATCH] Add method to get a list of poses from a path --- pathplannerlib-python/pathplannerlib/logging.py | 3 +-- pathplannerlib-python/pathplannerlib/path.py | 9 +++++++++ pathplannerlib/.vscode/settings.json | 3 ++- .../com/pathplanner/lib/path/PathPlannerPath.java | 12 ++++++++++++ .../pathplanner/lib/util/PathPlannerLogging.java | 9 +-------- .../include/pathplanner/lib/path/PathPlannerPath.h | 14 ++++++++++++++ .../pathplanner/lib/util/PathPlannerLogging.h | 5 +---- 7 files changed, 40 insertions(+), 15 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/logging.py b/pathplannerlib-python/pathplannerlib/logging.py index 9e307f57..7dc76997 100644 --- a/pathplannerlib-python/pathplannerlib/logging.py +++ b/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([]) diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index 323cc1de..1f12872b 100644 --- a/pathplannerlib-python/pathplannerlib/path.py +++ b/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: diff --git a/pathplannerlib/.vscode/settings.json b/pathplannerlib/.vscode/settings.json index 2312a6d0..e0a3ddd8 100644 --- a/pathplannerlib/.vscode/settings.json +++ b/pathplannerlib/.vscode/settings.json @@ -87,6 +87,7 @@ "forward_list": "cpp", "ranges": "cpp", "valarray": "cpp", - "*.mac": "cpp" + "*.mac": "cpp", + "charconv": "cpp" } } \ No newline at end of file diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java index 3b8c96ce..e3571727 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/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 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 * diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/PathPlannerLogging.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/PathPlannerLogging.java index d8cbf086..1818ca1a 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/PathPlannerLogging.java +++ b/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 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<>()); } diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h index 25b3d647..b795bc49 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h @@ -234,6 +234,20 @@ class PathPlannerPath: public std::enable_shared_from_this { */ std::shared_ptr 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 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 fromJson(const wpi::json &json); diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/util/PathPlannerLogging.h b/pathplannerlib/src/main/native/include/pathplanner/lib/util/PathPlannerLogging.h index 43e62616..50f182e9 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/util/PathPlannerLogging.h +++ b/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);