Convert event markers to be time based (#595)

* Change java event markers to be triggered based on time

* convert c++ event markers to time based

* convert python event markers to time based

pathplannerlib-python/pathplannerlib/commands.py

@@ -27,7 +27,7 @@ class FollowPathCommand(Command):
 
     # For event markers
     _currentEventCommands: dict = {}
-    _untriggeredMarkers: List[EventMarker] = []
+    _untriggeredEvents: List[Tuple[float, Command]] = []
 
     _timer: Timer = Timer()
     _path: PathPlannerPath = None
@@ -96,15 +96,11 @@ def initialize(self):
             PathPlannerLogging.logActivePath(self._path)
             PPLibTelemetry.setCurrentPath(self._path)
 
-        # Initialize markers
+        # Initialize marker stuff
         self._currentEventCommands.clear()
-
-        for marker in self._path.getEventMarkers():
-            marker.reset(currentPose)
-
-        self._untriggeredMarkers.clear()
-        for marker in self._path.getEventMarkers():
-            self._untriggeredMarkers.append(marker)
+        self._untriggeredEvents.clear()
+        for event in self._generatedTrajectory.getEventCommands():
+            self._untriggeredEvents.append(event)
 
         self._timer.reset()
         self._timer.start()
@@ -146,35 +142,33 @@ def execute(self):
 
         self._output(targetSpeeds)
 
-        # Execute event marker commands
-        for command in self._currentEventCommands:
-            if not self._currentEventCommands[command]:
-                continue
-
-            command.execute()
-
-            if command.isFinished():
-                command.end(False)
-                self._currentEventCommands[command] = False
-
-        toTrigger = [marker for marker in self._untriggeredMarkers if marker.shouldTrigger(currentPose)]
+        if len(self._untriggeredEvents) > 0 and self._timer.hasElapsed(self._untriggeredEvents[0][0]):
+            # Time to trigger this event command
+            cmd = self._untriggeredEvents.pop(0)[1]
 
-        for marker in toTrigger:
-            self._untriggeredMarkers.remove(marker)
-
-        for marker in toTrigger:
             for command in self._currentEventCommands:
                 if not self._currentEventCommands[command]:
                     continue
 
                 for req in command.getRequirements():
-                    if req in marker.command.getRequirements():
+                    if req in cmd.getRequirements():
                         command.end(True)
                         self._currentEventCommands[command] = False
                         break
 
-            marker.command.initialize()
-            self._currentEventCommands[marker.command] = True
+            cmd.initialize()
+            self._currentEventCommands[cmd] = True
+
+        # Execute event marker commands
+        for command in self._currentEventCommands:
+            if not self._currentEventCommands[command]:
+                continue
+
+            command.execute()
+
+            if command.isFinished():
+                command.end(False)
+                self._currentEventCommands[command] = False
 
     def isFinished(self) -> bool:
         return self._timer.hasElapsed(self._generatedTrajectory.getTotalTimeSeconds())
@@ -196,7 +190,7 @@ def end(self, interrupted: bool):
 
     def _replanPath(self, current_pose: Pose2d, current_speeds: ChassisSpeeds) -> None:
         replanned = self._path.replan(current_pose, current_speeds)
-        self._generatedTrajectory = PathPlannerTrajectory(replanned, current_speeds, current_pose.rotation())
+        self._generatedTrajectory = replanned.getTrajectory(current_speeds, current_pose.rotation())
         PathPlannerLogging.logActivePath(replanned)
         PPLibTelemetry.setCurrentPath(replanned)
 
@@ -353,7 +347,8 @@ def initialize(self):
         self._controller.reset(currentPose, self._speedsSupplier())
 
         if self._targetPath is not None:
-            self._originalTargetPose = Pose2d(self._targetPath.getPoint(0).position, self._originalTargetPose.rotation())
+            self._originalTargetPose = Pose2d(self._targetPath.getPoint(0).position,
+                                              self._originalTargetPose.rotation())
             if self._shouldFlipPath():
                 self._targetPose = flipFieldPose(self._originalTargetPose)
                 self._goalEndState = GoalEndState(self._goalEndState.velocity, self._targetPose.rotation(), True)
@@ -447,7 +442,7 @@ def execute(self):
 
                 if currentError >= self._replanningConfig.dynamicReplanningTotalErrorThreshold or currentError - previousError >= self._replanningConfig.dynamicReplanningErrorSpikeThreshold:
                     replanned = self._currentPath.replan(currentPose, currentSpeeds)
-                    self._currentTrajectory = PathPlannerTrajectory(replanned, currentSpeeds, currentPose.rotation())
+                    self._currentTrajectory = replanned.getTrajectory(currentSpeeds, currentPose.rotation())
                     PathPlannerLogging.logActivePath(replanned)
                     PPLibTelemetry.setCurrentPath(replanned)
 

pathplannerlib-python/pathplannerlib/path.py

@@ -198,13 +198,9 @@ class EventMarker:
     Args:
         waypointRelativePos (float): The waypoint relative position of the marker
         command (Command): The command that should be triggered at this marker
-        minimumTriggerDistance (float): The minimum distance the robot must be within for this marker to be triggered
     """
     waypointRelativePos: float
     command: Command
-    minimumTriggerDistance: float = 0.5
-    markerPos: Translation2d = None
-    lastRobotPos: Translation2d = None
 
     @staticmethod
     def fromJson(json_dict: dict) -> EventMarker:
@@ -213,34 +209,9 @@ def fromJson(json_dict: dict) -> EventMarker:
         command = CommandUtil.commandFromJson(json_dict['command'], False)
         return EventMarker(pos, command)
 
-    def reset(self, robot_pose: Pose2d) -> None:
-        """
-        Reset the current robot position
-
-        :param robot_pose: The current pose of the robot
-        """
-        self.lastRobotPos = robot_pose.translation()
-
-    def shouldTrigger(self, robot_pose: Pose2d) -> bool:
-        """
-        Get if this event marker should be triggered
-
-        :param robot_pose: Current pose of the robot
-        :return: True if this marker should be triggered
-        """
-        if self.lastRobotPos is None or self.markerPos is None:
-            self.lastRobotPos = robot_pose.translation()
-            return False
-
-        distanceToMarker = robot_pose.translation().distance(self.markerPos)
-        trigger = self.minimumTriggerDistance >= distanceToMarker > self.lastRobotPos.distance(self.markerPos)
-        self.lastRobotPos = robot_pose.translation()
-        return trigger
-
     def __eq__(self, other):
         return (isinstance(other, EventMarker)
                 and other.waypointRelativePos == self.waypointRelativePos
-                and other.minimumTriggerDistance == self.minimumTriggerDistance
                 and other.command == self.command)
 
 
@@ -453,18 +424,23 @@ def fromChoreoTrajectory(trajectory_name: str) -> PathPlannerPath:
 
             path._allPoints = pathPoints
             path._isChoreoPath = True
-            path._choreoTrajectory = PathPlannerTrajectory(None, None, None, states=trajStates)
 
-            if 'eventMarker' in trajJson:
+            eventCommands = []
+            if 'eventMarkers' in trajJson:
                 from .auto import CommandUtil
-                for m in trajJson['eventMarker']:
+                for m in trajJson['eventMarkers']:
                     timestamp = float(m['timestamp'])
                     cmd = CommandUtil.commandFromJson(m['command'], False)
 
                     eventMarker = EventMarker(timestamp, cmd)
-                    eventMarker.markerPos = path._choreoTrajectory.sample(timestamp).positionMeters
 
                     path._eventMarkers.append(eventMarker)
+                    eventCommands.append((timestamp, cmd))
+
+            eventCommands.sort(key=lambda a: a[0])
+
+            path._choreoTrajectory = PathPlannerTrajectory(None, None, None, states=trajStates,
+                                                           event_commands=eventCommands)
 
             return path
 
@@ -671,7 +647,7 @@ def replan(self, starting_pose: Pose2d, current_speeds: ChassisSpeeds) -> PathPl
                      self._rotationTargets],
                     [ConstraintsZone(z.minWaypointPos + 1, z.maxWaypointPos + 1, z.constraints) for z in
                      self._constraintZones],
-                    [EventMarker(m.waypointRelativePos + 1, m.command, m.minimumTriggerDistance) for m in
+                    [EventMarker(m.waypointRelativePos + 1, m.command) for m in
                      self._eventMarkers],
                     self._reversed,
                     self._previewStartingRotation
@@ -787,11 +763,11 @@ def replan(self, starting_pose: Pose2d, current_speeds: ChassisSpeeds) -> PathPl
         for m in self._eventMarkers:
             if m.waypointRelativePos >= nextWaypointIdx:
                 mappedMarkers.append(
-                    EventMarker(m.waypointRelativePos - nextWaypointIdx + 2, m.command, m.minimumTriggerDistance))
+                    EventMarker(m.waypointRelativePos - nextWaypointIdx + 2, m.command))
             elif m.waypointRelativePos >= nextWaypointIdx - 1:
                 pct = m.waypointRelativePos - (nextWaypointIdx - 1)
                 mappedMarkers.append(
-                    EventMarker(PathPlannerPath._mapPct(pct, segment1Pct), m.command, m.minimumTriggerDistance))
+                    EventMarker(PathPlannerPath._mapPct(pct, segment1Pct), m.command))
 
         # Throw out everything before nextWaypointIdx - 1, map everything from nextWaypointIdx -
         # 1 to nextWaypointIdx on to the 2 joining segments (waypoint rel pos within old segment = %
@@ -869,7 +845,7 @@ def flipPath(self) -> PathPlannerPath:
         newEndState = GoalEndState(self._goalEndState.velocity, flipFieldRotation(self._goalEndState.rotation),
                                    self._goalEndState.rotateFast)
         newPreviewRot = flipFieldRotation(self._previewStartingRotation)
-        newMarkers = [EventMarker(m.waypointRelativePos, m.command, m.minimumTriggerDistance) for m in
+        newMarkers = [EventMarker(m.waypointRelativePos, m.command) for m in
                       self._eventMarkers]
 
         return PathPlannerPath(newBezier, self._globalConstraints, newEndState, newRotTargets, self._constraintZones,
@@ -992,10 +968,6 @@ def _precalcValues(self) -> None:
                     point.distanceAlongPath = self.getPoint(i - 1).distanceAlongPath + (
                         self.getPoint(i - 1).position.distance(point.position))
 
-            for m in self._eventMarkers:
-                pointIndex = int(m.waypointRelativePos / RESOLUTION)
-                m.markerPos = self.getPoint(pointIndex).position
-
             self.getPoint(self.numPoints() - 1).rotationTarget = RotationTarget(-1, self._goalEndState.rotation,
                                                                                 self._goalEndState.rotateFast)
             self.getPoint(self.numPoints() - 1).maxV = self._goalEndState.velocity

pathplannerlib-python/pathplannerlib/trajectory.py

@@ -6,7 +6,8 @@
 from wpimath.kinematics import ChassisSpeeds
 from wpimath import inputModulus
 from .geometry_util import floatLerp, translationLerp, rotationLerp
-from typing import List, Union, TYPE_CHECKING
+from typing import List, Tuple, Union, TYPE_CHECKING
+from commands2 import Command
 
 if TYPE_CHECKING:
     from .path import PathPlannerPath, PathConstraints
@@ -106,9 +107,11 @@ def reverse(self) -> State:
 
 class PathPlannerTrajectory:
     _states: List[State]
+    _eventCommands: List[Tuple[float, Command]]
 
     def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[ChassisSpeeds, None],
-                 starting_rotation: Union[Rotation2d, None], states: List[State] = None):
+                 starting_rotation: Union[Rotation2d, None], states: List[State] = None,
+                 event_commands: List[Tuple[float, Command]] = None):
         """
         Generate a PathPlannerTrajectory. If "states" is provided, the other arguments can be None
 
@@ -120,11 +123,25 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch
 
         if states is not None:
             self._states = states
+            if event_commands is not None:
+                self._eventCommands = event_commands
+            else:
+                self._eventCommands = []
         else:
             if path.isChoreoPath():
-                self._states = path.getTrajectory(starting_speeds, starting_rotation).getStates()
+                traj = path.getTrajectory(starting_speeds, starting_rotation)
+                self._states = traj._states
+                self._eventCommands = traj._eventCommands
             else:
                 self._states = PathPlannerTrajectory._generateStates(path, starting_speeds, starting_rotation)
+                self._eventCommands = []
+
+                from .path import RESOLUTION
+                for m in path.getEventMarkers():
+                    pointIndex = int(m.waypointRelativePos / RESOLUTION)
+                    self._eventCommands.append((self._states[pointIndex].timeSeconds, m.command))
+
+                self._eventCommands.sort(key=lambda a: a[0])
 
     def getStates(self) -> List[State]:
         """
@@ -134,6 +151,14 @@ def getStates(self) -> List[State]:
         """
         return self._states
 
+    def getEventCommands(self) -> List[Tuple[float, Command]]:
+        """
+        Get all of the pairs of timestamps + commands to run at those timestamps
+
+        :return: Pairs of timestamps and event commands
+        """
+        return self._eventCommands
+
     def getState(self, index: int) -> State:
         """
         Get the goal state at the given index