From 258323bb482ed84b38997354f0c6266bb56fd9d8 Mon Sep 17 00:00:00 2001 From: PJ Reiniger Date: Wed, 1 Nov 2023 01:25:26 -0400 Subject: [PATCH 1/6] Expose holomonic preview path --- .../pathplanner/lib/path/PathPlannerPath.java | 73 ++++++++++++++-- .../pathplanner/lib/path/PathPlannerPath.cpp | 23 +++-- .../pathplanner/lib/path/PathPlannerPath.h | 21 ++++- .../lib/path/PathPlannerPathTest.java | 85 +++++++++++++++++++ .../lib/path/PathPlannerPathTest.cpp | 55 ++++++++++++ 5 files changed, 243 insertions(+), 14 deletions(-) create mode 100644 pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java create mode 100644 pathplannerlib/src/test/native/cpp/pathplanner/lib/path/PathPlannerPathTest.cpp 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 b1df9153..b2234c90 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -19,6 +19,27 @@ /** A PathPlanner path. NOTE: This is not a trajectory and isn't directly followed. */ public class PathPlannerPath { + + /** Container for the preview starting state */ + public static class PreviewStartingState { + private Rotation2d rotation; + private double velocityMps; + + /** Default constructor */ + public PreviewStartingState() {} + + /** + * Constructor + * + * @param rotation The rotation of the starting preview poin + * @param velocityMps The velocity of the starting preview point + */ + public PreviewStartingState(Rotation2d rotation, double velocityMps) { + this.rotation = rotation; + this.velocityMps = velocityMps; + } + } + private List bezierPoints; private List rotationTargets; private List constraintZones; @@ -27,6 +48,7 @@ public class PathPlannerPath { private GoalEndState goalEndState; private List allPoints; private boolean reversed; + private PreviewStartingState previewStartingState; /** * Create a new path planner path @@ -37,7 +59,8 @@ public class PathPlannerPath { * @param eventMarkers List of event markers along the path * @param globalConstraints The global constraints of the path * @param goalEndState The goal end state of the path - * @param reversed Should the robot follow the path reversed (differential drive only) + * @param reversed If the path is reversed. Differential mode only + * @param previewStartingState The settings used for previews in the UI */ public PathPlannerPath( List bezierPoints, @@ -46,7 +69,8 @@ public PathPlannerPath( List eventMarkers, PathConstraints globalConstraints, GoalEndState goalEndState, - boolean reversed) { + boolean reversed, + PreviewStartingState previewStartingState) { this.bezierPoints = bezierPoints; this.rotationTargets = holonomicRotations; this.constraintZones = constraintZones; @@ -55,6 +79,7 @@ public PathPlannerPath( this.goalEndState = goalEndState; this.reversed = reversed; this.allPoints = createPath(this.bezierPoints, this.rotationTargets, this.constraintZones); + this.previewStartingState = previewStartingState; precalcValues(); } @@ -82,7 +107,8 @@ public PathPlannerPath( Collections.emptyList(), constraints, goalEndState, - reversed); + reversed, + new PreviewStartingState()); } /** @@ -109,6 +135,7 @@ private PathPlannerPath(PathConstraints globalConstraints, GoalEndState goalEndS this.goalEndState = goalEndState; this.reversed = false; this.allPoints = new ArrayList<>(); + this.previewStartingState = new PreviewStartingState(); } /** @@ -204,6 +231,7 @@ public void hotReload(JSONObject pathJson) { this.goalEndState = updatedPath.goalEndState; this.allPoints = updatedPath.allPoints; this.reversed = updatedPath.reversed; + this.previewStartingState = updatedPath.previewStartingState; } /** @@ -259,6 +287,17 @@ private static PathPlannerPath fromJson(JSONObject pathJson) { eventMarkers.add(EventMarker.fromJson((JSONObject) markerJson)); } + PreviewStartingState previewStartingState = new PreviewStartingState(); + if (pathJson.containsKey("previewStartingState")) { + JSONObject previewStartingStateJson = (JSONObject) pathJson.get("previewStartingState"); + if (previewStartingStateJson != null) { + previewStartingState.rotation = + Rotation2d.fromDegrees((Double) previewStartingStateJson.get("rotation")); + previewStartingState.velocityMps = + ((Number) previewStartingStateJson.get("velocity")).doubleValue(); + } + } + return new PathPlannerPath( bezierPoints, rotationTargets, @@ -266,7 +305,8 @@ private static PathPlannerPath fromJson(JSONObject pathJson) { eventMarkers, globalConstraints, goalEndState, - reversed); + reversed, + previewStartingState); } private static List bezierPointsFromWaypointsJson(JSONArray waypointsJson) { @@ -317,6 +357,19 @@ public Pose2d getStartingDifferentialPose() { return new Pose2d(startPos, heading); } + /** + * Get the starting pose for the holomonic path based on the preview settings + * + * @return Pose at the path's starting point + */ + public Pose2d getStartingHolomonicPreviewPose() { + Translation2d startPos = getPoint(0).position; + Rotation2d heading = + previewStartingState == null ? Rotation2d.fromDegrees(0) : previewStartingState.rotation; + + return new Pose2d(startPos, heading); + } + /** * Get the constraints for a point along the path * @@ -566,7 +619,8 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds) Collections.emptyList(), globalConstraints, goalEndState, - reversed); + reversed, + previewStartingState); } else if ((closestPointIdx == 0 && robotNextControl == null) || (Math.abs(closestDist - startingPose.getTranslation().getDistance(getPoint(0).position)) <= 0.25 @@ -627,7 +681,8 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds) .collect(Collectors.toList()), globalConstraints, goalEndState, - reversed); + reversed, + previewStartingState); } } @@ -660,7 +715,8 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds) Collections.emptyList(), globalConstraints, goalEndState, - reversed); + reversed, + previewStartingState); } if (bezierPoints.isEmpty()) { @@ -796,7 +852,8 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds) mappedMarkers, globalConstraints, goalEndState, - reversed); + reversed, + previewStartingState); } /** diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp index 3bdce683..ea61d8cb 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -14,10 +14,11 @@ PathPlannerPath::PathPlannerPath(std::vector bezierPoints, std::vector constraintZones, std::vector eventMarkers, PathConstraints globalConstraints, GoalEndState goalEndState, - bool reversed) : m_bezierPoints(bezierPoints), m_rotationTargets( - rotationTargets), m_constraintZones(constraintZones), m_eventMarkers( - eventMarkers), m_globalConstraints(globalConstraints), m_goalEndState( - goalEndState), m_reversed(reversed) { + bool reversed, PreviewStartingState previewStartingState) : m_bezierPoints( + bezierPoints), m_rotationTargets(rotationTargets), m_constraintZones( + constraintZones), m_eventMarkers(eventMarkers), m_globalConstraints( + globalConstraints), m_goalEndState(goalEndState), m_reversed(reversed), m_previewStartingState( + previewStartingState) { m_allPoints = PathPlannerPath::createPath(m_bezierPoints, m_rotationTargets, m_constraintZones); @@ -127,8 +128,16 @@ PathPlannerPath PathPlannerPath::fromJson(const wpi::json &json) { eventMarkers.push_back(EventMarker::fromJson(markerJson)); } + PreviewStartingState previewStartingState; + if (json.contains("previewStartingState")) { + auto jsonStartingState = json["previewStartingState"]; + previewStartingState = { units::degree_t(jsonStartingState["rotation"]), + units::meters_per_second_t(jsonStartingState["velocity"]) }; + } + return PathPlannerPath(bezierPoints, rotationTargets, constraintZones, - eventMarkers, globalConstraints, goalEndState, reversed); + eventMarkers, globalConstraints, goalEndState, reversed, + previewStartingState); } std::vector PathPlannerPath::bezierPointsFromWaypointsJson( @@ -238,6 +247,10 @@ frc::Pose2d PathPlannerPath::getStartingDifferentialPose() { return frc::Pose2d(startPos, heading); } +frc::Pose2d PathPlannerPath::getStartingHolomonicPreviewPose() { + return frc::Pose2d(getPoint(0).position, m_previewStartingState.m_rotation); +} + void PathPlannerPath::precalcValues() { if (numPoints() > 0) { for (size_t i = 0; i < m_allPoints.size(); i++) { 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 d742d688..dea0154e 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h @@ -19,6 +19,16 @@ namespace pathplanner { class PathPlannerPath { public: + + /** + * Container for the preview starting state + */ + class PreviewStartingState { + public: + frc::Rotation2d m_rotation; + units::meters_per_second_t m_velocity; + }; + /** * Create a new path planner path * @@ -35,7 +45,8 @@ class PathPlannerPath { std::vector constraintZones, std::vector eventMarkers, PathConstraints globalConstraints, GoalEndState goalEndState, - bool reversed); + bool reversed, PreviewStartingState previewStartingState = + PreviewStartingState()); /** * Simplified constructor to create a path with no rotation targets, constraint zones, or event @@ -81,6 +92,13 @@ class PathPlannerPath { */ frc::Pose2d getStartingDifferentialPose(); + /** + * Get the starting pose for the holomonic path based on the preview settings + * + * @return Pose at the path's starting point + */ + frc::Pose2d getStartingHolomonicPreviewPose(); + /** * Get the constraints for a point along the path * @@ -234,5 +252,6 @@ class PathPlannerPath { GoalEndState m_goalEndState; std::vector m_allPoints; bool m_reversed; + PreviewStartingState m_previewStartingState; }; } diff --git a/pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java b/pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java new file mode 100644 index 00000000..521f9361 --- /dev/null +++ b/pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java @@ -0,0 +1,85 @@ +package com.pathplanner.lib.path; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotNull; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class PathPlannerPathTest { + private static final double EPSILON = 1e-6; + + @Test + public void testDifferentialStartingPose() { + PathPlannerPath path = + new PathPlannerPath( + List.of( + new Translation2d(2, 1), + new Translation2d(3.12, 1), + new Translation2d(3.67, 1.00), + new Translation2d(5.19, 1.00)), + new ArrayList<>(), + new ArrayList<>(), + new ArrayList<>(), + new PathConstraints(1, 2, 3, 4), + new GoalEndState(0, Rotation2d.fromDegrees(0)), + true, + null); + + Pose2d initialPose = path.getStartingDifferentialPose(); + assertNotNull(initialPose); + assertEquals(2, initialPose.getX(), EPSILON); + assertEquals(1, initialPose.getY(), EPSILON); + assertEquals(180, initialPose.getRotation().getDegrees(), EPSILON); + } + + @Test + public void testHolomonicStartingPoseSet() { + PathPlannerPath path = + new PathPlannerPath( + List.of( + new Translation2d(2, 1), + new Translation2d(3.12, 1), + new Translation2d(3.67, 1.00), + new Translation2d(5.19, 1.00)), + new ArrayList<>(), + new ArrayList<>(), + new ArrayList<>(), + new PathConstraints(1, 2, 3, 4), + new GoalEndState(0, Rotation2d.fromDegrees(0)), + true, + new PathPlannerPath.PreviewStartingState(Rotation2d.fromDegrees(90), 0)); + Pose2d initialPose = path.getStartingHolomonicPreviewPose(); + assertNotNull(initialPose); + assertEquals(2, initialPose.getX(), EPSILON); + assertEquals(1, initialPose.getY(), EPSILON); + assertEquals(90, initialPose.getRotation().getDegrees(), EPSILON); + } + + @Test + public void testHolomonicStartingPoseNotSet() { + PathPlannerPath path = + new PathPlannerPath( + List.of( + new Translation2d(2, 1), + new Translation2d(3.12, 1), + new Translation2d(3.67, 1.00), + new Translation2d(5.19, 1.00)), + new ArrayList<>(), + new ArrayList<>(), + new ArrayList<>(), + new PathConstraints(1, 2, 3, 4), + new GoalEndState(0, Rotation2d.fromDegrees(0)), + true, + null); + Pose2d initialPose = path.getStartingHolomonicPreviewPose(); + assertNotNull(initialPose); + assertEquals(2, initialPose.getX(), EPSILON); + assertEquals(1, initialPose.getY(), EPSILON); + assertEquals(0, initialPose.getRotation().getDegrees(), EPSILON); + } +} diff --git a/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/PathPlannerPathTest.cpp b/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/PathPlannerPathTest.cpp new file mode 100644 index 00000000..a5147ac8 --- /dev/null +++ b/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/PathPlannerPathTest.cpp @@ -0,0 +1,55 @@ +#include +#include "pathplanner/lib/path/PathPlannerPath.h" + +using namespace pathplanner; + +TEST(PathPlannerPathTest, DifferentialStartingPose) { + PathPlannerPath path( + std::vector {frc::Translation2d(2_m, 1_m), frc::Translation2d(3.12_m, 1_m), frc::Translation2d(3.67_m, 1.00_m), frc::Translation2d(5.19_m, 1.00_m)}, + std::vector(), + std::vector(), + std::vector(), + PathConstraints {1_mps, 2_mps_sq, 3_rad_per_s, 4_rad_per_s_sq}, + GoalEndState(0_mps, 0_deg), + true); + + frc::Pose2d initialPose = path.getStartingDifferentialPose(); + EXPECT_EQ(2, initialPose.X()()); + EXPECT_EQ(1, initialPose.Y()()); + EXPECT_EQ(180, initialPose.Rotation().Degrees()()); +} + +TEST(PathPlannerPathTest, HolomonicStartingPoseSet) +{ + PathPlannerPath path( + std::vector {frc::Translation2d(2_m, 1_m), frc::Translation2d(3.12_m, 1_m), frc::Translation2d(3.67_m, 1.00_m), frc::Translation2d(5.19_m, 1.00_m)}, + std::vector(), + std::vector(), + std::vector(), + PathConstraints {1_mps, 2_mps_sq, 3_rad_per_s, 4_rad_per_s_sq}, + GoalEndState(0_mps, 0_deg), + true, + PathPlannerPath::PreviewStartingState {90_deg, 0_mps}); + + frc::Pose2d initialPose = path.getStartingHolomonicPreviewPose(); + EXPECT_EQ(2, initialPose.X()()); + EXPECT_EQ(1, initialPose.Y()()); + EXPECT_EQ(90, initialPose.Rotation().Degrees()()); +} + +TEST(PathPlannerPathTest, HolomonicStartingPoseNotSet) +{ + PathPlannerPath path( + std::vector {frc::Translation2d(2_m, 1_m), frc::Translation2d(3.12_m, 1_m), frc::Translation2d(3.67_m, 1.00_m), frc::Translation2d(5.19_m, 1.00_m)}, + std::vector(), + std::vector(), + std::vector(), + PathConstraints {1_mps, 2_mps_sq, 3_rad_per_s, 4_rad_per_s_sq}, + GoalEndState(0_mps, 0_deg), + true); + + frc::Pose2d initialPose = path.getStartingHolomonicPreviewPose(); + EXPECT_EQ(2, initialPose.X()()); + EXPECT_EQ(1, initialPose.Y()()); + EXPECT_EQ(0, initialPose.Rotation().Degrees()()); +} \ No newline at end of file From 6008cfdb44576133d40c15e60e5489e3d2050e32 Mon Sep 17 00:00:00 2001 From: PJ Reiniger Date: Thu, 2 Nov 2023 23:53:49 -0400 Subject: [PATCH 2/6] Do suggested changes --- .../pathplanner/lib/path/PathPlannerPath.java | 66 +++++++------------ .../pathplanner/lib/path/PathPlannerPath.cpp | 19 +++--- .../pathplanner/lib/path/PathPlannerPath.h | 22 ++----- .../lib/path/PathPlannerPathTest.java | 6 +- .../lib/path/PathPlannerPathTest.cpp | 6 +- 5 files changed, 46 insertions(+), 73 deletions(-) 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 b2234c90..2b438e91 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -19,27 +19,6 @@ /** A PathPlanner path. NOTE: This is not a trajectory and isn't directly followed. */ public class PathPlannerPath { - - /** Container for the preview starting state */ - public static class PreviewStartingState { - private Rotation2d rotation; - private double velocityMps; - - /** Default constructor */ - public PreviewStartingState() {} - - /** - * Constructor - * - * @param rotation The rotation of the starting preview poin - * @param velocityMps The velocity of the starting preview point - */ - public PreviewStartingState(Rotation2d rotation, double velocityMps) { - this.rotation = rotation; - this.velocityMps = velocityMps; - } - } - private List bezierPoints; private List rotationTargets; private List constraintZones; @@ -48,7 +27,7 @@ public PreviewStartingState(Rotation2d rotation, double velocityMps) { private GoalEndState goalEndState; private List allPoints; private boolean reversed; - private PreviewStartingState previewStartingState; + private Rotation2d previewStartingRotation; /** * Create a new path planner path @@ -59,8 +38,8 @@ public PreviewStartingState(Rotation2d rotation, double velocityMps) { * @param eventMarkers List of event markers along the path * @param globalConstraints The global constraints of the path * @param goalEndState The goal end state of the path - * @param reversed If the path is reversed. Differential mode only - * @param previewStartingState The settings used for previews in the UI + * @param reversed Should the robot follow the path reversed (differential drive only) + * @param previewStartingRotation The settings used for previews in the UI */ public PathPlannerPath( List bezierPoints, @@ -70,7 +49,7 @@ public PathPlannerPath( PathConstraints globalConstraints, GoalEndState goalEndState, boolean reversed, - PreviewStartingState previewStartingState) { + Rotation2d previewStartingRotation) { this.bezierPoints = bezierPoints; this.rotationTargets = holonomicRotations; this.constraintZones = constraintZones; @@ -79,7 +58,7 @@ public PathPlannerPath( this.goalEndState = goalEndState; this.reversed = reversed; this.allPoints = createPath(this.bezierPoints, this.rotationTargets, this.constraintZones); - this.previewStartingState = previewStartingState; + this.previewStartingRotation = previewStartingRotation; precalcValues(); } @@ -108,7 +87,7 @@ public PathPlannerPath( constraints, goalEndState, reversed, - new PreviewStartingState()); + Rotation2d.fromDegrees(0)); } /** @@ -135,7 +114,7 @@ private PathPlannerPath(PathConstraints globalConstraints, GoalEndState goalEndS this.goalEndState = goalEndState; this.reversed = false; this.allPoints = new ArrayList<>(); - this.previewStartingState = new PreviewStartingState(); + this.previewStartingRotation = Rotation2d.fromDegrees(0); } /** @@ -231,7 +210,7 @@ public void hotReload(JSONObject pathJson) { this.goalEndState = updatedPath.goalEndState; this.allPoints = updatedPath.allPoints; this.reversed = updatedPath.reversed; - this.previewStartingState = updatedPath.previewStartingState; + this.previewStartingRotation = updatedPath.previewStartingRotation; } /** @@ -287,14 +266,12 @@ private static PathPlannerPath fromJson(JSONObject pathJson) { eventMarkers.add(EventMarker.fromJson((JSONObject) markerJson)); } - PreviewStartingState previewStartingState = new PreviewStartingState(); + Rotation2d previewStartingRotation = Rotation2d.fromDegrees(0); if (pathJson.containsKey("previewStartingState")) { JSONObject previewStartingStateJson = (JSONObject) pathJson.get("previewStartingState"); if (previewStartingStateJson != null) { - previewStartingState.rotation = + previewStartingRotation = Rotation2d.fromDegrees((Double) previewStartingStateJson.get("rotation")); - previewStartingState.velocityMps = - ((Number) previewStartingStateJson.get("velocity")).doubleValue(); } } @@ -306,7 +283,7 @@ private static PathPlannerPath fromJson(JSONObject pathJson) { globalConstraints, goalEndState, reversed, - previewStartingState); + previewStartingRotation); } private static List bezierPointsFromWaypointsJson(JSONArray waypointsJson) { @@ -358,16 +335,19 @@ public Pose2d getStartingDifferentialPose() { } /** - * Get the starting pose for the holomonic path based on the preview settings + * Get the starting pose for the holomonic path based on the preview settings. + * + *

NOTE: This should only be used for the first path you are running, and only if you are not + * using an auto mode file. Using this pose to reset the robots pose between sequential paths will + * cause a loss of accuracy. * * @return Pose at the path's starting point */ - public Pose2d getStartingHolomonicPreviewPose() { - Translation2d startPos = getPoint(0).position; + public Pose2d getPreviewStartingHolonomicPose() { Rotation2d heading = - previewStartingState == null ? Rotation2d.fromDegrees(0) : previewStartingState.rotation; + previewStartingRotation == null ? Rotation2d.fromDegrees(0) : previewStartingRotation; - return new Pose2d(startPos, heading); + return new Pose2d(getPoint(0).position, heading); } /** @@ -620,7 +600,7 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds) globalConstraints, goalEndState, reversed, - previewStartingState); + previewStartingRotation); } else if ((closestPointIdx == 0 && robotNextControl == null) || (Math.abs(closestDist - startingPose.getTranslation().getDistance(getPoint(0).position)) <= 0.25 @@ -682,7 +662,7 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds) globalConstraints, goalEndState, reversed, - previewStartingState); + previewStartingRotation); } } @@ -716,7 +696,7 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds) globalConstraints, goalEndState, reversed, - previewStartingState); + previewStartingRotation); } if (bezierPoints.isEmpty()) { @@ -853,7 +833,7 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds) globalConstraints, goalEndState, reversed, - previewStartingState); + previewStartingRotation); } /** diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp index ea61d8cb..9837103d 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -14,11 +14,11 @@ PathPlannerPath::PathPlannerPath(std::vector bezierPoints, std::vector constraintZones, std::vector eventMarkers, PathConstraints globalConstraints, GoalEndState goalEndState, - bool reversed, PreviewStartingState previewStartingState) : m_bezierPoints( + bool reversed, frc::Rotation2d previewStartingRotation) : m_bezierPoints( bezierPoints), m_rotationTargets(rotationTargets), m_constraintZones( constraintZones), m_eventMarkers(eventMarkers), m_globalConstraints( - globalConstraints), m_goalEndState(goalEndState), m_reversed(reversed), m_previewStartingState( - previewStartingState) { + globalConstraints), m_goalEndState(goalEndState), m_reversed(reversed), m_previewStartingRotation( + previewStartingRotation) { m_allPoints = PathPlannerPath::createPath(m_bezierPoints, m_rotationTargets, m_constraintZones); @@ -42,6 +42,7 @@ void PathPlannerPath::hotReload(const wpi::json &json) { m_goalEndState = updatedPath.m_goalEndState; m_reversed = updatedPath.m_reversed; m_allPoints = updatedPath.m_allPoints; + m_previewStartingRotation = updatedPath.m_previewStartingRotation; } std::vector PathPlannerPath::bezierFromPoses( @@ -128,16 +129,16 @@ PathPlannerPath PathPlannerPath::fromJson(const wpi::json &json) { eventMarkers.push_back(EventMarker::fromJson(markerJson)); } - PreviewStartingState previewStartingState; + frc::Rotation2d previewStartingRotation; if (json.contains("previewStartingState")) { auto jsonStartingState = json["previewStartingState"]; - previewStartingState = { units::degree_t(jsonStartingState["rotation"]), - units::meters_per_second_t(jsonStartingState["velocity"]) }; + previewStartingRotation = frc::Rotation2d( + units::degree_t(jsonStartingState["rotation"])); } return PathPlannerPath(bezierPoints, rotationTargets, constraintZones, eventMarkers, globalConstraints, goalEndState, reversed, - previewStartingState); + previewStartingRotation); } std::vector PathPlannerPath::bezierPointsFromWaypointsJson( @@ -247,8 +248,8 @@ frc::Pose2d PathPlannerPath::getStartingDifferentialPose() { return frc::Pose2d(startPos, heading); } -frc::Pose2d PathPlannerPath::getStartingHolomonicPreviewPose() { - return frc::Pose2d(getPoint(0).position, m_previewStartingState.m_rotation); +frc::Pose2d PathPlannerPath::getPreviewStartingHolonomicPose() { + return frc::Pose2d(getPoint(0).position, m_previewStartingRotation); } void PathPlannerPath::precalcValues() { 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 dea0154e..40b49acc 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h @@ -19,16 +19,6 @@ namespace pathplanner { class PathPlannerPath { public: - - /** - * Container for the preview starting state - */ - class PreviewStartingState { - public: - frc::Rotation2d m_rotation; - units::meters_per_second_t m_velocity; - }; - /** * Create a new path planner path * @@ -45,8 +35,8 @@ class PathPlannerPath { std::vector constraintZones, std::vector eventMarkers, PathConstraints globalConstraints, GoalEndState goalEndState, - bool reversed, PreviewStartingState previewStartingState = - PreviewStartingState()); + bool reversed, frc::Rotation2d previewStartingRotation = + frc::Rotation2d()); /** * Simplified constructor to create a path with no rotation targets, constraint zones, or event @@ -93,11 +83,13 @@ class PathPlannerPath { frc::Pose2d getStartingDifferentialPose(); /** - * Get the starting pose for the holomonic path based on the preview settings + * Get the starting pose for the holomonic path based on the preview settings. + * + * NOTE: This should only be used for the first path you are running, and only if you are not using an auto mode file. Using this pose to reset the robots pose between sequential paths will cause a loss of accuracy. * * @return Pose at the path's starting point */ - frc::Pose2d getStartingHolomonicPreviewPose(); + frc::Pose2d getPreviewStartingHolonomicPose(); /** * Get the constraints for a point along the path @@ -252,6 +244,6 @@ class PathPlannerPath { GoalEndState m_goalEndState; std::vector m_allPoints; bool m_reversed; - PreviewStartingState m_previewStartingState; + frc::Rotation2d m_previewStartingRotation; }; } diff --git a/pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java b/pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java index 521f9361..f8b28348 100644 --- a/pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java +++ b/pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java @@ -52,8 +52,8 @@ public void testHolomonicStartingPoseSet() { new PathConstraints(1, 2, 3, 4), new GoalEndState(0, Rotation2d.fromDegrees(0)), true, - new PathPlannerPath.PreviewStartingState(Rotation2d.fromDegrees(90), 0)); - Pose2d initialPose = path.getStartingHolomonicPreviewPose(); + Rotation2d.fromDegrees(90)); + Pose2d initialPose = path.getPreviewStartingHolonomicPose(); assertNotNull(initialPose); assertEquals(2, initialPose.getX(), EPSILON); assertEquals(1, initialPose.getY(), EPSILON); @@ -76,7 +76,7 @@ public void testHolomonicStartingPoseNotSet() { new GoalEndState(0, Rotation2d.fromDegrees(0)), true, null); - Pose2d initialPose = path.getStartingHolomonicPreviewPose(); + Pose2d initialPose = path.getPreviewStartingHolonomicPose(); assertNotNull(initialPose); assertEquals(2, initialPose.getX(), EPSILON); assertEquals(1, initialPose.getY(), EPSILON); diff --git a/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/PathPlannerPathTest.cpp b/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/PathPlannerPathTest.cpp index a5147ac8..d3950d33 100644 --- a/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/PathPlannerPathTest.cpp +++ b/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/PathPlannerPathTest.cpp @@ -29,9 +29,9 @@ TEST(PathPlannerPathTest, HolomonicStartingPoseSet) PathConstraints {1_mps, 2_mps_sq, 3_rad_per_s, 4_rad_per_s_sq}, GoalEndState(0_mps, 0_deg), true, - PathPlannerPath::PreviewStartingState {90_deg, 0_mps}); + frc::Rotation2d {90_deg}); - frc::Pose2d initialPose = path.getStartingHolomonicPreviewPose(); + frc::Pose2d initialPose = path.getPreviewStartingHolonomicPose(); EXPECT_EQ(2, initialPose.X()()); EXPECT_EQ(1, initialPose.Y()()); EXPECT_EQ(90, initialPose.Rotation().Degrees()()); @@ -48,7 +48,7 @@ TEST(PathPlannerPathTest, HolomonicStartingPoseNotSet) GoalEndState(0_mps, 0_deg), true); - frc::Pose2d initialPose = path.getStartingHolomonicPreviewPose(); + frc::Pose2d initialPose = path.getPreviewStartingHolonomicPose(); EXPECT_EQ(2, initialPose.X()()); EXPECT_EQ(1, initialPose.Y()()); EXPECT_EQ(0, initialPose.Rotation().Degrees()()); From 3d16be0b36b231a4000c49e63ab1848d5307a006 Mon Sep 17 00:00:00 2001 From: PJ Reiniger Date: Fri, 3 Nov 2023 01:21:06 -0400 Subject: [PATCH 3/6] Update pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java Co-authored-by: Michael Jansen --- .../src/main/java/com/pathplanner/lib/path/PathPlannerPath.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2b438e91..e744c37a 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -271,7 +271,7 @@ private static PathPlannerPath fromJson(JSONObject pathJson) { JSONObject previewStartingStateJson = (JSONObject) pathJson.get("previewStartingState"); if (previewStartingStateJson != null) { previewStartingRotation = - Rotation2d.fromDegrees((Double) previewStartingStateJson.get("rotation")); + Rotation2d.fromDegrees(((Number) previewStartingStateJson.get("rotation")).doubleValue()); } } From 642bdc730282e6909f6bd50a16740ae499836978 Mon Sep 17 00:00:00 2001 From: PJ Reiniger Date: Fri, 3 Nov 2023 01:21:27 -0400 Subject: [PATCH 4/6] Update pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp Co-authored-by: Michael Jansen --- .../native/cpp/pathplanner/lib/path/PathPlannerPath.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp index 9837103d..19241572 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -130,10 +130,10 @@ PathPlannerPath PathPlannerPath::fromJson(const wpi::json &json) { } frc::Rotation2d previewStartingRotation; - if (json.contains("previewStartingState")) { - auto jsonStartingState = json["previewStartingState"]; + if (json.contains("previewStartingState") && !json.at("previewStartingState").is_null()) { + auto jsonStartingState = json.at("previewStartingState"); previewStartingRotation = frc::Rotation2d( - units::degree_t(jsonStartingState["rotation"])); + units::degree_t(jsonStartingState.at("rotation").get())); } return PathPlannerPath(bezierPoints, rotationTargets, constraintZones, From fc8631e434c77bcc14cf27aa69a57b4de2c7ebf2 Mon Sep 17 00:00:00 2001 From: PJ Reiniger Date: Fri, 3 Nov 2023 01:23:59 -0400 Subject: [PATCH 5/6] Add overload with pre-PR interface --- .../pathplanner/lib/path/PathPlannerPath.java | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) 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 e744c37a..64526d52 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -63,6 +63,28 @@ public PathPlannerPath( precalcValues(); } + /** + * Create a new path planner path + * + * @param bezierPoints List of points representing the cubic Bezier curve of the path + * @param holonomicRotations List of rotation targets along the path + * @param constraintZones List of constraint zones along the path + * @param eventMarkers List of event markers along the path + * @param globalConstraints The global constraints of the path + * @param goalEndState The goal end state of the path + * @param reversed Should the robot follow the path reversed (differential drive only) + */ + public PathPlannerPath( + List bezierPoints, + List holonomicRotations, + List constraintZones, + List eventMarkers, + PathConstraints globalConstraints, + GoalEndState goalEndState, + boolean reversed) { + this(bezierPoints, holonomicRotations, constraintZones, eventMarkers, globalConstraints, goalEndState, reversed, Rotation2d.fromDegrees(0)); + } + /** * Simplified constructor to create a path with no rotation targets, constraint zones, or event * markers. From 7f0d9c7ac1c8f606da61683bced31429803fec28 Mon Sep 17 00:00:00 2001 From: PJ Reiniger Date: Fri, 3 Nov 2023 01:32:57 -0400 Subject: [PATCH 6/6] lint --- .../pathplanner/lib/path/PathPlannerPath.java | 27 ++++++++++++------- .../pathplanner/lib/path/PathPlannerPath.cpp | 6 +++-- 2 files changed, 22 insertions(+), 11 deletions(-) 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 64526d52..2fa5b116 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -75,14 +75,22 @@ public PathPlannerPath( * @param reversed Should the robot follow the path reversed (differential drive only) */ public PathPlannerPath( - List bezierPoints, - List holonomicRotations, - List constraintZones, - List eventMarkers, - PathConstraints globalConstraints, - GoalEndState goalEndState, - boolean reversed) { - this(bezierPoints, holonomicRotations, constraintZones, eventMarkers, globalConstraints, goalEndState, reversed, Rotation2d.fromDegrees(0)); + List bezierPoints, + List holonomicRotations, + List constraintZones, + List eventMarkers, + PathConstraints globalConstraints, + GoalEndState goalEndState, + boolean reversed) { + this( + bezierPoints, + holonomicRotations, + constraintZones, + eventMarkers, + globalConstraints, + goalEndState, + reversed, + Rotation2d.fromDegrees(0)); } /** @@ -293,7 +301,8 @@ private static PathPlannerPath fromJson(JSONObject pathJson) { JSONObject previewStartingStateJson = (JSONObject) pathJson.get("previewStartingState"); if (previewStartingStateJson != null) { previewStartingRotation = - Rotation2d.fromDegrees(((Number) previewStartingStateJson.get("rotation")).doubleValue()); + Rotation2d.fromDegrees( + ((Number) previewStartingStateJson.get("rotation")).doubleValue()); } } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp index 19241572..a10f06cf 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -130,10 +130,12 @@ PathPlannerPath PathPlannerPath::fromJson(const wpi::json &json) { } frc::Rotation2d previewStartingRotation; - if (json.contains("previewStartingState") && !json.at("previewStartingState").is_null()) { + if (json.contains("previewStartingState") + && !json.at("previewStartingState").is_null()) { auto jsonStartingState = json.at("previewStartingState"); previewStartingRotation = frc::Rotation2d( - units::degree_t(jsonStartingState.at("rotation").get())); + units::degree_t( + jsonStartingState.at("rotation").get())); } return PathPlannerPath(bezierPoints, rotationTargets, constraintZones,