Choreo 2025 event marker parsing (#816)

pathplannerlib-python/pathplannerlib/events.py

@@ -31,6 +31,9 @@ def handleEvent(self, eventScheduler: 'EventScheduler') -> None:
     def cancelEvent(self, eventScheduler: 'EventScheduler') -> None:
         raise NotImplementedError
 
+    def copyWithTime(self, timestamp: float) -> 'Event':
+        raise NotImplementedError
+
 
 class ScheduleCommandEvent(Event):
     _command: Command
@@ -54,6 +57,10 @@ def cancelEvent(self, eventScheduler: 'EventScheduler') -> None:
         # Do nothing
         pass
 
+    @override
+    def copyWithTime(self, timestamp: float) -> Event:
+        return ScheduleCommandEvent(timestamp, self._command)
+
 
 class CancelCommandEvent(Event):
     _command: Command
@@ -77,6 +84,10 @@ def cancelEvent(self, eventScheduler: 'EventScheduler') -> None:
         # Do nothing
         pass
 
+    @override
+    def copyWithTime(self, timestamp: float) -> Event:
+        return CancelCommandEvent(timestamp, self._command)
+
 
 class ActivateTriggerEvent(Event):
     _name: str
@@ -100,6 +111,10 @@ def cancelEvent(self, eventScheduler: 'EventScheduler') -> None:
         # Do nothing
         pass
 
+    @override
+    def copyWithTime(self, timestamp: float) -> Event:
+        return ActivateTriggerEvent(timestamp, self._name)
+
 
 class DeactivateTriggerEvent(Event):
     _name: str
@@ -123,6 +138,10 @@ def cancelEvent(self, eventScheduler: 'EventScheduler') -> None:
         # Ensure the condition gets set to false
         eventScheduler.setCondition(self._name, False)
 
+    @override
+    def copyWithTime(self, timestamp: float) -> Event:
+        return DeactivateTriggerEvent(timestamp, self._name)
+
 
 class OneShotTriggerEvent(Event):
     _name: str
@@ -152,6 +171,10 @@ def cancelEvent(self, eventScheduler: 'EventScheduler') -> None:
         # Do nothing
         pass
 
+    @override
+    def copyWithTime(self, timestamp: float) -> Event:
+        return OneShotTriggerEvent(timestamp, self._name)
+
 
 class EventScheduler:
     _eventCommands: dict

pathplannerlib-python/pathplannerlib/path.py

@@ -8,6 +8,9 @@
 import wpimath.units as units
 from wpimath import inputModulus
 from commands2 import Command, cmd
+
+from .auto import CommandUtil
+from .events import OneShotTriggerEvent, ScheduleCommandEvent, Event
 from .util import cubicLerp, calculateRadius, floatLerp, FlippingUtil
 from .trajectory import PathPlannerTrajectory, PathPlannerTrajectoryState
 from .config import RobotConfig
@@ -490,7 +493,8 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None:
         filePath = os.path.join(getDeployDirectory(), 'choreo', trajectory_name + '.traj')
 
         with open(filePath, 'r') as f:
-            trajJson = json.loads(f.read())['trajectory']
+            fJson = json.loads(f.read())
+            trajJson = fJson['trajectory']
 
             fullTrajStates = []
             for s in trajJson['samples']:
@@ -511,6 +515,38 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None:
 
                 fullTrajStates.append(state)
 
+            fullEvents: List[Event] = []
+            # Events from pplibCommands
+            for m in fJson['pplibCommands']:
+                dataJson = m['data']
+                offsetJson = dataJson['offset']
+                eventJson = m['event']
+
+                name = str(dataJson['name'])
+                targetTimestamp = float(dataJson['targetTimestamp'])
+                offset = float(offsetJson['val'])
+                timestamp = targetTimestamp + offset
+                eventCommand = CommandUtil.commandFromJson(eventJson, True)
+
+                fullEvents.append(OneShotTriggerEvent(timestamp, name))
+                fullEvents.append(ScheduleCommandEvent(timestamp, eventCommand))
+
+            # Events from choreolib events
+            for m in fJson['events']:
+                dataJson = m['data']
+                offsetJson = dataJson['offset']
+                eventJson = m['event']
+                eventDataJson = eventJson['data']
+
+                event = str(eventDataJson['event'])
+                targetTimestamp = float(dataJson['targetTimestamp'])
+                offset = float(offsetJson['val'])
+                timestamp = targetTimestamp + offset
+
+                fullEvents.append(OneShotTriggerEvent(timestamp, event))
+
+            fullEvents.sort(key=lambda e: e.getTimestamp())
+
             # Add the full path to the cache
             fullPath = PathPlannerPath([], PathConstraints(
                 float('inf'),
@@ -521,7 +557,6 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None:
             fullPathPoints = [PathPoint(state.pose.translation()) for state in fullTrajStates]
             fullPath._allPoints = fullPathPoints
             fullPath._isChoreoPath = True
-            fullEvents = []
             fullPath._idealTrajectory = PathPlannerTrajectory(None, None, None, None, states=fullTrajStates,
                                                               events=fullEvents)
             PathPlannerPath._choreoPathCache[trajectory_name] = fullPath
@@ -540,9 +575,15 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None:
                     splitEndIdx = int(splits[i + 1])
 
                 startTime = fullTrajStates[splitStartIdx].timeSeconds
+                endTime = fullTrajStates[splitEndIdx - 1].timeSeconds
                 for s in range(splitStartIdx, splitEndIdx):
                     states.append(fullTrajStates[s].copyWithTime(fullTrajStates[s].timeSeconds - startTime))
 
+                events: List[Event] = []
+                for originalEvent in fullEvents:
+                    if startTime <= originalEvent.getTimestamp() < endTime:
+                        events.append(originalEvent.copyWithTime(originalEvent.getTimestamp() - startTime))
+
                 path = PathPlannerPath([], PathConstraints(
                     float('inf'),
                     float('inf'),
@@ -552,7 +593,6 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None:
                 pathPoints = [PathPoint(state.pose.translation()) for state in states]
                 path._allPoints = pathPoints
                 path._isChoreoPath = True
-                events = []
                 path._idealTrajectory = PathPlannerTrajectory(None, None, None, None, states=states,
                                                               events=events)
                 PathPlannerPath._choreoPathCache[name] = path

pathplannerlib/src/main/java/com/pathplanner/lib/events/ActivateTriggerEvent.java

@@ -25,14 +25,13 @@ public void handleEvent(EventScheduler eventScheduler) {
     EventScheduler.setCondition(name, true);
   }
 
-  /**
-   * Cancel this event. This will be called if a path following command ends before this event gets
-   * handled.
-   *
-   * @param eventScheduler Reference to the EventScheduler handling this event
-   */
   @Override
   public void cancelEvent(EventScheduler eventScheduler) {
     // Do nothing
   }
+
+  @Override
+  public Event copyWithTimestamp(double timestamp) {
+    return new ActivateTriggerEvent(timestamp, name);
+  }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/events/CancelCommandEvent.java

@@ -22,14 +22,13 @@ public void handleEvent(EventScheduler eventScheduler) {
     eventScheduler.cancelCommand(command);
   }
 
-  /**
-   * Cancel this event. This will be called if a path following command ends before this event gets
-   * handled.
-   *
-   * @param eventScheduler Reference to the EventScheduler handling this event
-   */
   @Override
   public void cancelEvent(EventScheduler eventScheduler) {
     // Do nothing, the event scheduler will already cancel all commands
   }
+
+  @Override
+  public Event copyWithTimestamp(double timestamp) {
+    return new CancelCommandEvent(timestamp, command);
+  }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/events/DeactivateTriggerEvent.java

@@ -15,25 +15,19 @@ public DeactivateTriggerEvent(double timestamp, String name) {
     this.name = name;
   }
 
-  /**
-   * Handle this event
-   *
-   * @param eventScheduler Reference to the EventScheduler running this event
-   */
   @Override
   public void handleEvent(EventScheduler eventScheduler) {
     EventScheduler.setCondition(name, false);
   }
 
-  /**
-   * Cancel this event. This will be called if a path following command ends before this event gets
-   * handled.
-   *
-   * @param eventScheduler Reference to the EventScheduler handling this event
-   */
   @Override
   public void cancelEvent(EventScheduler eventScheduler) {
     // Ensure the condition gets set to false
     EventScheduler.setCondition(name, false);
   }
+
+  @Override
+  public Event copyWithTimestamp(double timestamp) {
+    return new DeactivateTriggerEvent(timestamp, name);
+  }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/events/Event.java

@@ -45,4 +45,12 @@ public void setTimestamp(double timestamp) {
    * @param eventScheduler Reference to the EventScheduler handling this event
    */
   public abstract void cancelEvent(EventScheduler eventScheduler);
+
+  /**
+   * Copy this event with a different timestamp
+   *
+   * @param timestamp The new timestamp
+   * @return Copied event with new time
+   */
+  public abstract Event copyWithTimestamp(double timestamp);
 }

pathplannerlib/src/main/java/com/pathplanner/lib/events/OneShotTriggerEvent.java

@@ -24,11 +24,6 @@ public OneShotTriggerEvent(double timestamp, String name) {
             .ignoringDisable(true);
   }
 
-  /**
-   * Handle this event
-   *
-   * @param eventScheduler Reference to the EventScheduler handling this event
-   */
   @Override
   public void handleEvent(EventScheduler eventScheduler) {
     EventScheduler.setCondition(name, true);
@@ -37,14 +32,13 @@ public void handleEvent(EventScheduler eventScheduler) {
     CommandScheduler.getInstance().schedule(resetCommand);
   }
 
-  /**
-   * Cancel this event. This will be called if a path following command ends before this event gets
-   * handled.
-   *
-   * @param eventScheduler Reference to the EventScheduler handling this event
-   */
   @Override
   public void cancelEvent(EventScheduler eventScheduler) {
     // Do nothing
   }
+
+  @Override
+  public Event copyWithTimestamp(double timestamp) {
+    return new OneShotTriggerEvent(timestamp, name);
+  }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/events/ScheduleCommandEvent.java

@@ -22,14 +22,13 @@ public void handleEvent(EventScheduler eventScheduler) {
     eventScheduler.scheduleCommand(command);
   }
 
-  /**
-   * Cancel this event. This will be called if a path following command ends before this event gets
-   * handled.
-   *
-   * @param eventScheduler Reference to the EventScheduler handling this event
-   */
   @Override
   public void cancelEvent(EventScheduler eventScheduler) {
     // Do nothing
   }
+
+  @Override
+  public Event copyWithTimestamp(double timestamp) {
+    return new ScheduleCommandEvent(timestamp, command);
+  }
 }

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

@@ -1,6 +1,10 @@
 package com.pathplanner.lib.path;
 
+import com.pathplanner.lib.auto.CommandUtil;
 import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.events.Event;
+import com.pathplanner.lib.events.OneShotTriggerEvent;
+import com.pathplanner.lib.events.ScheduleCommandEvent;
 import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
 import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState;
 import com.pathplanner.lib.util.DriveFeedforward;
@@ -14,6 +18,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj2.command.Command;
 import java.io.*;
 import java.util.*;
 import java.util.stream.Collectors;
@@ -353,6 +358,42 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName)
         fullTrajStates.add(state);
       }
 
+      List<Event> fullEvents = new ArrayList<>();
+      // Events from pplibCommands
+      for (var m : (JSONArray) json.get("pplibCommands")) {
+        JSONObject markerJson = (JSONObject) m;
+        JSONObject dataJson = (JSONObject) markerJson.get("data");
+        JSONObject offsetJson = (JSONObject) dataJson.get("offset");
+        JSONObject eventJson = (JSONObject) markerJson.get("event");
+
+        String name = (String) dataJson.get("name");
+        double targetTimestamp = ((Number) dataJson.get("targetTimestamp")).doubleValue();
+        double offset = ((Number) offsetJson.get("val")).doubleValue();
+        double timestamp = targetTimestamp + offset;
+        Command eventCommand = CommandUtil.commandFromJson(eventJson, true);
+
+        fullEvents.add(new OneShotTriggerEvent(timestamp, name));
+        fullEvents.add(new ScheduleCommandEvent(timestamp, eventCommand));
+      }
+
+      // Events from choreolib events
+      for (var m : (JSONArray) json.get("events")) {
+        JSONObject markerJson = (JSONObject) m;
+        JSONObject dataJson = (JSONObject) markerJson.get("data");
+        JSONObject offsetJson = (JSONObject) dataJson.get("offset");
+        JSONObject eventJson = (JSONObject) markerJson.get("event");
+        JSONObject eventDataJson = (JSONObject) eventJson.get("data");
+
+        String event = (String) eventDataJson.get("event");
+        double targetTimestamp = ((Number) dataJson.get("targetTimestamp")).doubleValue();
+        double offset = ((Number) offsetJson.get("val")).doubleValue();
+        double timestamp = targetTimestamp + offset;
+
+        fullEvents.add(new OneShotTriggerEvent(timestamp, event));
+      }
+
+      fullEvents.sort(Comparator.comparingDouble(Event::getTimestamp));
+
       // Add the full path to the cache
       PathPlannerPath fullPath = new PathPlannerPath();
       fullPath.globalConstraints =
@@ -373,8 +414,7 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName)
 
       fullPath.allPoints = fullPathPoints;
       fullPath.isChoreoPath = true;
-      fullPath.idealTrajectory =
-          Optional.of(new PathPlannerTrajectory(fullTrajStates, Collections.emptyList()));
+      fullPath.idealTrajectory = Optional.of(new PathPlannerTrajectory(fullTrajStates, fullEvents));
       choreoPathCache.put(trajectoryName, fullPath);
 
       JSONArray splits = (JSONArray) trajJson.get("splits");
@@ -393,11 +433,20 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName)
         }
 
         double startTime = fullTrajStates.get(splitStartIdx).timeSeconds;
+        double endTime = fullTrajStates.get(splitEndIdx - 1).timeSeconds;
         for (int s = splitStartIdx; s < splitEndIdx; s++) {
           states.add(
               fullTrajStates.get(s).copyWithTime(fullTrajStates.get(s).timeSeconds - startTime));
         }
 
+        List<Event> events = new ArrayList<>();
+        for (Event originalEvent : fullEvents) {
+          if (originalEvent.getTimestamp() >= startTime
+              && originalEvent.getTimestamp() <= endTime) {
+            events.add(originalEvent.copyWithTimestamp(originalEvent.getTimestamp() - startTime));
+          }
+        }
+
         PathPlannerPath path = new PathPlannerPath();
         path.globalConstraints =
             new PathConstraints(
@@ -417,8 +466,7 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName)
 
         path.allPoints = pathPoints;
         path.isChoreoPath = true;
-        path.idealTrajectory =
-            Optional.of(new PathPlannerTrajectory(states, Collections.emptyList()));
+        path.idealTrajectory = Optional.of(new PathPlannerTrajectory(states, events));
         choreoPathCache.put(name, path);
       }
     }