A couple random fixes (#404)

* fix path simulator replanning crash * [java] fix dynamic obstacle pathfinding issue * c++ pathfinding fix
mjansen4857 · Oct 15, 2023 · d2c1589 · d2c1589
1 parent a3bd7ae
commit d2c1589
Show file tree

Hide file tree

Showing 7 changed files with 114 additions and 26 deletions.
diff --git a/lib/services/simulator/trajectory_generator.dart b/lib/services/simulator/trajectory_generator.dart
@@ -206,7 +206,7 @@ class TrajectoryGenerator {
               robotToJoinDelta / 3.0 * sin(heading));
     }
 
-    if (joinAnchorIdx == path.pathPoints.length) {
+    if (joinAnchorIdx == path.pathPoints.length - 1) {
       // Throw out rotation targets, event markers, and constraint zones since we are skipping all
       // of the path
       return PathPlannerPath(

diff --git a/lib/widgets/editor/split_auto_editor.dart b/lib/widgets/editor/split_auto_editor.dart
@@ -3,6 +3,7 @@ import 'dart:math';
 import 'package:flutter/material.dart';
 import 'package:multi_split_view/multi_split_view.dart';
 import 'package:pathplanner/auto/pathplanner_auto.dart';
+import 'package:pathplanner/services/log.dart';
 import 'package:pathplanner/services/simulator/trajectory_generator.dart';
 import 'package:pathplanner/util/pose2d.dart';
 import 'package:pathplanner/path/pathplanner_path.dart';
@@ -271,7 +272,11 @@ class _SplitAutoEditorState extends State<SplitAutoEditor>
                     },
                     onAutoChanged: () {
                       widget.onAutoChanged?.call();
-                      _simulateAuto();
+                      // Delay this because it needs the parent widget to rebuild first
+                      Future.delayed(const Duration(milliseconds: 100))
+                          .then((_) {
+                        _simulateAuto();
+                      });
                     },
                     onSideSwapped: () => setState(() {
                       _treeOnRight = !_treeOnRight;
@@ -292,8 +297,14 @@ class _SplitAutoEditorState extends State<SplitAutoEditor>
 
   // Marked as async so it can run from initState
   void _simulateAuto() async {
-    Trajectory? simPath = TrajectoryGenerator.simulateAuto(
-        widget.autoPaths, widget.auto.startingPose);
+    Trajectory? simPath;
+
+    try {
+      simPath = TrajectoryGenerator.simulateAuto(
+          widget.autoPaths, widget.auto.startingPose);
+    } catch (err) {
+      Log.error('Failed to simulate auto', err);
+    }
 
     if (simPath != null &&
         simPath.states.last.time.isFinite &&

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java
@@ -3,6 +3,7 @@
 import com.pathplanner.lib.controllers.PathFollowingController;
 import com.pathplanner.lib.path.*;
 import com.pathplanner.lib.pathfinding.ADStar;
+import com.pathplanner.lib.util.GeometryUtil;
 import com.pathplanner.lib.util.PPLibTelemetry;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -33,6 +34,8 @@ public class PathfindingCommand extends Command {
   private PathPlannerTrajectory currentTrajectory;
   private Pose2d startingPose;
 
+  private double timeOffset = 0;
+
   /**
    * Constructs a new base pathfinding command that will generate a path towards the given path.
    *
@@ -123,6 +126,7 @@ public PathfindingCommand(
   @Override
   public void initialize() {
     currentTrajectory = null;
+    timeOffset = 0;
 
     Pose2d currentPose = poseSupplier.get();
 
@@ -168,12 +172,47 @@ public void execute() {
         if (currentPose.getTranslation().getDistance(path.getPoint(0).position) <= 0.25) {
           currentTrajectory = new PathPlannerTrajectory(path, currentSpeeds);
 
+          // Find the two closest states in front of and behind robot
+          int closestState1Idx = 0;
+          int closestState2Idx = 1;
+          while (true) {
+            double closest2Dist =
+                currentTrajectory
+                    .getState(closestState2Idx)
+                    .positionMeters
+                    .getDistance(currentPose.getTranslation());
+            double nextDist =
+                currentTrajectory
+                    .getState(closestState2Idx + 1)
+                    .positionMeters
+                    .getDistance(currentPose.getTranslation());
+            if (nextDist < closest2Dist) {
+              closestState1Idx++;
+              closestState2Idx++;
+            } else {
+              break;
+            }
+          }
+
+          // Use the closest 2 states to interpolate what the time offset should be
+          // This will account for the delay in pathfinding
+          var closestState1 = currentTrajectory.getState(closestState1Idx);
+          var closestState2 = currentTrajectory.getState(closestState2Idx);
+
+          double d = closestState1.positionMeters.getDistance(closestState2.positionMeters);
+          double t = (currentPose.getTranslation().getDistance(closestState1.positionMeters)) / d;
+
+          timeOffset =
+              GeometryUtil.doubleLerp(closestState1.timeSeconds, closestState2.timeSeconds, t);
+
           PathPlannerLogging.logActivePath(path);
           PPLibTelemetry.setCurrentPath(path);
         } else {
           PathPlannerPath replanned = path.replan(currentPose, currentSpeeds);
           currentTrajectory = new PathPlannerTrajectory(replanned, currentSpeeds);
 
+          timeOffset = 0;
+
           PathPlannerLogging.logActivePath(replanned);
           PPLibTelemetry.setCurrentPath(replanned);
         }
@@ -184,7 +223,7 @@ public void execute() {
     }
 
     if (currentTrajectory != null) {
-      PathPlannerTrajectory.State targetState = currentTrajectory.sample(timer.get());
+      PathPlannerTrajectory.State targetState = currentTrajectory.sample(timer.get() + timeOffset);
 
       // Set the target rotation to the starting rotation if we have not yet traveled the rotation
       // delay distance
@@ -237,7 +276,7 @@ public boolean isFinished() {
     }
 
     if (currentTrajectory != null) {
-      return timer.hasElapsed(currentTrajectory.getTotalTimeSeconds());
+      return timer.hasElapsed(currentTrajectory.getTotalTimeSeconds() - timeOffset);
     }
 
     return false;

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/ADStar.java b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/ADStar.java
@@ -305,12 +305,10 @@ public static void setDynamicObstacles(
       needsReset = true;
       doMinor = true;
       doMajor = true;
-
-      if (dynamicObstacles.contains(getGridPos(currentRobotPos))) {
-        // Set the start position to the closest non-obstacle
-        setStartPos(currentRobotPos);
-      }
     }
+
+    setStartPos(currentRobotPos);
+    setGoalPos(realGoalPos);
   }
 
   private static List<Translation2d> extractPath() {

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp
@@ -1,5 +1,6 @@
 #include "pathplanner/lib/commands/PathfindingCommand.h"
 #include "pathplanner/lib/pathfinding/ADStar.h"
+#include "pathplanner/lib/util/GeometryUtil.h"
 #include <vector>
 
 using namespace pathplanner;
@@ -52,6 +53,7 @@ PathfindingCommand::PathfindingCommand(frc::Pose2d targetPose,
 
 void PathfindingCommand::Initialize() {
 	m_currentTrajectory = PathPlannerTrajectory();
+	m_timeOffset = 0_s;
 
 	frc::Pose2d currentPose = m_poseSupplier();
 
@@ -95,13 +97,48 @@ void PathfindingCommand::Execute() {
 				m_currentTrajectory = PathPlannerTrajectory(path,
 						currentSpeeds);
 
+				// Find the two closest states in front of and behind robot
+				size_t closestState1Idx = 0;
+				size_t closestState2Idx = 1;
+				while (true) {
+					auto closest2Dist = m_currentTrajectory.getState(
+							closestState2Idx).position.Distance(
+							currentPose.Translation());
+					auto nextDist = m_currentTrajectory.getState(
+							closestState2Idx + 1).position.Distance(
+							currentPose.Translation());
+					if (nextDist < closest2Dist) {
+						closestState1Idx++;
+						closestState2Idx++;
+					} else {
+						break;
+					}
+				}
+
+				// Use the closest 2 states to interpolate what the time offset should be
+				// This will account for the delay in pathfinding
+				auto closestState1 = m_currentTrajectory.getState(
+						closestState1Idx);
+				auto closestState2 = m_currentTrajectory.getState(
+						closestState2Idx);
+
+				auto d = closestState1.position.Distance(
+						closestState2.position);
+				double t = ((currentPose.Translation().Distance(
+						closestState1.position)) / d)();
+
+				m_timeOffset = GeometryUtil::unitLerp(closestState1.time,
+						closestState2.time, t);
+
 				PathPlannerLogging::logActivePath(path);
 				PPLibTelemetry::setCurrentPath(path);
 			} else {
 				auto replanned = path->replan(currentPose, currentSpeeds);
 				m_currentTrajectory = PathPlannerTrajectory(replanned,
 						currentSpeeds);
 
+				m_timeOffset = 0_s;
+
 				PathPlannerLogging::logActivePath(replanned);
 				PPLibTelemetry::setCurrentPath(replanned);
 			}
@@ -113,7 +150,7 @@ void PathfindingCommand::Execute() {
 
 	if (m_currentTrajectory.getStates().size() > 0) {
 		PathPlannerTrajectory::State targetState = m_currentTrajectory.sample(
-				m_timer.Get());
+				m_timer.Get() + m_timeOffset);
 
 		// Set the target rotation to the starting rotation if we have not yet traveled the rotation
 		// delay distance
@@ -165,7 +202,8 @@ bool PathfindingCommand::IsFinished() {
 	}
 
 	if (m_currentTrajectory.getStates().size() > 0) {
-		return m_timer.HasElapsed(m_currentTrajectory.getTotalTime());
+		return m_timer.HasElapsed(
+				m_currentTrajectory.getTotalTime() - m_timeOffset);
 	}
 
 	return false;

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/ADStar.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/pathfinding/ADStar.cpp
@@ -256,21 +256,21 @@ void ADStar::setDynamicObstacles(
 		}
 	}
 
-	std::lock_guard < std::mutex > lock(mutex);
+	{
+		std::lock_guard < std::mutex > lock(mutex);
 
-	dynamicObstacles.clear();
-	dynamicObstacles.insert(newObs.begin(), newObs.end());
-	obstacles.clear();
-	obstacles.insert(staticObstacles.begin(), staticObstacles.end());
-	obstacles.insert(dynamicObstacles.begin(), dynamicObstacles.end());
-	needsReset = true;
-	doMinor = true;
-	doMajor = true;
-
-	if (dynamicObstacles.contains(getGridPos(currentRobotPos))) {
-		// Set the start position to the closest non-obstacle
-		setStartPos(currentRobotPos);
+		dynamicObstacles.clear();
+		dynamicObstacles.insert(newObs.begin(), newObs.end());
+		obstacles.clear();
+		obstacles.insert(staticObstacles.begin(), staticObstacles.end());
+		obstacles.insert(dynamicObstacles.begin(), dynamicObstacles.end());
+		needsReset = true;
+		doMinor = true;
+		doMajor = true;
 	}
+
+	setStartPos(currentRobotPos);
+	setGoalPos (realGoalPos);
 }
 
 std::vector<frc::Translation2d> ADStar::extractPath() {

diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h
@@ -90,5 +90,7 @@ class PathfindingCommand: public frc2::CommandHelper<frc2::Command,
 
 	PathPlannerTrajectory m_currentTrajectory;
 	frc::Pose2d m_startingPose;
+
+	units::second_t m_timeOffset;
 };
 }