diff --git a/pathplannerlib-python/pathplannerlib/auto.py b/pathplannerlib-python/pathplannerlib/auto.py index 575f3804..8871ebc8 100644 --- a/pathplannerlib-python/pathplannerlib/auto.py +++ b/pathplannerlib-python/pathplannerlib/auto.py @@ -109,8 +109,12 @@ def commandFromJson(command_json: dict, load_choreo_paths: bool) -> Command: @staticmethod def _waitCommandFromData(data_json: dict) -> Command: - waitTime = float(data_json['waitTime']) - return cmd.waitSeconds(waitTime) + waitJson = data_json['waitTime'] + if waitJson is dict: + # Choreo expression + return cmd.waitSeconds(float(waitJson['val'])) + else: + return cmd.waitSeconds(float(waitJson)) @staticmethod def _namedCommandFromData(data_json: dict) -> Command: diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index 964959d6..da619a32 100644 --- a/pathplannerlib-python/pathplannerlib/path.py +++ b/pathplannerlib-python/pathplannerlib/path.py @@ -545,36 +545,21 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None: fullTrajStates.append(state) fullEvents: List[Event] = [] - # Events from pplibCommands - if 'pplibCommands' in fJson: - for m in fJson['pplibCommands']: - dataJson = m['data'] - offsetJson = dataJson['offset'] - - name = str(dataJson['name']) - targetTimestamp = float(dataJson['targetTimestamp']) - offset = float(offsetJson['val']) - timestamp = targetTimestamp + offset - - if m['event'] is not None: - eventCommand = CommandUtil.commandFromJson(m['event'], True) - fullEvents.append(ScheduleCommandEvent(timestamp, eventCommand)) - - fullEvents.append(OneShotTriggerEvent(timestamp, name)) - - # Events from choreolib events if 'events' in fJson: - for m in fJson['events']: - dataJson = m['data'] - offsetJson = dataJson['offset'] + for markerJson in fJson['events']: + name = str(markerJson['name']) - name = str(dataJson['name']) - targetTimestamp = float(dataJson['targetTimestamp']) - offset = float(offsetJson['val']) - timestamp = targetTimestamp + offset + fromJson = markerJson['from'] + fromOffsetJson = fromJson['offset'] + fromTargetTimestamp = float(fromJson['targetTimestamp']) + fromOffset = float(fromOffsetJson['val']) + fromTimestamp = fromTargetTimestamp + fromOffset - fullEvents.append(OneShotTriggerEvent(timestamp, name)) + fullEvents.append(OneShotTriggerEvent(fromTimestamp, name)) + if markerJson['event'] is not None: + eventCommand = CommandUtil.commandFromJson(markerJson['event'], True) + fullEvents.append(ScheduleCommandEvent(fromTimestamp, eventCommand)) fullEvents.sort(key=lambda e: e.getTimestamp()) # Add the full path to the cache diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/CommandUtil.java b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/CommandUtil.java index d81ee7f1..70aae80b 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/CommandUtil.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/CommandUtil.java @@ -53,8 +53,15 @@ public static Command commandFromJson(JSONObject commandJson, boolean loadChoreo } private static Command waitCommandFromData(JSONObject dataJson) { - double waitTime = ((Number) dataJson.get("waitTime")).doubleValue(); - return Commands.waitSeconds(waitTime); + try { + double waitTime = ((Number) dataJson.get("waitTime")).doubleValue(); + return Commands.waitSeconds(waitTime); + } catch (Exception ignored) { + // Failed to load wait time as a number. This is probably a choreo expression + JSONObject waitTimeJson = (JSONObject) dataJson.get("waitTime"); + double waitTime = ((Number) waitTimeJson.get("val")).doubleValue(); + return Commands.waitSeconds(waitTime); + } } private static Command namedCommandFromData(JSONObject dataJson) { 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 b0e190c5..05a3a827 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -381,43 +381,24 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName) } List fullEvents = new ArrayList<>(); - // Events from pplibCommands - if (json.containsKey("pplibCommands")) { - for (var m : (JSONArray) json.get("pplibCommands")) { - JSONObject markerJson = (JSONObject) m; - JSONObject dataJson = (JSONObject) markerJson.get("data"); - JSONObject offsetJson = (JSONObject) dataJson.get("offset"); - - String name = (String) dataJson.get("name"); - double targetTimestamp = ((Number) dataJson.get("targetTimestamp")).doubleValue(); - double offset = ((Number) offsetJson.get("val")).doubleValue(); - double timestamp = targetTimestamp + offset; - if (markerJson.get("event") != null) { - Command eventCommand = - CommandUtil.commandFromJson((JSONObject) markerJson.get("event"), true); - fullEvents.add(new ScheduleCommandEvent(timestamp, eventCommand)); - } - - fullEvents.add(new OneShotTriggerEvent(timestamp, name)); - } - } - - // Events from choreolib events - if (json.containsKey("events")) { - for (var m : (JSONArray) json.get("events")) { - JSONObject markerJson = (JSONObject) m; - JSONObject dataJson = (JSONObject) markerJson.get("data"); - JSONObject offsetJson = (JSONObject) dataJson.get("offset"); - - String name = (String) dataJson.get("name"); - double targetTimestamp = ((Number) dataJson.get("targetTimestamp")).doubleValue(); - double offset = ((Number) offsetJson.get("val")).doubleValue(); - double timestamp = targetTimestamp + offset; - - fullEvents.add(new OneShotTriggerEvent(timestamp, name)); + for (var m : (JSONArray) json.get("events")) { + JSONObject markerJson = (JSONObject) m; + String name = (String) markerJson.get("name"); + + JSONObject fromJson = (JSONObject) markerJson.get("from"); + JSONObject fromOffsetJson = (JSONObject) fromJson.get("offset"); + double fromTargetTimestamp = ((Number) fromJson.get("targetTimestamp")).doubleValue(); + double fromOffset = ((Number) fromOffsetJson.get("val")).doubleValue(); + double fromTimestamp = fromTargetTimestamp + fromOffset; + + fullEvents.add(new OneShotTriggerEvent(fromTimestamp, name)); + + if (markerJson.get("event") != null) { + Command eventCommand = + CommandUtil.commandFromJson((JSONObject) markerJson.get("event"), true); + fullEvents.add(new ScheduleCommandEvent(fromTimestamp, eventCommand)); } } - fullEvents.sort(Comparator.comparingDouble(Event::getTimestamp)); // Add the full path to the cache diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/CommandUtil.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/CommandUtil.cpp index ee396a24..3f3a4e94 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/CommandUtil.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/CommandUtil.cpp @@ -53,8 +53,14 @@ frc2::CommandPtr CommandUtil::commandFromJson(const wpi::json &json, } frc2::CommandPtr CommandUtil::waitCommandFromJson(const wpi::json &json) { - auto waitTime = units::second_t(json.at("waitTime").get()); - return frc2::cmd::Wait(waitTime); + auto waitJson = json.at("waitTime"); + if (waitJson.is_number()) { + return frc2::cmd::Wait(units::second_t { waitJson.get() }); + } else { + // Field is not a number, probably a choreo expression + return frc2::cmd::Wait(units::second_t { + waitJson.at("val").get() }); + } } frc2::CommandPtr CommandUtil::namedCommandFromJson(const wpi::json &json) { 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 a61c018b..99845e8f 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -193,50 +193,33 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache( } std::vector < std::shared_ptr < Event >> fullEvents; - // Events from pplibCommands - if (json.contains("pplibCommands")) { - for (wpi::json::const_reference m : json.at("pplibCommands")) { - auto dataJson = m.at("data"); - auto offsetJson = dataJson.at("offset"); - - std::string name = dataJson.at("name").get(); - units::second_t targetTimestamp { - dataJson.at("targetTimestamp").get() }; - units::second_t offset { offsetJson.at("val").get() }; - units::second_t timestamp = targetTimestamp + offset; + if (json.contains("events")) { + for (wpi::json::const_reference markerJson : json.at("events")) { + std::string name = markerJson.at("name").get(); - frc2::CommandPtr eventCommand = frc2::cmd::None(); - if (!m.at("event").is_null()) { - eventCommand = CommandUtil::commandFromJson(m.at("event"), - true); - } + auto fromJson = markerJson.at("from"); + auto fromOffsetJson = markerJson.at("offset"); + + units::second_t fromTargetTimestamp { + fromJson.at("targetTimestamp").get() }; + units::second_t fromOffset { fromOffsetJson.at("val").get() }; + units::second_t fromTimestamp = fromTargetTimestamp + fromOffset; fullEvents.emplace_back( - std::make_shared < OneShotTriggerEvent > (timestamp, name)); + std::make_shared < OneShotTriggerEvent + > (fromTimestamp, name)); + + frc2::CommandPtr eventCommand = frc2::cmd::None(); + if (!markerJson.at("event").is_null()) { + eventCommand = CommandUtil::commandFromJson( + markerJson.at("event"), true); + } fullEvents.emplace_back( std::make_shared < ScheduleCommandEvent - > (timestamp, std::shared_ptr < frc2::Command + > (fromTimestamp, std::shared_ptr < frc2::Command > (std::move(eventCommand).Unwrap()))); } } - - // Events from choreolib events - if (json.contains("events")) { - for (wpi::json::const_reference m : json.at("events")) { - auto dataJson = m.at("data"); - auto offsetJson = dataJson.at("offset"); - - std::string name = dataJson.at("name").get(); - units::second_t targetTimestamp { - dataJson.at("targetTimestamp").get() }; - units::second_t offset { offsetJson.at("val").get() }; - units::second_t timestamp = targetTimestamp + offset; - - fullEvents.emplace_back( - std::make_shared < OneShotTriggerEvent > (timestamp, name)); - } - } - std::sort(fullEvents.begin(), fullEvents.end(), [](auto &left, auto &right) { return left->getTimestamp() < right->getTimestamp();