Add support for loading choreo event markers

mjansen4857 · Jan 31, 2024 · 3fd383c · 3fd383c
1 parent 6234067
commit 3fd383c
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 0 deletions.
diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py
@@ -455,6 +455,17 @@ def fromChoreoTrajectory(trajectory_name: str) -> PathPlannerPath:
             path._isChoreoPath = True
             path._choreoTrajectory = PathPlannerTrajectory(None, None, None, states=trajStates)
 
+            if 'eventMarker' in trajJson:
+                from .auto import CommandUtil
+                for m in trajJson['eventMarker']:
+                    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)
+
             return path
 
     @staticmethod

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java
@@ -1,5 +1,6 @@
 package com.pathplanner.lib.path;
 
+import com.pathplanner.lib.auto.CommandUtil;
 import com.pathplanner.lib.util.GeometryUtil;
 import com.pathplanner.lib.util.PPLibTelemetry;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
@@ -10,6 +11,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.BufferedReader;
 import java.io.File;
 import java.io.FileReader;
@@ -377,6 +379,20 @@ public static PathPlannerPath fromChoreoTrajectory(String trajectoryName) {
       path.isChoreoPath = true;
       path.choreoTrajectory = new PathPlannerTrajectory(trajStates);
 
+      if (json.containsKey("eventMarkers")) {
+        for (var m : (JSONArray) json.get("eventMarkers")) {
+          JSONObject marker = (JSONObject) m;
+
+          double timestamp = ((Number) marker.get("timestamp")).doubleValue();
+          Command cmd = CommandUtil.commandFromJson((JSONObject) marker.get("command"), false);
+
+          EventMarker eventMarker = new EventMarker(timestamp, cmd);
+          eventMarker.markerPos = path.choreoTrajectory.sample(timestamp).positionMeters;
+
+          path.eventMarkers.add(eventMarker);
+        }
+      }
+
       return path;
     } catch (Exception e) {
       throw new RuntimeException(e);

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp
@@ -1,6 +1,7 @@
 #include "pathplanner/lib/path/PathPlannerPath.h"
 #include "pathplanner/lib/util/GeometryUtil.h"
 #include "pathplanner/lib/util/PPLibTelemetry.h"
+#include "pathplanner/lib/auto/CommandUtil.h"
 #include <frc/Filesystem.h>
 #include <frc/MathUtil.h>
 #include <wpi/MemoryBuffer.h>
@@ -198,6 +199,19 @@ std::shared_ptr<PathPlannerPath> PathPlannerPath::fromChoreoTrajectory(
 	path->m_isChoreoPath = true;
 	path->m_choreoTrajectory = PathPlannerTrajectory(trajStates);
 
+	if (json.contains("eventMarkers")) {
+		for (wpi::json::const_reference m : json.at("eventMarkers")) {
+			units::second_t timestamp { m.at("timestamp").get<double>() };
+
+			EventMarker eventMarker = EventMarker(timestamp(),
+					CommandUtil::commandFromJson(json.at("command"), false));
+			eventMarker.setMarkerPosition(
+					path->m_choreoTrajectory.sample(timestamp).position);
+
+			path->m_eventMarkers.emplace_back(eventMarker);
+		}
+	}
+
 	return path;
 }