Generate path point at exact locations of rotation targets (#760)

lib/path/pathplanner_path.dart

@@ -408,18 +408,35 @@ class PathPlannerPath {
         }
       }
 
-      // Add a rotation target to the previous point if it is closer to it than
-      // the current point
-      if (unaddedTargets.isNotEmpty) {
-        if ((unaddedTargets[0].waypointRelativePos - prevPos).abs() <=
-            (unaddedTargets[0].waypointRelativePos - pos).abs()) {
-          pathPoints.last.rotationTarget = unaddedTargets.removeAt(0);
+      // Add rotation targets
+      RotationTarget? target;
+      PathPoint prevPoint = pathPoints.last;
+
+      while (unaddedTargets.isNotEmpty &&
+          unaddedTargets[0].waypointRelativePos >= prevPos &&
+          unaddedTargets[0].waypointRelativePos <= pos) {
+        if ((unaddedTargets[0].waypointRelativePos - prevPos).abs() < 0.001) {
+          // Close enough to prev pos
+          prevPoint.rotationTarget = unaddedTargets.removeAt(0);
+        } else if ((unaddedTargets[0].waypointRelativePos - pos).abs() <
+            0.001) {
+          // Close enough to next pos
+          target = unaddedTargets.removeAt(0);
+        } else {
+          // We should insert a point at the exact position
+          RotationTarget t = unaddedTargets.removeAt(0);
+          pathPoints.add(PathPoint(
+            position: samplePath(t.waypointRelativePos),
+            rotationTarget: t,
+            constraints: _constraintsForPos(t.waypointRelativePos),
+            waypointPos: t.waypointRelativePos,
+          ));
         }
       }
 
       pathPoints.add(PathPoint(
         position: position,
-        rotationTarget: null,
+        rotationTarget: target,
         constraints: _constraintsForPos(pos),
         waypointPos: pos,
       ));

pathplannerlib-python/pathplannerlib/path.py

@@ -773,14 +773,25 @@ def _createPath(self) -> List[PathPoint]:
                     pos = pos + (correctIncrement * 0.5)
                     position = self._samplePath(pos)
 
-            # Add a rotation target to the previous point if it is closer to it than
-            # the current point
-            if len(unaddedTargets) > 0:
-                if abs(unaddedTargets[0].waypointRelativePosition - prevWaypointPos) <= abs(
-                        unaddedTargets[0].waypointRelativePosition - pos):
-                    points[-1].rotationTarget = unaddedTargets.pop(0)
+            # Add rotation targets
+            target: Union[RotationTarget, None] = None
+            prevPoint = points[-1]
+
+            while len(unaddedTargets) > 0 and prevWaypointPos <= unaddedTargets[0].waypointRelativePosition <= pos:
+                if abs(unaddedTargets[0].waypointRelativePosition - prevWaypointPos) < 0.001:
+                    # Close enough to prev pos
+                    prevPoint.rotationTarget = unaddedTargets.pop(0)
+                elif abs(unaddedTargets[0].waypointRelativePosition - pos) < 0.001:
+                    # Close enough to next pos
+                    target = unaddedTargets.pop(0)
+                else:
+                    # We should insert a point at the exact position
+                    t = unaddedTargets.pop(0)
+                    points.append(PathPoint(self._samplePath(t.waypointRelativePosition), t,
+                                            self._constraintsForWaypointPos(t.waypointRelativePosition)))
+                    points[-1].waypointRelativePos = t.waypointRelativePosition
 
-            points.append(PathPoint(position, None, self._constraintsForWaypointPos(pos)))
+            points.append(PathPoint(position, target, self._constraintsForWaypointPos(pos)))
             points[-1].waypointRelativePos = pos
             pos = min(pos + targetIncrement, numSegments)
 

pathplannerlib/.vscode/settings.json

@@ -100,6 +100,7 @@
     "xlocmes": "cpp",
     "xlocmon": "cpp",
     "xlocnum": "cpp",
-    "xloctime": "cpp"
+    "xloctime": "cpp",
+    "xtree": "cpp"
   }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java

@@ -614,16 +614,30 @@ private List<PathPoint> createPath() {
         }
       }
 
-      // Add a rotation target to the previous point if it is closer to it than
-      // the current point
-      if (!unaddedTargets.isEmpty()) {
-        if (Math.abs(unaddedTargets.get(0).getPosition() - prevWaypointPos)
-            <= Math.abs(unaddedTargets.get(0).getPosition() - pos)) {
-          points.get(points.size() - 1).rotationTarget = unaddedTargets.remove(0);
+      // Add rotation targets
+      RotationTarget target = null;
+      PathPoint prevPoint = points.get(points.size() - 1);
+
+      while (!unaddedTargets.isEmpty()
+          && unaddedTargets.get(0).getPosition() >= prevWaypointPos
+          && unaddedTargets.get(0).getPosition() <= pos) {
+        if (Math.abs(unaddedTargets.get(0).getPosition() - prevWaypointPos) < 0.001) {
+          // Close enough to prev pos
+          prevPoint.rotationTarget = unaddedTargets.remove(0);
+        } else if (Math.abs(unaddedTargets.get(0).getPosition() - pos) < 0.001) {
+          // Close enough to next pos
+          target = unaddedTargets.remove(0);
+        } else {
+          // We should insert a point at the exact position
+          RotationTarget t = unaddedTargets.remove(0);
+          points.add(
+              new PathPoint(
+                  samplePath(t.getPosition()), t, constraintsForWaypointPos(t.getPosition())));
+          points.get(points.size() - 1).waypointRelativePos = t.getPosition();
         }
       }
 
-      points.add(new PathPoint(position, null, constraintsForWaypointPos(pos)));
+      points.add(new PathPoint(position, target, constraintsForWaypointPos(pos)));
       points.get(points.size() - 1).waypointRelativePos = pos;
       pos = Math.min(pos + targetIncrement, numSegments);
     }

pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp

@@ -381,18 +381,34 @@ std::vector<PathPoint> PathPlannerPath::createPath() {
 			}
 		}
 
-		// Add a rotation target to the previous point if it is closer to it than
-		// the current point
-		if (!unaddedTargets.empty()) {
+		// Add rotation targets
+		std::optional < RotationTarget > target = std::nullopt;
+		PathPoint prevPoint = points[points.size() - 1];
+
+		while (!unaddedTargets.empty()
+				&& unaddedTargets[0].getPosition() >= prevWaypointPos
+				&& unaddedTargets[0].getPosition() <= pos) {
 			if (std::abs(unaddedTargets[0].getPosition() - prevWaypointPos)
-					<= std::abs(unaddedTargets[0].getPosition() - pos)) {
-				points[points.size() - 1].rotationTarget = unaddedTargets[0];
+					< 0.001) {
+				// Close enough to prev pos
+				prevPoint.rotationTarget = unaddedTargets[0];
 				unaddedTargets.erase(unaddedTargets.begin());
+			} else if (std::abs(unaddedTargets[0].getPosition() - pos)
+					< 0.001) {
+				// Close enough to next pos
+				target = unaddedTargets[0];
+				unaddedTargets.erase(unaddedTargets.begin());
+			} else {
+				// We should insert a point at the exact position
+				RotationTarget t = unaddedTargets[0];
+				unaddedTargets.erase(unaddedTargets.begin());
+				points.emplace_back(samplePath(t.getPosition()), t,
+						constraintsForWaypointPos(t.getPosition()));
+				points[points.size() - 1].waypointRelativePos = t.getPosition();
 			}
 		}
 
-		points.emplace_back(position, std::nullopt,
-				constraintsForWaypointPos(pos));
+		points.emplace_back(position, target, constraintsForWaypointPos(pos));
 		points[points.size() - 1].waypointRelativePos = pos;
 		pos = std::min(pos + targetIncrement, static_cast<double>(numSegments));
 	}