Merge branch 'main' into warn_on_unknown_command

mjansen4857 · Jan 26, 2024 · ce6e880 · ce6e880
2 parents 23920e2 + 6789c3e
commit ce6e880
Show file tree

Hide file tree

Showing 4 changed files with 30 additions and 15 deletions.
diff --git a/pathplannerlib-python/pathplannerlib/commands.py b/pathplannerlib-python/pathplannerlib/commands.py
@@ -275,6 +275,7 @@ class PathfindingCommand(Command):
     _timer: Timer = Timer()
     _targetPath: Union[PathPlannerPath, None]
     _targetPose: Pose2d
+    _originalTargetPose: Pose2d
     _goalEndState: GoalEndState
     _constraints: PathConstraints
     _poseSupplier: Callable[[], Pose2d]
@@ -332,10 +333,12 @@ def __init__(self, constraints: PathConstraints, pose_supplier: Callable[[], Pos
                         break
             self._targetPath = target_path
             self._targetPose = Pose2d(target_path.getPoint(0).position, targetRotation)
+            self._originalTargetPose = Pose2d(target_path.getPoint(0).position, targetRotation)
             self._goalEndState = GoalEndState(goalEndVel, targetRotation, True)
         else:
             self._targetPath = None
             self._targetPose = target_pose
+            self._originalTargetPose = target_pose
             self._goalEndState = GoalEndState(goal_end_vel, target_pose.rotation(), True)
 
         PathfindingCommand._instances += 1
@@ -350,9 +353,10 @@ def initialize(self):
         self._controller.reset(currentPose, self._speedsSupplier())
 
         if self._targetPath is not None:
-            self._targetPose = Pose2d(self._targetPath.getPoint(0).position, self._goalEndState.rotation)
+            self._originalTargetPose = Pose2d(self._targetPath.getPoint(0).position, self._originalTargetPose.rotation())
             if self._shouldFlipPath():
-                self._targetPose = flipFieldPose(self._targetPose)
+                self._targetPose = flipFieldPose(self._originalTargetPose)
+                self._goalEndState = GoalEndState(self._goalEndState.velocity, self._targetPose.rotation(), True)
 
         if currentPose.translation().distance(self._targetPose.translation()) < 0.5:
             self._output(ChassisSpeeds())

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java
@@ -27,7 +27,8 @@ public class PathfindingCommand extends Command {
   private final Timer timer = new Timer();
   private final PathPlannerPath targetPath;
   private Pose2d targetPose;
-  private final GoalEndState goalEndState;
+  private Pose2d originalTargetPose;
+  private GoalEndState goalEndState;
   private final PathConstraints constraints;
   private final Supplier<Pose2d> poseSupplier;
   private final Supplier<ChassisSpeeds> speedsSupplier;
@@ -94,6 +95,8 @@ public PathfindingCommand(
 
     this.targetPath = targetPath;
     this.targetPose = new Pose2d(this.targetPath.getPoint(0).position, targetRotation);
+    this.originalTargetPose =
+        new Pose2d(this.targetPose.getTranslation(), this.targetPose.getRotation());
     this.goalEndState = new GoalEndState(goalEndVel, targetRotation, true);
     this.constraints = constraints;
     this.controller = controller;
@@ -141,6 +144,8 @@ public PathfindingCommand(
 
     this.targetPath = null;
     this.targetPose = targetPose;
+    this.originalTargetPose =
+        new Pose2d(this.targetPose.getTranslation(), this.targetPose.getRotation());
     this.goalEndState = new GoalEndState(goalEndVel, targetPose.getRotation(), true);
     this.constraints = constraints;
     this.controller = controller;
@@ -165,9 +170,11 @@ public void initialize() {
     controller.reset(currentPose, speedsSupplier.get());
 
     if (targetPath != null) {
-      targetPose = new Pose2d(this.targetPath.getPoint(0).position, goalEndState.getRotation());
+      originalTargetPose =
+          new Pose2d(this.targetPath.getPoint(0).position, originalTargetPose.getRotation());
       if (shouldFlipPath.getAsBoolean()) {
-        targetPose = GeometryUtil.flipFieldPose(targetPose);
+        targetPose = GeometryUtil.flipFieldPose(this.originalTargetPose);
+        goalEndState = new GoalEndState(goalEndState.getVelocity(), targetPose.getRotation());
       }
     }
 

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp
@@ -46,6 +46,7 @@ PathfindingCommand::PathfindingCommand(
 
 	m_targetPose = frc::Pose2d(m_targetPath->getPoint(0).position,
 			targetRotation);
+	m_originalTargetPose = m_targetPose;
 	m_goalEndState = GoalEndState(goalEndVel, targetRotation, true);
 
 	m_instances++;
@@ -61,13 +62,13 @@ PathfindingCommand::PathfindingCommand(frc::Pose2d targetPose,
 		std::unique_ptr<PathFollowingController> controller,
 		units::meter_t rotationDelayDistance, ReplanningConfig replanningConfig,
 		frc2::Requirements requirements) : m_targetPath(), m_targetPose(
-		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), m_shouldFlipPath(
-		[]() {
-			return false;
-		}) {
+		targetPose), m_originalTargetPose(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), m_shouldFlipPath([]() {
+	return false;
+}) {
 	AddRequirements(requirements);
 
 	Pathfinding::ensureInitialized();
@@ -86,10 +87,12 @@ void PathfindingCommand::Initialize() {
 	m_controller->reset(currentPose, m_speedsSupplier());
 
 	if (m_targetPath) {
-		m_targetPose = frc::Pose2d(m_targetPath->getPoint(0).position,
-				m_goalEndState.getRotation());
+		m_originalTargetPose = frc::Pose2d(m_targetPath->getPoint(0).position,
+				m_originalTargetPose.Rotation());
 		if (m_shouldFlipPath()) {
-			m_targetPose = GeometryUtil::flipFieldPose(m_targetPose);
+			m_targetPose = GeometryUtil::flipFieldPose(m_originalTargetPose);
+			m_goalEndState = GoalEndState(m_goalEndState.getVelocity(),
+					m_targetPose.Rotation(), true);
 		}
 	}
 

diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h
@@ -86,6 +86,7 @@ class PathfindingCommand: public frc2::CommandHelper<frc2::Command,
 	frc::Timer m_timer;
 	std::shared_ptr<PathPlannerPath> m_targetPath;
 	frc::Pose2d m_targetPose;
+	frc::Pose2d m_originalTargetPose;
 	GoalEndState m_goalEndState;
 	PathConstraints m_constraints;
 	std::function<frc::Pose2d()> m_poseSupplier;