From 97aae7047e2556073bc577a4f116330e162805df Mon Sep 17 00:00:00 2001 From: shueja <32416547+shueja@users.noreply.github.com> Date: Fri, 11 Oct 2024 22:18:13 -0700 Subject: [PATCH] Filter event markers with null timestamps or empty names (#862) --- choreolib/py/choreo/__init__.py | 28 +++++++++---------- choreolib/py/choreo/trajectory/__init__.py | 12 ++++++++ choreolib/src/main/java/choreo/Choreo.java | 11 +++++++- .../java/choreo/trajectory/EventMarker.java | 7 +++-- .../cpp/choreo/trajectory/EventMarker.cpp | 14 +++++++--- .../cpp/choreo/trajectory/Trajectory.cpp | 16 +++++++++-- 6 files changed, 65 insertions(+), 23 deletions(-) diff --git a/choreolib/py/choreo/__init__.py b/choreolib/py/choreo/__init__.py index ff45f99d5..5cb4d4507 100644 --- a/choreolib/py/choreo/__init__.py +++ b/choreolib/py/choreo/__init__.py @@ -1,5 +1,6 @@ import json +from choreo.trajectory import load_event_marker from choreo.trajectory import EventMarker from choreo.trajectory import DifferentialSample from choreo.trajectory import DifferentialTrajectory @@ -39,14 +40,15 @@ def load_differential_trajectory_string( for sample in data["trajectory"]["samples"] ] splits = [int(split) for split in data["trajectory"]["splits"]] - events = [ - EventMarker( - float(event["from"]["offset"]["val"]) - + float(event["from"]["targetTimestamp"]), - event["name"], + # Add 0 as the first split index + if len(splits) == 0 or splits[0] != 0: + splits.insert(0, 0) + events = list( + filter( + lambda marker: marker is not None, + [load_event_marker(event) for event in data["events"]], ) - for event in data["events"] - ] + ) return DifferentialTrajectory(data["name"], samples, splits, events) @@ -97,14 +99,12 @@ def load_swerve_trajectory_string(trajectory_json_string: str) -> SwerveTrajecto # Add 0 as the first split index if len(splits) == 0 or splits[0] != 0: splits.insert(0, 0) - events = [ - EventMarker( - float(event["from"]["offset"]["val"]) - + float(event["from"]["targetTimestamp"]), - event["name"], + events = list( + filter( + lambda marker: marker is not None, + [load_event_marker(event) for event in data["events"]], ) - for event in data["events"] - ] + ) return SwerveTrajectory(data["name"], samples, splits, events) diff --git a/choreolib/py/choreo/trajectory/__init__.py b/choreolib/py/choreo/trajectory/__init__.py index 9b7f2fba1..fd98dcf43 100644 --- a/choreolib/py/choreo/trajectory/__init__.py +++ b/choreolib/py/choreo/trajectory/__init__.py @@ -49,6 +49,18 @@ def __eq__(self, other: EventMarker) -> bool: return self.timestamp == other.timestamp and self.event == other.event +def load_event_marker(event) -> EventMarker | None: + try: + offset = float(event["from"]["offset"]["val"]) + target_timestamp = float(event["from"]["targetTimestamp"]) + name = event["name"] + if target_timestamp + offset < 0 or len(name) == 0: + return None + return EventMarker(target_timestamp + offset, name) + except TypeError: + return None + + class DifferentialSample: def __init__( self, diff --git a/choreolib/src/main/java/choreo/Choreo.java b/choreolib/src/main/java/choreo/Choreo.java index 92a25b64b..3539c9489 100644 --- a/choreolib/src/main/java/choreo/Choreo.java +++ b/choreolib/src/main/java/choreo/Choreo.java @@ -29,6 +29,8 @@ import java.io.FileNotFoundException; import java.io.FileReader; import java.io.IOException; +import java.util.ArrayList; +import java.util.Arrays; import java.util.HashMap; import java.util.List; import java.util.Map; @@ -162,7 +164,14 @@ static Trajectory> loadTrajectoryString( throw new RuntimeException( name + ".traj: Wrong version: " + version + ". Expected " + SPEC_VERSION); } - EventMarker[] events = GSON.fromJson(wholeTrajectory.get("events"), EventMarker[].class); + // Filter out markers with negative timestamps or empty names + List unfilteredEvents = + new ArrayList( + Arrays.asList(GSON.fromJson(wholeTrajectory.get("events"), EventMarker[].class))); + unfilteredEvents.removeIf(marker -> marker.timestamp < 0 || marker.event.length() == 0); + EventMarker[] events = new EventMarker[unfilteredEvents.size()]; + unfilteredEvents.toArray(events); + JsonObject trajectoryObj = wholeTrajectory.getAsJsonObject("trajectory"); Integer[] splits = GSON.fromJson(trajectoryObj.get("splits"), Integer[].class); if (splits.length == 0 || splits[0] != 0) { diff --git a/choreolib/src/main/java/choreo/trajectory/EventMarker.java b/choreolib/src/main/java/choreo/trajectory/EventMarker.java index 66e384cfd..c2204aab9 100644 --- a/choreolib/src/main/java/choreo/trajectory/EventMarker.java +++ b/choreolib/src/main/java/choreo/trajectory/EventMarker.java @@ -49,8 +49,11 @@ public EventMarker deserialize( var event = json.getAsJsonObject().get("name").getAsString(); return new EventMarker(targetTimestamp + offset, event); - } catch (IllegalStateException e) { - return new EventMarker(0, ""); + } catch (IllegalStateException + | UnsupportedOperationException + | NullPointerException + | NumberFormatException e) { + return new EventMarker(-1, ""); } } } diff --git a/choreolib/src/main/native/cpp/choreo/trajectory/EventMarker.cpp b/choreolib/src/main/native/cpp/choreo/trajectory/EventMarker.cpp index df37bd1f4..c85ac5a1e 100644 --- a/choreolib/src/main/native/cpp/choreo/trajectory/EventMarker.cpp +++ b/choreolib/src/main/native/cpp/choreo/trajectory/EventMarker.cpp @@ -14,8 +14,14 @@ void choreo::to_json(wpi::json& json, const EventMarker& event) { } void choreo::from_json(const wpi::json& json, EventMarker& event) { - event.timestamp = - units::second_t{json.at("from").at("offset").at("val").get() + - json.at("from").at("targetTimestamp").get()}; - event.event = json.at("name").get(); + auto targetTimestamp = json.at("from").at("targetTimestamp"); + if (!targetTimestamp.is_number()) { + event.timestamp = units::second_t{-1}; + event.event = ""; + } else { + event.timestamp = + units::second_t{json.at("from").at("offset").at("val").get() + + targetTimestamp.get()}; + event.event = json.at("name").get(); + } } diff --git a/choreolib/src/main/native/cpp/choreo/trajectory/Trajectory.cpp b/choreolib/src/main/native/cpp/choreo/trajectory/Trajectory.cpp index 7fd502caa..142af6b0b 100644 --- a/choreolib/src/main/native/cpp/choreo/trajectory/Trajectory.cpp +++ b/choreolib/src/main/native/cpp/choreo/trajectory/Trajectory.cpp @@ -27,7 +27,13 @@ void choreo::from_json(const wpi::json& json, if (trajectory.splits.size() == 0 || trajectory.splits.at(0) != 0) { trajectory.splits.insert(trajectory.splits.begin(), 0); } - trajectory.events = json.at("events").get>(); + auto events = json.at("events").get>(); + trajectory.events.clear(); + for (EventMarker event : events) { + if (event.timestamp >= units::second_t{0} || event.event.size() == 0) { + trajectory.events.push_back(event); + } + } } void choreo::to_json(wpi::json& json, @@ -49,5 +55,11 @@ void choreo::from_json(const wpi::json& json, if (trajectory.splits.size() == 0 || trajectory.splits.at(0) != 0) { trajectory.splits.insert(trajectory.splits.begin(), 0); } - trajectory.events = json.at("events").get>(); + auto events = json.at("events").get>(); + trajectory.events.clear(); + for (EventMarker event : events) { + if (event.timestamp >= units::second_t{0} || event.event.size() == 0) { + trajectory.events.push_back(event); + } + } }