Filter event markers with null timestamps or empty names (#862)

SleipnirGroup · Oct 12, 2024 · 97aae70 · 97aae70
1 parent ac0e902
commit 97aae70
Show file tree

Hide file tree

Showing 6 changed files with 65 additions and 23 deletions.
diff --git 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
@@ -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
@@ -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<? extends TrajectorySample<?>> 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<EventMarker> unfilteredEvents =
+        new ArrayList<EventMarker>(
+            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
@@ -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
@@ -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<double>() +
-                      json.at("from").at("targetTimestamp").get<double>()};
-  event.event = json.at("name").get<std::string>();
+  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<double>() +
+                        targetTimestamp.get<double>()};
+    event.event = json.at("name").get<std::string>();
+  }
 }
diff --git 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<std::vector<EventMarker>>();
+  auto events = json.at("events").get<std::vector<EventMarker>>();
+  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<std::vector<EventMarker>>();
+  auto events = json.at("events").get<std::vector<EventMarker>>();
+  trajectory.events.clear();
+  for (EventMarker event : events) {
+    if (event.timestamp >= units::second_t{0} || event.event.size() == 0) {
+      trajectory.events.push_back(event);
+    }
+  }
 }