diff --git a/pathplannerlib-python/pathplannerlib/events.py b/pathplannerlib-python/pathplannerlib/events.py index ab1ea273..ac82f071 100644 --- a/pathplannerlib-python/pathplannerlib/events.py +++ b/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 diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index 95b0a37c..09eaf167 100644 --- a/pathplannerlib-python/pathplannerlib/path.py +++ b/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 diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/events/ActivateTriggerEvent.java b/pathplannerlib/src/main/java/com/pathplanner/lib/events/ActivateTriggerEvent.java index 399ac42f..980def27 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/events/ActivateTriggerEvent.java +++ b/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); + } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/events/CancelCommandEvent.java b/pathplannerlib/src/main/java/com/pathplanner/lib/events/CancelCommandEvent.java index 7f204244..19beb07e 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/events/CancelCommandEvent.java +++ b/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); + } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/events/DeactivateTriggerEvent.java b/pathplannerlib/src/main/java/com/pathplanner/lib/events/DeactivateTriggerEvent.java index 3b1d57c8..88dfce55 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/events/DeactivateTriggerEvent.java +++ b/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); + } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/events/Event.java b/pathplannerlib/src/main/java/com/pathplanner/lib/events/Event.java index 0ffd6c20..0d48ead4 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/events/Event.java +++ b/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); } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/events/OneShotTriggerEvent.java b/pathplannerlib/src/main/java/com/pathplanner/lib/events/OneShotTriggerEvent.java index c67a0e8c..79b26c4d 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/events/OneShotTriggerEvent.java +++ b/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); + } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/events/ScheduleCommandEvent.java b/pathplannerlib/src/main/java/com/pathplanner/lib/events/ScheduleCommandEvent.java index 28a3b894..0ce4e6ba 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/events/ScheduleCommandEvent.java +++ b/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); + } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java index f78046a6..e7bcd3f0 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/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 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 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); } } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp index 6f9fa21d..40f9db3c 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -3,6 +3,8 @@ #include "pathplanner/lib/util/PPLibTelemetry.h" #include "pathplanner/lib/auto/CommandUtil.h" #include "pathplanner/lib/events/Event.h" +#include "pathplanner/lib/events/OneShotTriggerEvent.h" +#include "pathplanner/lib/events/ScheduleCommandEvent.h" #include "pathplanner/lib/util/FlippingUtil.h" #include #include @@ -165,6 +167,50 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache( fullTrajStates.emplace_back(state); } + std::vector < std::shared_ptr < Event >> fullEvents; + // Events from pplibCommands + for (wpi::json::const_reference m : json.at("pplibCommands")) { + auto dataJson = m.at("data"); + auto offsetJson = dataJson.at("offset"); + auto eventJson = m.at("event"); + + std::string name = dataJson.at("name").get(); + units::second_t targetTimestamp { dataJson.at("targetTimestamp").get< + double>() }; + units::second_t offset { offsetJson.at("val").get() }; + units::second_t timestamp = targetTimestamp + offset; + auto eventCommand = std::shared_ptr < frc2::Command + > (CommandUtil::commandFromJson(eventJson, true).Unwrap()); + + fullEvents.emplace_back( + std::make_shared < OneShotTriggerEvent > (timestamp, name)); + fullEvents.emplace_back( + std::make_shared < ScheduleCommandEvent + > (timestamp, eventCommand)); + } + + // Events from choreolib events + for (wpi::json::const_reference m : json.at("events")) { + auto dataJson = m.at("data"); + auto offsetJson = dataJson.at("offset"); + auto eventJson = m.at("event"); + auto eventDataJson = eventJson.at("data"); + + std::string event = eventDataJson.at("event").get(); + units::second_t targetTimestamp { dataJson.at("targetTimestamp").get< + double>() }; + units::second_t offset { offsetJson.at("val").get() }; + units::second_t timestamp = targetTimestamp + offset; + + fullEvents.emplace_back( + std::make_shared < OneShotTriggerEvent > (timestamp, event)); + } + + std::sort(fullEvents.begin(), fullEvents.end(), + [](auto &left, auto &right) { + return left->getTimestamp() < right->getTimestamp(); + }); + // Add the full path to the cache auto fullPath = std::make_shared < PathPlannerPath > (PathConstraints( @@ -185,7 +231,6 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache( fullPath->m_allPoints = fullPathPoints; fullPath->m_isChoreoPath = true; - std::vector < std::shared_ptr < Event >> fullEvents; fullPath->m_idealTrajectory = PathPlannerTrajectory(fullTrajStates, fullEvents); PathPlannerPath::getChoreoPathCache().emplace(trajectoryName, fullPath); @@ -208,12 +253,23 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache( } auto startTime = fullTrajStates[splitStartIdx].time; + auto endTime = fullTrajStates[splitEndIdx - 1].time; for (size_t s = splitStartIdx; s < splitEndIdx; s++) { states.emplace_back( fullTrajStates[s].copyWithTime( fullTrajStates[s].time - startTime)); } + std::vector < std::shared_ptr < Event >> events; + for (auto originalEvent : fullEvents) { + if (originalEvent->getTimestamp() >= startTime + && originalEvent->getTimestamp() < endTime) { + events.emplace_back( + originalEvent->copyWithTimestamp( + originalEvent->getTimestamp() - startTime)); + } + } + auto path = std::make_shared < PathPlannerPath > (PathConstraints(units::meters_per_second_t { @@ -234,7 +290,6 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache( path->m_allPoints = pathPoints; path->m_isChoreoPath = true; - std::vector < std::shared_ptr < Event >> events; path->m_idealTrajectory = PathPlannerTrajectory(states, events); PathPlannerPath::getChoreoPathCache().emplace(name, path); } diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/events/ActivateTriggerEvent.h b/pathplannerlib/src/main/native/include/pathplanner/lib/events/ActivateTriggerEvent.h index 7f4e50dc..86ba52cf 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/events/ActivateTriggerEvent.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/events/ActivateTriggerEvent.h @@ -25,6 +25,11 @@ class ActivateTriggerEvent: public Event { // Do nothing } + inline std::shared_ptr copyWithTimestamp(units::second_t timestamp) + override { + return std::make_shared < ActivateTriggerEvent > (timestamp, m_name); + } + private: std::string m_name; }; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/events/CancelCommandEvent.h b/pathplannerlib/src/main/native/include/pathplanner/lib/events/CancelCommandEvent.h index 7a6bd628..6b8d4d01 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/events/CancelCommandEvent.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/events/CancelCommandEvent.h @@ -27,6 +27,11 @@ class CancelCommandEvent: public Event { // Do nothing } + inline std::shared_ptr copyWithTimestamp(units::second_t timestamp) + override { + return std::make_shared < CancelCommandEvent > (timestamp, m_command); + } + private: std::shared_ptr m_command; }; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/events/DeactivateTriggerEvent.h b/pathplannerlib/src/main/native/include/pathplanner/lib/events/DeactivateTriggerEvent.h index 7999ced5..bf5fe4f3 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/events/DeactivateTriggerEvent.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/events/DeactivateTriggerEvent.h @@ -26,6 +26,11 @@ class DeactivateTriggerEvent: public Event { EventScheduler::setCondition(m_name, false); } + inline std::shared_ptr copyWithTimestamp(units::second_t timestamp) + override { + return std::make_shared < DeactivateTriggerEvent > (timestamp, m_name); + } + private: std::string m_name; }; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/events/Event.h b/pathplannerlib/src/main/native/include/pathplanner/lib/events/Event.h index 18c70173..e1bafffc 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/events/Event.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/events/Event.h @@ -1,6 +1,7 @@ #pragma once #include +#include namespace pathplanner { @@ -52,6 +53,15 @@ class Event { */ virtual void cancelEvent(EventScheduler *eventScheduler) = 0; + /** + * Copy this event with a different timestamp + * + * @param timestamp The new timestamp + * @return Copied event with new time + */ + virtual std::shared_ptr copyWithTimestamp( + units::second_t timestamp) = 0; + private: units::second_t m_timestamp; }; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/events/OneShotTriggerEvent.h b/pathplannerlib/src/main/native/include/pathplanner/lib/events/OneShotTriggerEvent.h index 8fb74775..7850ced6 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/events/OneShotTriggerEvent.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/events/OneShotTriggerEvent.h @@ -35,6 +35,11 @@ class OneShotTriggerEvent: public Event { // Do nothing } + inline std::shared_ptr copyWithTimestamp(units::second_t timestamp) + override { + return std::make_shared < OneShotTriggerEvent > (timestamp, m_name); + } + private: std::string m_name; frc2::CommandPtr m_resetCommand; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/events/ScheduleCommandEvent.h b/pathplannerlib/src/main/native/include/pathplanner/lib/events/ScheduleCommandEvent.h index 592eea85..c3d21b38 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/events/ScheduleCommandEvent.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/events/ScheduleCommandEvent.h @@ -27,6 +27,11 @@ class ScheduleCommandEvent: public Event { // Do nothing } + inline std::shared_ptr copyWithTimestamp(units::second_t timestamp) + override { + return std::make_shared < ScheduleCommandEvent > (timestamp, m_command); + } + private: std::shared_ptr m_command; };