diff --git a/.github/workflows/format-command.yml b/.github/workflows/format-command.yml index 610814fd..4844a1b6 100644 --- a/.github/workflows/format-command.yml +++ b/.github/workflows/format-command.yml @@ -36,7 +36,7 @@ jobs: - name: Setup flutter uses: subosito/flutter-action@v2 with: - flutter-version: 3.13.9 + flutter-version: 3.16.0 cache: true cache-path: ${{ runner.tool_cache }}/flutter/linux diff --git a/.github/workflows/pathplanner-ci.yaml b/.github/workflows/pathplanner-ci.yaml index 142d0702..4d9a11e9 100644 --- a/.github/workflows/pathplanner-ci.yaml +++ b/.github/workflows/pathplanner-ci.yaml @@ -14,7 +14,7 @@ on: required: true env: - FLUTTER_VERSION: 3.13.9 + FLUTTER_VERSION: 3.16.0 jobs: formatting-analysis: @@ -85,7 +85,7 @@ jobs: include: - os: windows-2022 build-option: "windows" - artifact-path: "build/windows/runner/Release" + artifact-path: "build/windows/x64/runner/Release" artifact-name: PathPlanner-Windows - os: macos-13 build-option: "macos" @@ -145,9 +145,9 @@ jobs: - name: Copy redist if: ${{ matrix.build-option == 'windows' }} run: | - cp windows/redist/msvcp140.dll build/windows/runner/Release - cp windows/redist/vcruntime140.dll build/windows/runner/Release - cp windows/redist/vcruntime140_1.dll build/windows/runner/Release + cp windows/redist/msvcp140.dll build/windows/x64/runner/Release + cp windows/redist/vcruntime140.dll build/windows/x64/runner/Release + cp windows/redist/vcruntime140_1.dll build/windows/x64/runner/Release - name: Zip release if: ${{ matrix.build-option == 'macos' }} diff --git a/lib/path/goal_end_state.dart b/lib/path/goal_end_state.dart index e83547db..bcca819e 100644 --- a/lib/path/goal_end_state.dart +++ b/lib/path/goal_end_state.dart @@ -1,25 +1,30 @@ class GoalEndState { num velocity; num rotation; + bool rotateFast; GoalEndState({ this.velocity = 0, this.rotation = 0, + this.rotateFast = false, }); GoalEndState.fromJson(Map json) : velocity = json['velocity'] ?? 0, - rotation = json['rotation'] ?? 0; + rotation = json['rotation'] ?? 0, + rotateFast = json['rotateFast'] ?? false; Map toJson() { return { 'velocity': velocity, 'rotation': rotation, + 'rotateFast': rotateFast, }; } GoalEndState clone() { - return GoalEndState(velocity: velocity, rotation: rotation); + return GoalEndState( + velocity: velocity, rotation: rotation, rotateFast: rotateFast); } @override @@ -27,8 +32,9 @@ class GoalEndState { other is GoalEndState && other.runtimeType == runtimeType && other.velocity == velocity && - other.rotation == rotation; + other.rotation == rotation && + other.rotateFast == rotateFast; @override - int get hashCode => Object.hash(velocity, rotation); + int get hashCode => Object.hash(velocity, rotation, rotateFast); } diff --git a/lib/path/path_point.dart b/lib/path/path_point.dart index b98597c1..3b309348 100644 --- a/lib/path/path_point.dart +++ b/lib/path/path_point.dart @@ -1,17 +1,18 @@ import 'dart:math'; import 'package:pathplanner/path/path_constraints.dart'; +import 'package:pathplanner/path/rotation_target.dart'; class PathPoint { final Point position; - final num? holonomicRotation; + final RotationTarget? rotationTarget; final PathConstraints constraints; final num distanceAlongPath; num maxV = double.infinity; PathPoint({ required this.position, - required this.holonomicRotation, + required this.rotationTarget, required this.constraints, required this.distanceAlongPath, }); diff --git a/lib/path/pathplanner_path.dart b/lib/path/pathplanner_path.dart index c4697621..7f7780d2 100644 --- a/lib/path/pathplanner_path.dart +++ b/lib/path/pathplanner_path.dart @@ -342,7 +342,7 @@ class PathPlannerPath { for (int i = 0; i < waypoints.length - 1; i++) { for (double t = 0; t < 1.0; t += pathResolution) { num actualWaypointPos = i + t; - num? rotation; + RotationTarget? rotation; if (unaddedTargets.isNotEmpty) { if ((unaddedTargets[0].waypointRelativePos - actualWaypointPos) @@ -351,7 +351,7 @@ class PathPlannerPath { min(actualWaypointPos + pathResolution, waypoints.length - 1)) .abs()) { - rotation = unaddedTargets.removeAt(0).rotationDegrees; + rotation = unaddedTargets.removeAt(0); } } @@ -378,7 +378,7 @@ class PathPlannerPath { pathPoints.add( PathPoint( position: position, - holonomicRotation: rotation, + rotationTarget: rotation, constraints: constraints ?? globalConstraints, distanceAlongPath: dist, ), @@ -388,7 +388,10 @@ class PathPlannerPath { if (i == waypoints.length - 2) { pathPoints.add(PathPoint( position: waypoints[waypoints.length - 1].anchor, - holonomicRotation: goalEndState.rotation, + rotationTarget: RotationTarget( + rotationDegrees: goalEndState.rotation, + waypointRelativePos: waypoints.length - 1, + rotateFast: goalEndState.rotateFast), constraints: globalConstraints, distanceAlongPath: pathPoints.last.distanceAlongPath + (pathPoints.last.position diff --git a/lib/path/rotation_target.dart b/lib/path/rotation_target.dart index bbbff0c5..c5a56069 100644 --- a/lib/path/rotation_target.dart +++ b/lib/path/rotation_target.dart @@ -1,20 +1,24 @@ class RotationTarget { num waypointRelativePos; num rotationDegrees; + bool rotateFast; RotationTarget({ this.waypointRelativePos = 0.5, this.rotationDegrees = 0, + this.rotateFast = false, }); RotationTarget.fromJson(Map json) : waypointRelativePos = json['waypointRelativePos'] ?? 0.5, - rotationDegrees = json['rotationDegrees'] ?? 0; + rotationDegrees = json['rotationDegrees'] ?? 0, + rotateFast = json['rotateFast'] ?? false; Map toJson() { return { 'waypointRelativePos': waypointRelativePos, 'rotationDegrees': rotationDegrees, + 'rotateFast': rotateFast, }; } @@ -22,6 +26,7 @@ class RotationTarget { return RotationTarget( waypointRelativePos: waypointRelativePos, rotationDegrees: rotationDegrees, + rotateFast: rotateFast, ); } @@ -30,8 +35,10 @@ class RotationTarget { other is RotationTarget && other.runtimeType == runtimeType && other.waypointRelativePos == waypointRelativePos && - other.rotationDegrees == rotationDegrees; + other.rotationDegrees == rotationDegrees && + other.rotateFast == rotateFast; @override - int get hashCode => Object.hash(waypointRelativePos, rotationDegrees); + int get hashCode => + Object.hash(waypointRelativePos, rotationDegrees, rotateFast); } diff --git a/lib/services/simulator/trajectory_generator.dart b/lib/services/simulator/trajectory_generator.dart index 1fa526fe..2896db73 100644 --- a/lib/services/simulator/trajectory_generator.dart +++ b/lib/services/simulator/trajectory_generator.dart @@ -389,7 +389,7 @@ class Trajectory { num startingRotationRadians, { num maxModuleSpeed = 4.5, num driveBaseRadius = 0.425, - }) : states = _generateStates(path, linearVel) { + }) : states = _generateStates(path, linearVel, startingRotationRadians) { _simulateRotation(startingRotationRadians, maxModuleSpeed, driveBaseRadius); } @@ -453,7 +453,7 @@ class Trajectory { int idx = path.pathPoints.length - 1; for (int i = startingIndex; i < path.pathPoints.length - 2; i++) { - if (path.pathPoints[i].holonomicRotation != null) { + if (path.pathPoints[i].rotationTarget != null) { idx = i; break; } @@ -463,12 +463,16 @@ class Trajectory { } static List _generateStates( - PathPlannerPath path, num linearVel) { + PathPlannerPath path, num linearVel, num currentRotationRadians) { List states = []; num startVel = linearVel.abs(); + num prevRotationTargetDist = 0; + num prevRotationTargetRot = currentRotationRadians; int nextRotationTargetIdx = _getNextRotationTargetIdx(path, 0); + num distBetweenTargets = + path.pathPoints[nextRotationTargetIdx].distanceAlongPath; // Initial pass. Creates all states and handles linear acceleration for (int i = 0; i < path.pathPoints.length; i++) { @@ -478,11 +482,37 @@ class Trajectory { state.constraints = constraints; if (i > nextRotationTargetIdx) { + prevRotationTargetDist = + path.pathPoints[nextRotationTargetIdx].distanceAlongPath; + prevRotationTargetRot = GeometryUtil.toRadians(path + .pathPoints[nextRotationTargetIdx].rotationTarget!.rotationDegrees); nextRotationTargetIdx = _getNextRotationTargetIdx(path, i); + distBetweenTargets = + path.pathPoints[nextRotationTargetIdx].distanceAlongPath - + prevRotationTargetDist; } - state.holonomicRotationRadians = GeometryUtil.toRadians( - path.pathPoints[nextRotationTargetIdx].holonomicRotation!); + RotationTarget nextTarget = + path.pathPoints[nextRotationTargetIdx].rotationTarget!; + + if (nextTarget.rotateFast) { + state.holonomicRotationRadians = + GeometryUtil.toRadians(nextTarget.rotationDegrees); + } else { + double t = + (path.pathPoints[i].distanceAlongPath - prevRotationTargetDist) / + distBetweenTargets; + t = min(max(0.0, t), 1.0); + if (!t.isFinite) { + t = 0; + } + + state.holonomicRotationRadians = GeometryUtil.rotationLerp( + prevRotationTargetRot, + GeometryUtil.toRadians(nextTarget.rotationDegrees), + t, + pi); + } state.position = path.pathPoints[i].position; diff --git a/lib/widgets/editor/tree_widgets/goal_end_state_tree.dart b/lib/widgets/editor/tree_widgets/goal_end_state_tree.dart index 59d76bc9..de65cc92 100644 --- a/lib/widgets/editor/tree_widgets/goal_end_state_tree.dart +++ b/lib/widgets/editor/tree_widgets/goal_end_state_tree.dart @@ -66,6 +66,24 @@ class GoalEndStateTree extends StatelessWidget { ], ), ), + if (holonomicMode) const SizedBox(height: 12), + if (holonomicMode) + Row( + children: [ + Checkbox( + value: path.goalEndState.rotateFast, + onChanged: (value) { + _addChange( + () => path.goalEndState.rotateFast = value ?? false); + }, + ), + const SizedBox(width: 4), + const Text( + 'Rotate as Fast as Possible', + style: TextStyle(fontSize: 18), + ), + ], + ), ], ); } diff --git a/lib/widgets/editor/tree_widgets/rotation_targets_tree.dart b/lib/widgets/editor/tree_widgets/rotation_targets_tree.dart index 1d3b2167..37bafa5e 100644 --- a/lib/widgets/editor/tree_widgets/rotation_targets_tree.dart +++ b/lib/widgets/editor/tree_widgets/rotation_targets_tree.dart @@ -194,6 +194,32 @@ class _RotationTargetsTreeState extends State { ), ), const SizedBox(height: 12), + Row( + children: [ + Checkbox( + value: rotations[targetIdx].rotateFast, + onChanged: (value) { + widget.undoStack.add(Change( + rotations[targetIdx].clone(), + () { + rotations[targetIdx].rotateFast = value ?? false; + widget.onPathChanged?.call(); + }, + (oldValue) { + rotations[targetIdx].rotateFast = oldValue.rotateFast; + widget.onPathChanged?.call(); + }, + )); + }, + ), + const SizedBox(width: 4), + const Text( + 'Reach as Fast as Possible', + style: TextStyle(fontSize: 18), + ), + ], + ), + const SizedBox(height: 12), Slider( value: rotations[targetIdx].waypointRelativePos.toDouble(), min: 0.0, diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java index 08c99e76..1c2f4b81 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java @@ -69,7 +69,8 @@ public void initialize() { >= 0.25)) { replanPath(currentPose, currentSpeeds); } else { - generatedTrajectory = new PathPlannerTrajectory(path, currentSpeeds); + generatedTrajectory = + new PathPlannerTrajectory(path, currentSpeeds, currentPose.getRotation()); PathPlannerLogging.logActivePath(path); PPLibTelemetry.setCurrentPath(path); } @@ -146,7 +147,8 @@ public void end(boolean interrupted) { private void replanPath(Pose2d currentPose, ChassisSpeeds currentSpeeds) { PathPlannerPath replanned = path.replan(currentPose, currentSpeeds); - generatedTrajectory = new PathPlannerTrajectory(replanned, currentSpeeds); + generatedTrajectory = + new PathPlannerTrajectory(replanned, currentSpeeds, currentPose.getRotation()); PathPlannerLogging.logActivePath(replanned); PPLibTelemetry.setCurrentPath(replanned); } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java index d670e19e..ffadf8b8 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java @@ -66,8 +66,8 @@ public PathfindingCommand( Rotation2d targetRotation = new Rotation2d(); for (PathPoint p : targetPath.getAllPathPoints()) { - if (p.holonomicRotation != null) { - targetRotation = p.holonomicRotation; + if (p.rotationTarget != null) { + targetRotation = p.rotationTarget.getTarget(); break; } } @@ -76,7 +76,7 @@ public PathfindingCommand( this.targetPose = new Pose2d(this.targetPath.getPoint(0).position, targetRotation); this.goalEndState = new GoalEndState( - this.targetPath.getGlobalConstraints().getMaxVelocityMps(), targetRotation); + this.targetPath.getGlobalConstraints().getMaxVelocityMps(), targetRotation, true); this.constraints = constraints; this.controller = controller; this.poseSupplier = poseSupplier; @@ -119,7 +119,7 @@ public PathfindingCommand( this.targetPath = null; this.targetPose = targetPose; - this.goalEndState = new GoalEndState(goalEndVel, targetPose.getRotation()); + this.goalEndState = new GoalEndState(goalEndVel, targetPose.getRotation(), true); this.constraints = constraints; this.controller = controller; this.poseSupplier = poseSupplier; @@ -178,7 +178,8 @@ public void execute() { if (!replanningConfig.enableInitialReplanning || (currentPose.getTranslation().getDistance(currentPath.getPoint(0).position) <= 0.25 && onHeading)) { - currentTrajectory = new PathPlannerTrajectory(currentPath, currentSpeeds); + currentTrajectory = + new PathPlannerTrajectory(currentPath, currentSpeeds, currentPose.getRotation()); // Find the two closest states in front of and behind robot int closestState1Idx = 0; @@ -217,7 +218,8 @@ public void execute() { PPLibTelemetry.setCurrentPath(currentPath); } else { PathPlannerPath replanned = currentPath.replan(currentPose, currentSpeeds); - currentTrajectory = new PathPlannerTrajectory(replanned, currentSpeeds); + currentTrajectory = + new PathPlannerTrajectory(replanned, currentSpeeds, currentPose.getRotation()); timeOffset = 0; @@ -316,7 +318,8 @@ public void end(boolean interrupted) { private void replanPath(Pose2d currentPose, ChassisSpeeds currentSpeeds) { PathPlannerPath replanned = currentPath.replan(currentPose, currentSpeeds); - currentTrajectory = new PathPlannerTrajectory(replanned, currentSpeeds); + currentTrajectory = + new PathPlannerTrajectory(replanned, currentSpeeds, currentPose.getRotation()); PathPlannerLogging.logActivePath(replanned); PPLibTelemetry.setCurrentPath(replanned); } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/GoalEndState.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/GoalEndState.java index ba3ceae6..ede5e691 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/GoalEndState.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/GoalEndState.java @@ -8,16 +8,19 @@ public class GoalEndState { private final double velocity; private final Rotation2d rotation; + private final boolean rotateFast; /** * Create a new goal end state * * @param velocity The goal end velocity (M/S) * @param rotation The goal rotation + * @param rotateFast Should the robot reach the rotation as fast as possible */ - public GoalEndState(double velocity, Rotation2d rotation) { + public GoalEndState(double velocity, Rotation2d rotation, boolean rotateFast) { this.velocity = velocity; this.rotation = rotation; + this.rotateFast = rotateFast; } /** @@ -29,7 +32,11 @@ public GoalEndState(double velocity, Rotation2d rotation) { static GoalEndState fromJson(JSONObject endStateJson) { double vel = ((Number) endStateJson.get("velocity")).doubleValue(); double deg = ((Number) endStateJson.get("rotation")).doubleValue(); - return new GoalEndState(vel, Rotation2d.fromDegrees(deg)); + boolean rotateFast = false; + if (endStateJson.get("rotateFast") != null) { + rotateFast = (boolean) endStateJson.get("rotation"); + } + return new GoalEndState(vel, Rotation2d.fromDegrees(deg), rotateFast); } /** @@ -50,21 +57,39 @@ public Rotation2d getRotation() { return rotation; } + /** + * Get if the robot should reach the rotation as fast as possible + * + * @return True if the robot should reach the rotation as fast as possible + */ + public boolean shouldRotateFast() { + return rotateFast; + } + @Override public boolean equals(Object o) { if (this == o) return true; if (o == null || getClass() != o.getClass()) return false; GoalEndState that = (GoalEndState) o; - return Math.abs(that.velocity - velocity) < 1E-3 && Objects.equals(rotation, that.rotation); + return Math.abs(that.velocity - velocity) < 1E-3 + && Objects.equals(rotation, that.rotation) + && rotateFast == that.rotateFast; } @Override public int hashCode() { - return Objects.hash(velocity, rotation); + return Objects.hash(velocity, rotation, rotateFast); } @Override public String toString() { - return "GoalEndState{" + "velocity=" + velocity + ", rotation=" + rotation + '}'; + return "GoalEndState{" + + "velocity=" + + velocity + + ", rotation=" + + rotation + + ", rotateFast=" + + rotateFast + + "}"; } } 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 d823082e..1889ac2c 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -478,7 +478,8 @@ private void precalcValues() { m.markerPos = allPoints.get(pointIndex).position; } - allPoints.get(allPoints.size() - 1).holonomicRotation = goalEndState.getRotation(); + allPoints.get(allPoints.size() - 1).rotationTarget = + new RotationTarget(-1, goalEndState.getRotation(), goalEndState.shouldRotateFast()); allPoints.get(allPoints.size() - 1).maxV = goalEndState.getVelocity(); } } @@ -684,7 +685,12 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds) return new PathPlannerPath( replannedBezier, rotationTargets.stream() - .map((target) -> new RotationTarget(target.getPosition() + 1, target.getTarget())) + .map( + (target) -> + new RotationTarget( + target.getPosition() + 1, + target.getTarget(), + target.shouldRotateFast())) .collect(Collectors.toList()), constraintZones.stream() .map( @@ -820,10 +826,13 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds) for (RotationTarget t : rotationTargets) { if (t.getPosition() >= nextWaypointIdx) { - mappedTargets.add(new RotationTarget(t.getPosition() - nextWaypointIdx + 2, t.getTarget())); + mappedTargets.add( + new RotationTarget( + t.getPosition() - nextWaypointIdx + 2, t.getTarget(), t.shouldRotateFast())); } else if (t.getPosition() >= nextWaypointIdx - 1) { double pct = t.getPosition() - (nextWaypointIdx - 1); - mappedTargets.add(new RotationTarget(mapPct(pct, segment1Pct), t.getTarget())); + mappedTargets.add( + new RotationTarget(mapPct(pct, segment1Pct), t.getTarget(), t.shouldRotateFast())); } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java index b15880ae..f87037a0 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java @@ -18,16 +18,18 @@ public class PathPlannerTrajectory { * * @param path {@link com.pathplanner.lib.path.PathPlannerPath} to generate the trajectory for * @param startingSpeeds Starting speeds of the robot when starting the trajectory + * @param startingRotation Starting rotation of the robot when starting the trajectory */ - public PathPlannerTrajectory(PathPlannerPath path, ChassisSpeeds startingSpeeds) { - this.states = generateStates(path, startingSpeeds); + public PathPlannerTrajectory( + PathPlannerPath path, ChassisSpeeds startingSpeeds, Rotation2d startingRotation) { + this.states = generateStates(path, startingSpeeds, startingRotation); } private static int getNextRotationTargetIdx(PathPlannerPath path, int startingIndex) { int idx = path.numPoints() - 1; for (int i = startingIndex; i < path.numPoints() - 2; i++) { - if (path.getPoint(i).holonomicRotation != null) { + if (path.getPoint(i).rotationTarget != null) { idx = i; break; } @@ -36,13 +38,17 @@ private static int getNextRotationTargetIdx(PathPlannerPath path, int startingIn return idx; } - private static List generateStates(PathPlannerPath path, ChassisSpeeds startingSpeeds) { + private static List generateStates( + PathPlannerPath path, ChassisSpeeds startingSpeeds, Rotation2d startingRotation) { List states = new ArrayList<>(); double startVel = Math.hypot(startingSpeeds.vxMetersPerSecond, startingSpeeds.vyMetersPerSecond); + double prevRotationTargetDist = 0.0; + Rotation2d prevRotationTargetRot = startingRotation; int nextRotationTargetIdx = getNextRotationTargetIdx(path, 0); + double distanceBetweenTargets = path.getPoint(nextRotationTargetIdx).distanceAlongPath; // Initial pass. Creates all states and handles linear acceleration for (int i = 0; i < path.numPoints(); i++) { @@ -52,10 +58,28 @@ private static List generateStates(PathPlannerPath path, ChassisSpeeds st state.constraints = constraints; if (i > nextRotationTargetIdx) { + prevRotationTargetDist = path.getPoint(nextRotationTargetIdx).distanceAlongPath; + prevRotationTargetRot = path.getPoint(nextRotationTargetIdx).rotationTarget.getTarget(); nextRotationTargetIdx = getNextRotationTargetIdx(path, i); + distanceBetweenTargets = + path.getPoint(nextRotationTargetIdx).distanceAlongPath - prevRotationTargetDist; } - state.targetHolonomicRotation = path.getPoint(nextRotationTargetIdx).holonomicRotation; + RotationTarget nextTarget = path.getPoint(nextRotationTargetIdx).rotationTarget; + + if (nextTarget.shouldRotateFast()) { + state.targetHolonomicRotation = nextTarget.getTarget(); + } else { + double t = + (path.getPoint(i).distanceAlongPath - prevRotationTargetDist) / distanceBetweenTargets; + t = Math.min(Math.max(0.0, t), 1.0); + if (!Double.isFinite(t)) { + t = 0.0; + } + + state.targetHolonomicRotation = + prevRotationTargetRot.interpolate(nextTarget.getTarget(), t); + } state.positionMeters = path.getPoint(i).position; double curveRadius = path.getPoint(i).curveRadius; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPoint.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPoint.java index 22975a41..7e9a0cf6 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPoint.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPoint.java @@ -1,6 +1,5 @@ package com.pathplanner.lib.path; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import java.util.Objects; @@ -16,7 +15,7 @@ public class PathPoint { /** The max velocity at this point */ public double maxV = Double.POSITIVE_INFINITY; /** The target rotation at this point */ - public Rotation2d holonomicRotation = null; + public RotationTarget rotationTarget = null; /** The constraints applied to this point */ public PathConstraints constraints = null; @@ -24,13 +23,13 @@ public class PathPoint { * Create a path point * * @param position Position of the point - * @param holonomicRotation Rotation target at this point + * @param rotationTarget Rotation target at this point * @param constraints The constraints at this point */ public PathPoint( - Translation2d position, Rotation2d holonomicRotation, PathConstraints constraints) { + Translation2d position, RotationTarget rotationTarget, PathConstraints constraints) { this.position = position; - this.holonomicRotation = holonomicRotation; + this.rotationTarget = rotationTarget; this.constraints = constraints; } @@ -38,11 +37,20 @@ public PathPoint( * Create a path point * * @param position Position of the point - * @param holonomicRotation Rotation target at this point + * @param rotationTarget Rotation target at this point */ - public PathPoint(Translation2d position, Rotation2d holonomicRotation) { + public PathPoint(Translation2d position, RotationTarget rotationTarget) { + this.position = position; + this.rotationTarget = rotationTarget; + } + + /** + * Create a path point + * + * @param position Position of the point + */ + public PathPoint(Translation2d position) { this.position = position; - this.holonomicRotation = holonomicRotation; } @Override @@ -53,12 +61,12 @@ public boolean equals(Object o) { return Math.abs(pathPoint.distanceAlongPath - distanceAlongPath) < 1E-3 && Math.abs(pathPoint.maxV - maxV) < 1E-3 && Objects.equals(position, pathPoint.position) - && Objects.equals(holonomicRotation, pathPoint.holonomicRotation) + && Objects.equals(rotationTarget, pathPoint.rotationTarget) && Objects.equals(constraints, pathPoint.constraints); } @Override public int hashCode() { - return Objects.hash(position, distanceAlongPath, maxV, holonomicRotation, constraints); + return Objects.hash(position, distanceAlongPath, maxV, rotationTarget, constraints); } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathSegment.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathSegment.java index 29bb977a..7949931d 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathSegment.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathSegment.java @@ -1,7 +1,6 @@ package com.pathplanner.lib.path; import com.pathplanner.lib.util.GeometryUtil; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import java.util.ArrayList; import java.util.List; @@ -36,13 +35,13 @@ public PathSegment( this.segmentPoints = new ArrayList<>(); for (double t = 0.0; t < 1.0; t += RESOLUTION) { - Rotation2d holonomicRotation = null; + RotationTarget holonomicRotation = null; if (!targetHolonomicRotations.isEmpty()) { if (Math.abs(targetHolonomicRotations.get(0).getPosition() - t) <= Math.abs( targetHolonomicRotations.get(0).getPosition() - Math.min(t + RESOLUTION, 1.0))) { - holonomicRotation = targetHolonomicRotations.remove(0).getTarget(); + holonomicRotation = targetHolonomicRotations.remove(0); } } @@ -61,10 +60,8 @@ public PathSegment( } if (endSegment) { - Rotation2d holonomicRotation = - targetHolonomicRotations.isEmpty() - ? null - : targetHolonomicRotations.remove(0).getTarget(); + RotationTarget holonomicRotation = + targetHolonomicRotations.isEmpty() ? null : targetHolonomicRotations.remove(0); this.segmentPoints.add( new PathPoint(GeometryUtil.cubicLerp(p1, p2, p3, p4, 1.0), holonomicRotation)); } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/RotationTarget.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/RotationTarget.java index 2e5ee4fd..eca626a1 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/RotationTarget.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/RotationTarget.java @@ -8,16 +8,19 @@ public class RotationTarget { private final double waypointRelativePosition; private final Rotation2d target; + private final boolean rotateFast; /** * Create a new rotation target * * @param waypointRelativePosition Waypoint relative position of this target * @param target Target rotation + * @param rotateFast Should the robot reach the rotation as fast as possible */ - public RotationTarget(double waypointRelativePosition, Rotation2d target) { + public RotationTarget(double waypointRelativePosition, Rotation2d target, boolean rotateFast) { this.waypointRelativePosition = waypointRelativePosition; this.target = target; + this.rotateFast = rotateFast; } /** @@ -29,7 +32,11 @@ public RotationTarget(double waypointRelativePosition, Rotation2d target) { static RotationTarget fromJson(JSONObject targetJson) { double pos = ((Number) targetJson.get("waypointRelativePos")).doubleValue(); double deg = ((Number) targetJson.get("rotationDegrees")).doubleValue(); - return new RotationTarget(pos, Rotation2d.fromDegrees(deg)); + boolean rotateFast = false; + if (targetJson.get("rotateFast") != null) { + rotateFast = (boolean) targetJson.get("rotation"); + } + return new RotationTarget(pos, Rotation2d.fromDegrees(deg), rotateFast); } /** @@ -50,6 +57,15 @@ public Rotation2d getTarget() { return target; } + /** + * Get if the robot should reach the rotation as fast as possible + * + * @return True if the robot should reach the rotation as fast as possible + */ + public boolean shouldRotateFast() { + return rotateFast; + } + /** * Transform the position of this target for a given segment number. * @@ -59,7 +75,7 @@ public Rotation2d getTarget() { * @return The transformed target */ public RotationTarget forSegmentIndex(int segmentIndex) { - return new RotationTarget(waypointRelativePosition - segmentIndex, target); + return new RotationTarget(waypointRelativePosition - segmentIndex, target, rotateFast); } @Override @@ -68,7 +84,8 @@ public boolean equals(Object o) { if (o == null || getClass() != o.getClass()) return false; RotationTarget that = (RotationTarget) o; return Math.abs(that.waypointRelativePosition - waypointRelativePosition) < 1E-3 - && Objects.equals(target, that.target); + && Objects.equals(target, that.target) + && rotateFast == that.rotateFast; } @Override @@ -83,6 +100,8 @@ public String toString() { + waypointRelativePosition + ", target=" + target - + '}'; + + ", rotateFast=" + + rotateFast + + "}"; } } 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 5eb8932c..c53c3178 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/LocalADStar.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/LocalADStar.java @@ -338,7 +338,7 @@ private List extractPath( Translation2d realGoalPos, Set obstacles) { if (sGoal.equals(sStart)) { - return List.of(new PathPoint(realGoalPos, null)); + return new ArrayList<>(); } List path = new ArrayList<>(); @@ -445,10 +445,10 @@ private List extractPath( } for (double t = 0.0; t < 1.0; t += resolution) { - pathPoints.add(new PathPoint(GeometryUtil.cubicLerp(p1, p2, p3, p4, t), null)); + pathPoints.add(new PathPoint(GeometryUtil.cubicLerp(p1, p2, p3, p4, t))); } } - pathPoints.add(new PathPoint(bezierPoints.get(bezierPoints.size() - 1), null)); + pathPoints.add(new PathPoint(bezierPoints.get(bezierPoints.size() - 1))); return pathPoints; } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/RemoteADStar.java b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/RemoteADStar.java index 6e4b28c4..6744854a 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/RemoteADStar.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/RemoteADStar.java @@ -18,6 +18,7 @@ import java.util.concurrent.atomic.AtomicBoolean; import java.util.concurrent.atomic.AtomicReference; +/** Implementation of ADStar running on a coprocessor */ public class RemoteADStar implements Pathfinder { private final StringPublisher navGridJsonPub; private final DoubleArrayPublisher startPosPub; @@ -30,6 +31,7 @@ public class RemoteADStar implements Pathfinder { new AtomicReference<>(new ArrayList<>()); private final AtomicBoolean newPathAvailable = new AtomicBoolean(false); + /** Create a RemoteADStar object. This will use NT4 to communicate with the coprocessor. */ public RemoteADStar() { var nt = NetworkTableInstance.getDefault(); diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp index 981f0943..23014b5a 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp @@ -27,7 +27,8 @@ void FollowPathCommand::Initialize() { >= 0.25_mps)) { replanPath(currentPose, currentSpeeds); } else { - m_generatedTrajectory = PathPlannerTrajectory(m_path, currentSpeeds); + m_generatedTrajectory = PathPlannerTrajectory(m_path, currentSpeeds, + currentPose.Rotation()); PathPlannerLogging::logActivePath (m_path); PPLibTelemetry::setCurrentPath(m_path); } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp index bee88438..3b4adb41 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp @@ -13,7 +13,7 @@ PathfindingCommand::PathfindingCommand( std::unique_ptr controller, units::meter_t rotationDelayDistance, ReplanningConfig replanningConfig, frc2::Requirements requirements) : m_targetPath(targetPath), m_targetPose(), m_goalEndState( - 0_mps, frc::Rotation2d()), m_constraints(constraints), m_poseSupplier( + 0_mps, frc::Rotation2d(), true), m_constraints(constraints), m_poseSupplier( poseSupplier), m_speedsSupplier(speedsSupplier), m_output(output), m_controller( std::move(controller)), m_rotationDelayDistance(rotationDelayDistance), m_replanningConfig( replanningConfig) { @@ -23,8 +23,8 @@ PathfindingCommand::PathfindingCommand( frc::Rotation2d targetRotation; for (PathPoint p : m_targetPath->getAllPathPoints()) { - if (p.holonomicRotation.has_value()) { - targetRotation = p.holonomicRotation.value(); + if (p.rotationTarget.has_value()) { + targetRotation = p.rotationTarget.value().getTarget(); break; } } @@ -33,7 +33,7 @@ PathfindingCommand::PathfindingCommand( targetRotation); m_goalEndState = GoalEndState( m_targetPath->getGlobalConstraints().getMaxVelocity(), - targetRotation); + targetRotation, true); } PathfindingCommand::PathfindingCommand(frc::Pose2d targetPose, @@ -44,7 +44,7 @@ PathfindingCommand::PathfindingCommand(frc::Pose2d targetPose, std::unique_ptr controller, units::meter_t rotationDelayDistance, ReplanningConfig replanningConfig, frc2::Requirements requirements) : m_targetPath(), m_targetPose( - targetPose), m_goalEndState(goalEndVel, targetPose.Rotation()), m_constraints( + targetPose), m_goalEndState(goalEndVel, targetPose.Rotation(), true), m_constraints( constraints), m_poseSupplier(poseSupplier), m_speedsSupplier( speedsSupplier), m_output(output), m_controller(std::move(controller)), m_rotationDelayDistance( rotationDelayDistance), m_replanningConfig(replanningConfig) { @@ -105,7 +105,7 @@ void PathfindingCommand::Execute() { m_currentPath->getPoint(0).position) <= 0.25_m && onHeading)) { m_currentTrajectory = PathPlannerTrajectory(m_currentPath, - currentSpeeds); + currentSpeeds, currentPose.Rotation()); // Find the two closest states in front of and behind robot size_t closestState1Idx = 0; @@ -146,7 +146,7 @@ void PathfindingCommand::Execute() { auto replanned = m_currentPath->replan(currentPose, currentSpeeds); m_currentTrajectory = PathPlannerTrajectory(replanned, - currentSpeeds); + currentSpeeds, currentPose.Rotation()); m_timeOffset = 0_s; diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/GoalEndState.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/GoalEndState.cpp index ca6ff37f..772858b4 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/GoalEndState.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/GoalEndState.cpp @@ -7,6 +7,10 @@ using namespace pathplanner; GoalEndState GoalEndState::fromJson(const wpi::json &json) { auto vel = units::meters_per_second_t(json.at("velocity").get()); auto rotationDeg = units::degree_t(json.at("rotation").get()); + bool rotateFast = false; + if (json.contains("rotateFast")) { + rotateFast = json.at("rotateFast").get(); + } - return GoalEndState(vel, frc::Rotation2d(rotationDeg)); + return GoalEndState(vel, frc::Rotation2d(rotationDeg), rotateFast); } 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 006defa6..39450f45 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -305,8 +305,9 @@ void PathPlannerPath::precalcValues() { m.setMarkerPosition(m_allPoints[pointIndex].position); } - m_allPoints[m_allPoints.size() - 1].holonomicRotation = - m_goalEndState.getRotation(); + m_allPoints[m_allPoints.size() - 1].rotationTarget = RotationTarget(-1, + m_goalEndState.getRotation(), + m_goalEndState.shouldRotateFast()); m_allPoints[m_allPoints.size() - 1].maxV = m_goalEndState.getVelocity(); } } @@ -436,7 +437,7 @@ std::shared_ptr PathPlannerPath::replan( std::back_inserter(targets), [](RotationTarget target) { return RotationTarget(target.getPosition() + 1, - target.getTarget()); + target.getTarget(), target.shouldRotateFast()); }); std::vector < ConstraintsZone > zones; std::transform(m_constraintZones.begin(), m_constraintZones.end(), @@ -573,10 +574,11 @@ std::shared_ptr PathPlannerPath::replan( for (RotationTarget t : m_rotationTargets) { if (t.getPosition() >= nextWaypointIdx) { mappedTargets.emplace_back(t.getPosition() - nextWaypointIdx + 2, - t.getTarget()); + t.getTarget(), t.shouldRotateFast()); } else if (t.getPosition() >= nextWaypointIdx - 1) { double pct = t.getPosition() - (nextWaypointIdx - 1); - mappedTargets.emplace_back(mapPct(pct, segment1Pct), t.getTarget()); + mappedTargets.emplace_back(mapPct(pct, segment1Pct), t.getTarget(), + t.shouldRotateFast()); } } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp index f242cd0b..07890d4a 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerTrajectory.cpp @@ -36,7 +36,7 @@ size_t PathPlannerTrajectory::getNextRotationTargetIdx( size_t idx = path->numPoints() - 1; for (size_t i = startingIndex; i < path->numPoints() - 2; i++) { - if (path->getPoint(i).holonomicRotation) { + if (path->getPoint(i).rotationTarget) { idx = i; break; } @@ -47,13 +47,18 @@ size_t PathPlannerTrajectory::getNextRotationTargetIdx( std::vector PathPlannerTrajectory::generateStates( std::shared_ptr path, - const frc::ChassisSpeeds &startingSpeeds) { + const frc::ChassisSpeeds &startingSpeeds, + const frc::Rotation2d &startingRotation) { std::vector < State > states; units::meters_per_second_t startVel = units::math::hypot(startingSpeeds.vx, startingSpeeds.vy); + units::meter_t prevRotationTargetDist = 0.0_m; + frc::Rotation2d prevRotationTargetRot = startingRotation; size_t nextRotationTargetIdx = getNextRotationTargetIdx(path, 0); + units::meter_t distanceBetweenTargets = path->getPoint( + nextRotationTargetIdx).distanceAlongPath; // Initial pass. Creates all states and handles linear acceleration for (size_t i = 0; i < path->numPoints(); i++) { @@ -63,11 +68,32 @@ std::vector PathPlannerTrajectory::generateStates( state.constraints = constraints; if (i > nextRotationTargetIdx) { + prevRotationTargetDist = + path->getPoint(nextRotationTargetIdx).distanceAlongPath; + prevRotationTargetRot = + path->getPoint(nextRotationTargetIdx).rotationTarget.value().getTarget(); nextRotationTargetIdx = getNextRotationTargetIdx(path, i); + distanceBetweenTargets = + path->getPoint(nextRotationTargetIdx).distanceAlongPath + - prevRotationTargetDist; } - state.targetHolonomicRotation = - path->getPoint(nextRotationTargetIdx).holonomicRotation.value(); + RotationTarget nextTarget = + path->getPoint(nextRotationTargetIdx).rotationTarget.value(); + + if (nextTarget.shouldRotateFast()) { + state.targetHolonomicRotation = nextTarget.getTarget(); + } else { + double t = ((path->getPoint(i).distanceAlongPath + - prevRotationTargetDist) / distanceBetweenTargets)(); + t = std::min(std::max(0.0, t), 1.0); + if (!std::isfinite(t)) { + t = 0.0; + } + + state.targetHolonomicRotation = (prevRotationTargetRot + + (nextTarget.getTarget() - prevRotationTargetRot)) * t; + } state.position = path->getPoint(i).position; units::meter_t curveRadius = path->getPoint(i).curveRadius; diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathSegment.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathSegment.cpp index f1d76822..c29a66e5 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathSegment.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathSegment.cpp @@ -9,7 +9,7 @@ PathSegment::PathSegment(frc::Translation2d p1, frc::Translation2d p2, std::vector constraintZones, bool endSegment) : m_segmentPoints() { for (double t = 0.0; t < 1.0; t += PathSegment::RESOLUTION) { - std::optional < frc::Rotation2d > holonomicRotation = std::nullopt; + std::optional < RotationTarget > holonomicRotation = std::nullopt; if (!targetHolonomicRotations.empty()) { if (std::abs(targetHolonomicRotations[0].getPosition() - t) @@ -17,7 +17,7 @@ PathSegment::PathSegment(frc::Translation2d p1, frc::Translation2d p2, targetHolonomicRotations[0].getPosition() - std::min(t + PathSegment::RESOLUTION, 1.0))) { - holonomicRotation = targetHolonomicRotations[0].getTarget(); + holonomicRotation = targetHolonomicRotations[0]; targetHolonomicRotations.erase( targetHolonomicRotations.begin()); } @@ -36,6 +36,17 @@ PathSegment::PathSegment(frc::Translation2d p1, frc::Translation2d p2, holonomicRotation, std::nullopt)); } } + + if (endSegment) { + std::optional < RotationTarget > holonomicRotation = std::nullopt; + if (!targetHolonomicRotations.empty()) { + holonomicRotation = targetHolonomicRotations[0]; + } + + m_segmentPoints.push_back( + PathPoint(GeometryUtil::cubicLerp(p1, p2, p3, p4, 1.0), + holonomicRotation, std::nullopt)); + } } std::optional PathSegment::findConstraintsZone( diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/RotationTarget.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/RotationTarget.cpp index 57299301..f31dcf8f 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/RotationTarget.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/RotationTarget.cpp @@ -6,6 +6,11 @@ using namespace pathplanner; RotationTarget RotationTarget::fromJson(const wpi::json &json) { double pos = json.at("waypointRelativePos").get(); auto targetDeg = units::degree_t(json.at("rotationDegrees").get()); + bool rotateFast = false; - return RotationTarget(pos, frc::Rotation2d(targetDeg)); + if (json.contains("rotateFast")) { + rotateFast = json.at("rotateFast").get(); + } + + return RotationTarget(pos, frc::Rotation2d(targetDeg), rotateFast); } 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 f3e8c353..c09c4ddc 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/LocalADStar.cpp @@ -297,8 +297,7 @@ std::vector LocalADStar::extractPath(const GridPosition &sStart, const frc::Translation2d &realGoalPos, const std::unordered_set &obstacles) { if (sGoal == sStart) { - return std::vector { PathPoint(realGoalPos, std::nullopt, - std::nullopt) }; + return std::vector(); } std::vector < GridPosition > path; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h index 2687bb60..7ea16808 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h @@ -63,7 +63,8 @@ class FollowPathCommand: public frc2::CommandHelperreplan(currentPose, currentSpeeds); - m_generatedTrajectory = PathPlannerTrajectory(replanned, currentSpeeds); + m_generatedTrajectory = PathPlannerTrajectory(replanned, currentSpeeds, + currentPose.Rotation()); PathPlannerLogging::logActivePath(replanned); PPLibTelemetry::setCurrentPath(replanned); } diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h index 18eb02a8..78064c55 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h @@ -100,7 +100,8 @@ class PathfindingCommand: public frc2::CommandHelperreplan(currentPose, currentSpeeds); - m_currentTrajectory = PathPlannerTrajectory(replanned, currentSpeeds); + m_currentTrajectory = PathPlannerTrajectory(replanned, currentSpeeds, + currentPose.Rotation()); PathPlannerLogging::logActivePath(replanned); PPLibTelemetry::setCurrentPath(replanned); } diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/path/GoalEndState.h b/pathplannerlib/src/main/native/include/pathplanner/lib/path/GoalEndState.h index 33764622..d53d9991 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/path/GoalEndState.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/path/GoalEndState.h @@ -13,10 +13,11 @@ class GoalEndState { * * @param velocity The goal end velocity (M/S) * @param rotation The goal rotation + * @param rotateFast Should the robot reach the rotation as fast as possible */ constexpr GoalEndState(units::meters_per_second_t velocity, - frc::Rotation2d rotation) : m_velocity(velocity), m_rotation( - rotation) { + frc::Rotation2d rotation, bool rotateFast) : m_velocity(velocity), m_rotation( + rotation), m_rotateFast(rotateFast) { } /** @@ -45,13 +46,24 @@ class GoalEndState { return m_rotation; } + /** + * Get if the robot should reach the rotation as fast as possible + * + * @return True if the robot should reach the rotation as fast as possible + */ + constexpr bool shouldRotateFast() const { + return m_rotateFast; + } + inline bool operator==(const GoalEndState &other) const { return std::abs(m_velocity() - other.m_velocity()) < 1E-9 - && m_rotation == other.m_rotation; + && m_rotation == other.m_rotation + && m_rotateFast == other.m_rotateFast; } private: units::meters_per_second_t m_velocity; frc::Rotation2d m_rotation; + bool m_rotateFast; }; } diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerTrajectory.h b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerTrajectory.h index 3f1e8edb..5645ca5b 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerTrajectory.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerTrajectory.h @@ -128,10 +128,12 @@ class PathPlannerTrajectory { * * @param path PathPlannerPath to generate the trajectory for * @param startingSpeeds Starting speeds of the robot when starting the trajectory + * @param startingRotation Starting rotation of the robot when starting the trajectory */ PathPlannerTrajectory(std::shared_ptr path, - const frc::ChassisSpeeds &startingSpeeds) : m_states( - generateStates(path, startingSpeeds)) { + const frc::ChassisSpeeds &startingSpeeds, + const frc::Rotation2d &startingRotation) : m_states( + generateStates(path, startingSpeeds, startingRotation)) { } /** @@ -216,6 +218,7 @@ class PathPlannerTrajectory { static std::vector generateStates( std::shared_ptr path, - const frc::ChassisSpeeds &startingSpeeds); + const frc::ChassisSpeeds &startingSpeeds, + const frc::Rotation2d &startingRotation); }; } diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPoint.h b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPoint.h index 04eb89de..34808ed7 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPoint.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPoint.h @@ -1,12 +1,12 @@ #pragma once #include -#include #include #include #include #include #include "pathplanner/lib/path/PathConstraints.h" +#include "pathplanner/lib/path/RotationTarget.h" namespace pathplanner { class PathPoint { @@ -16,12 +16,12 @@ class PathPoint { units::meter_t curveRadius = 0_m; units::meters_per_second_t maxV = units::meters_per_second_t { std::numeric_limits::infinity() }; - std::optional holonomicRotation = std::nullopt; + std::optional rotationTarget = std::nullopt; std::optional constraints = std::nullopt; constexpr PathPoint(frc::Translation2d pos, - std::optional rot, - std::optional pathCostriaints) : position(pos), holonomicRotation( + std::optional rot, + std::optional pathCostriaints) : position(pos), rotationTarget( rot), constraints(pathCostriaints) { } }; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/path/RotationTarget.h b/pathplannerlib/src/main/native/include/pathplanner/lib/path/RotationTarget.h index c036f0fc..f8abffdf 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/path/RotationTarget.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/path/RotationTarget.h @@ -11,10 +11,12 @@ class RotationTarget { * * @param waypointRelativePosition Waypoint relative position of this target * @param target Target rotation + * @param rotateFast Should the robot reach the rotation as fast as possible */ constexpr RotationTarget(double waypointRelativePosition, - frc::Rotation2d target) : m_position(waypointRelativePosition), m_target( - target) { + frc::Rotation2d target, bool rotateFast) : m_position( + waypointRelativePosition), m_target(target), m_rotateFast( + rotateFast) { } /** @@ -43,6 +45,15 @@ class RotationTarget { return m_target; } + /** + * Get if the robot should reach the rotation as fast as possible + * + * @return True if the robot should reach the rotation as fast as possible + */ + constexpr bool shouldRotateFast() const { + return m_rotateFast; + } + /** * Transform the position of this target for a given segment number. * @@ -52,16 +63,18 @@ class RotationTarget { * @return The transformed target */ constexpr RotationTarget forSegmentIndex(int segmentIndex) const { - return RotationTarget(m_position - segmentIndex, m_target); + return RotationTarget(m_position - segmentIndex, m_target, m_rotateFast); } inline bool operator==(const RotationTarget &other) const { return std::abs(m_position - other.m_position) < 1E-9 - && m_target == other.m_target; + && m_target == other.m_target + && m_rotateFast == other.m_rotateFast; } private: double m_position; frc::Rotation2d m_target; + bool m_rotateFast; }; } 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 f8b28348..0bf4e9f3 100644 --- a/pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java +++ b/pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java @@ -26,7 +26,7 @@ public void testDifferentialStartingPose() { new ArrayList<>(), new ArrayList<>(), new PathConstraints(1, 2, 3, 4), - new GoalEndState(0, Rotation2d.fromDegrees(0)), + new GoalEndState(0, Rotation2d.fromDegrees(0), false), true, null); @@ -50,7 +50,7 @@ public void testHolomonicStartingPoseSet() { new ArrayList<>(), new ArrayList<>(), new PathConstraints(1, 2, 3, 4), - new GoalEndState(0, Rotation2d.fromDegrees(0)), + new GoalEndState(0, Rotation2d.fromDegrees(0), false), true, Rotation2d.fromDegrees(90)); Pose2d initialPose = path.getPreviewStartingHolonomicPose(); @@ -73,7 +73,7 @@ public void testHolomonicStartingPoseNotSet() { new ArrayList<>(), new ArrayList<>(), new PathConstraints(1, 2, 3, 4), - new GoalEndState(0, Rotation2d.fromDegrees(0)), + new GoalEndState(0, Rotation2d.fromDegrees(0), false), true, null); Pose2d initialPose = path.getPreviewStartingHolonomicPose(); diff --git a/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/GoalEndStateTest.cpp b/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/GoalEndStateTest.cpp index a59094e6..f78a3a55 100644 --- a/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/GoalEndStateTest.cpp +++ b/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/GoalEndStateTest.cpp @@ -4,7 +4,7 @@ using namespace pathplanner; TEST(GoalEndStateTest, TestGetters) { - GoalEndState endState(2_mps, frc::Rotation2d(35_deg)); + GoalEndState endState(2_mps, frc::Rotation2d(35_deg), false); EXPECT_DOUBLE_EQ(2.0, endState.getVelocity()()); EXPECT_EQ(frc::Rotation2d(35_deg), endState.getRotation()); @@ -14,6 +14,7 @@ TEST(GoalEndStateTest, TestFromJson) { wpi::json json; json.emplace("velocity", 1.25); json.emplace("rotation", -15.5); + json.emplace("rotateFast", true); - EXPECT_EQ(GoalEndState(1.25_mps, frc::Rotation2d(-15.5_deg)), GoalEndState::fromJson(json)); + EXPECT_EQ(GoalEndState(1.25_mps, frc::Rotation2d(-15.5_deg), true), GoalEndState::fromJson(json)); } \ No newline at end of file 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 d3950d33..bbc74184 100644 --- a/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/PathPlannerPathTest.cpp +++ b/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/PathPlannerPathTest.cpp @@ -10,7 +10,7 @@ TEST(PathPlannerPathTest, DifferentialStartingPose) { std::vector(), std::vector(), PathConstraints {1_mps, 2_mps_sq, 3_rad_per_s, 4_rad_per_s_sq}, - GoalEndState(0_mps, 0_deg), + GoalEndState(0_mps, 0_deg, false), true); frc::Pose2d initialPose = path.getStartingDifferentialPose(); @@ -27,7 +27,7 @@ TEST(PathPlannerPathTest, HolomonicStartingPoseSet) std::vector(), std::vector(), PathConstraints {1_mps, 2_mps_sq, 3_rad_per_s, 4_rad_per_s_sq}, - GoalEndState(0_mps, 0_deg), + GoalEndState(0_mps, 0_deg, false), true, frc::Rotation2d {90_deg}); @@ -45,7 +45,7 @@ TEST(PathPlannerPathTest, HolomonicStartingPoseNotSet) std::vector(), std::vector(), PathConstraints {1_mps, 2_mps_sq, 3_rad_per_s, 4_rad_per_s_sq}, - GoalEndState(0_mps, 0_deg), + GoalEndState(0_mps, 0_deg, false), true); frc::Pose2d initialPose = path.getPreviewStartingHolonomicPose(); diff --git a/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/RotationTargetTest.cpp b/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/RotationTargetTest.cpp index 829807fd..9e57c952 100644 --- a/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/RotationTargetTest.cpp +++ b/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/RotationTargetTest.cpp @@ -4,14 +4,14 @@ using namespace pathplanner; TEST(RotationTargetTest, TestGetters) { - RotationTarget target(1.5, frc::Rotation2d(90_deg)); + RotationTarget target(1.5, frc::Rotation2d(90_deg), false); EXPECT_DOUBLE_EQ(1.5, target.getPosition()); EXPECT_EQ(frc::Rotation2d(90_deg), target.getTarget()); } TEST(RotationTargetTest, TestForSegmentIndex) { - RotationTarget target(1.5, frc::Rotation2d(90_deg)); + RotationTarget target(1.5, frc::Rotation2d(90_deg), false); RotationTarget forSegment = target.forSegmentIndex(1); EXPECT_DOUBLE_EQ(0.5, forSegment.getPosition()); @@ -22,6 +22,7 @@ TEST(RotationTargetTest, TestFromJson) { wpi::json json; json.emplace("waypointRelativePos", 2.1); json.emplace("rotationDegrees", -45); + json.emplace("rotateFast", true); - EXPECT_EQ(RotationTarget(2.1, frc::Rotation2d(-45_deg)), RotationTarget::fromJson(json)); + EXPECT_EQ(RotationTarget(2.1, frc::Rotation2d(-45_deg), true), RotationTarget::fromJson(json)); } \ No newline at end of file diff --git a/pubspec.lock b/pubspec.lock index eb463df5..32a4b101 100644 --- a/pubspec.lock +++ b/pubspec.lock @@ -173,10 +173,10 @@ packages: dependency: "direct main" description: name: collection - sha256: f092b211a4319e98e5ff58223576de6c2803db36221657b46c82574721240687 + sha256: ee67cb0715911d28db6bf4af1026078bd6f0128b07a5f66fb2ed94ec6783c09a url: "https://pub.dev" source: hosted - version: "1.17.2" + version: "1.18.0" console: dependency: transitive description: @@ -604,10 +604,10 @@ packages: dependency: transitive description: name: meta - sha256: "3c74dbf8763d36539f114c799d8a2d87343b5067e9d796ca22b5eb8437090ee3" + sha256: a6e590c838b18133bb482a2745ad77c5bb7715fb0451209e1a7567d416678b8e url: "https://pub.dev" source: hosted - version: "1.9.1" + version: "1.10.0" mime: dependency: transitive description: @@ -905,18 +905,18 @@ packages: dependency: transitive description: name: stack_trace - sha256: c3c7d8edb15bee7f0f74debd4b9c5f3c2ea86766fe4178eb2a18eb30a0bdaed5 + sha256: "73713990125a6d93122541237550ee3352a2d84baad52d375a4cad2eb9b7ce0b" url: "https://pub.dev" source: hosted - version: "1.11.0" + version: "1.11.1" stream_channel: dependency: transitive description: name: stream_channel - sha256: "83615bee9045c1d322bbbd1ba209b7a749c2cbcdcb3fdd1df8eb488b3279c1c8" + sha256: ba2aa5d8cc609d96bbb2899c28934f9e1af5cddbd60a827822ea467161eb54e7 url: "https://pub.dev" source: hosted - version: "2.1.1" + version: "2.1.2" stream_transform: dependency: transitive description: @@ -945,10 +945,10 @@ packages: dependency: transitive description: name: test_api - sha256: "75760ffd7786fffdfb9597c35c5b27eaeec82be8edfb6d71d32651128ed7aab8" + sha256: "5c2f730018264d276c20e4f1503fd1308dfbbae39ec8ee63c5236311ac06954b" url: "https://pub.dev" source: hosted - version: "0.6.0" + version: "0.6.1" timing: dependency: transitive description: @@ -1089,10 +1089,10 @@ packages: dependency: transitive description: name: web - sha256: dc8ccd225a2005c1be616fe02951e2e342092edf968cf0844220383757ef8f10 + sha256: afe077240a270dcfd2aafe77602b4113645af95d0ad31128cc02bce5ac5d5152 url: "https://pub.dev" source: hosted - version: "0.1.4-beta" + version: "0.3.0" web_socket_channel: dependency: transitive description: @@ -1142,5 +1142,5 @@ packages: source: hosted version: "3.1.2" sdks: - dart: ">=3.1.0 <4.0.0" + dart: ">=3.2.0-194.0.dev <4.0.0" flutter: ">=3.13.0" diff --git a/resources/default_navgrid.json b/resources/default_navgrid.json index 93cd467d..1da30af4 100644 --- a/resources/default_navgrid.json +++ b/resources/default_navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.02},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.02},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/test/path/goal_end_state_test.dart b/test/path/goal_end_state_test.dart index 28153ee0..22d5a8e1 100644 --- a/test/path/goal_end_state_test.dart +++ b/test/path/goal_end_state_test.dart @@ -6,16 +6,19 @@ void main() { GoalEndState g = GoalEndState( velocity: 1.0, rotation: 2.0, + rotateFast: true, ); expect(g.velocity, 1.0); expect(g.rotation, 2.0); + expect(g.rotateFast, true); }); test('toJson/fromJson interoperability', () { GoalEndState g = GoalEndState( velocity: 1.0, rotation: 2.0, + rotateFast: true, ); Map json = g.toJson(); diff --git a/test/path/rotation_target_test.dart b/test/path/rotation_target_test.dart index ddf3730f..ff6498c0 100644 --- a/test/path/rotation_target_test.dart +++ b/test/path/rotation_target_test.dart @@ -3,11 +3,12 @@ import 'package:pathplanner/path/rotation_target.dart'; void main() { test('Constructor functions', () { - RotationTarget t = - RotationTarget(waypointRelativePos: 1.0, rotationDegrees: 5.0); + RotationTarget t = RotationTarget( + waypointRelativePos: 1.0, rotationDegrees: 5.0, rotateFast: true); expect(t.waypointRelativePos, 1.0); expect(t.rotationDegrees, 5.0); + expect(t.rotateFast, true); }); test('toJson/fromJson interoperability', () { diff --git a/test/services/simulator/trajectory_generator_test.dart b/test/services/simulator/trajectory_generator_test.dart index 50b2fe4a..7d09b8be 100644 --- a/test/services/simulator/trajectory_generator_test.dart +++ b/test/services/simulator/trajectory_generator_test.dart @@ -29,7 +29,11 @@ void main() { goalEndState: GoalEndState(), constraintZones: [], rotationTargets: [ - RotationTarget(waypointRelativePos: 0.5, rotationDegrees: 45), + RotationTarget( + waypointRelativePos: 0.5, + rotationDegrees: 45, + rotateFast: true, + ), ], eventMarkers: [], pathDir: '', diff --git a/test/widgets/editor/tree_widgets/goal_end_state_tree_test.dart b/test/widgets/editor/tree_widgets/goal_end_state_tree_test.dart index 3a1ef42e..2b3d94c0 100644 --- a/test/widgets/editor/tree_widgets/goal_end_state_tree_test.dart +++ b/test/widgets/editor/tree_widgets/goal_end_state_tree_test.dart @@ -19,7 +19,11 @@ void main() { fs: MemoryFileSystem(), ); path.goalEndStateExpanded = true; - path.goalEndState = GoalEndState(velocity: 1.0, rotation: 1.0); + path.goalEndState = GoalEndState( + velocity: 1.0, + rotation: 1.0, + rotateFast: false, + ); pathChanged = false; }); @@ -105,4 +109,31 @@ void main() { await widgetTester.pump(); expect(path.goalEndState.rotation, 1.0); }); + + testWidgets('rotate fast', (widgetTester) async { + await widgetTester.pumpWidget(MaterialApp( + home: Scaffold( + body: GoalEndStateTree( + path: path, + onPathChanged: () => pathChanged = true, + undoStack: undoStack, + holonomicMode: true, + ), + ), + )); + + final checkbox = find.byType(Checkbox); + + expect(checkbox, findsOneWidget); + + await widgetTester.tap(checkbox); + await widgetTester.pump(); + + expect(pathChanged, true); + expect(path.goalEndState.rotateFast, true); + + undoStack.undo(); + await widgetTester.pump(); + expect(path.goalEndState.rotateFast, false); + }); } diff --git a/test/widgets/editor/tree_widgets/rotation_targets_tree_test.dart b/test/widgets/editor/tree_widgets/rotation_targets_tree_test.dart index dffe45cd..f328f219 100644 --- a/test/widgets/editor/tree_widgets/rotation_targets_tree_test.dart +++ b/test/widgets/editor/tree_widgets/rotation_targets_tree_test.dart @@ -23,8 +23,16 @@ void main() { fs: MemoryFileSystem(), ); path.rotationTargets = [ - RotationTarget(waypointRelativePos: 0.5, rotationDegrees: 0.0), - RotationTarget(waypointRelativePos: 1.5, rotationDegrees: 0.0), + RotationTarget( + waypointRelativePos: 0.5, + rotationDegrees: 0.0, + rotateFast: false, + ), + RotationTarget( + waypointRelativePos: 1.5, + rotationDegrees: 0.0, + rotateFast: false, + ), ]; path.rotationTargetsExpanded = true; pathChanged = false; @@ -281,4 +289,34 @@ void main() { await widgetTester.pump(); expect(path.rotationTargets.length, 2); }); + + testWidgets('rotate fast', (widgetTester) async { + await widgetTester.pumpWidget(MaterialApp( + home: Scaffold( + body: RotationTargetsTree( + path: path, + onPathChanged: () => pathChanged = true, + onTargetHovered: (value) => hoveredTarget = value, + onTargetSelected: (value) => selectedTarget = value, + undoStack: undoStack, + initiallySelectedTarget: 0, + ), + ), + )); + + var checkbox = find.byType(Checkbox); + + expect(checkbox, findsOneWidget); + + await widgetTester.tap(checkbox); + await widgetTester.pump(); + + expect(pathChanged, true); + expect(path.rotationTargets[0].rotateFast, true); + + undoStack.undo(); + await widgetTester.pump(); + + expect(path.rotationTargets[0].rotateFast, false); + }); } diff --git a/windows/flutter/CMakeLists.txt b/windows/flutter/CMakeLists.txt index b02c5485..86edc67b 100644 --- a/windows/flutter/CMakeLists.txt +++ b/windows/flutter/CMakeLists.txt @@ -9,6 +9,11 @@ include(${EPHEMERAL_DIR}/generated_config.cmake) # https://github.com/flutter/flutter/issues/57146. set(WRAPPER_ROOT "${EPHEMERAL_DIR}/cpp_client_wrapper") +# Set fallback configurations for older versions of the flutter tool. +if (NOT DEFINED FLUTTER_TARGET_PLATFORM) + set(FLUTTER_TARGET_PLATFORM "windows-x64") +endif() + # === Flutter Library === set(FLUTTER_LIBRARY "${EPHEMERAL_DIR}/flutter_windows.dll") @@ -91,7 +96,7 @@ add_custom_command( COMMAND ${CMAKE_COMMAND} -E env ${FLUTTER_TOOL_ENVIRONMENT} "${FLUTTER_ROOT}/packages/flutter_tools/bin/tool_backend.bat" - windows-x64 $ + ${FLUTTER_TARGET_PLATFORM} $ VERBATIM ) add_custom_target(flutter_assemble DEPENDS