From 3fd383cc843c16eaf346306d80a90d6ec5b6e2e9 Mon Sep 17 00:00:00 2001
From: Michael Jansen <mjansen4857@gmail.com>
Date: Wed, 31 Jan 2024 13:21:21 -0500
Subject: [PATCH] Add support for loading choreo event markers

---
 pathplannerlib-python/pathplannerlib/path.py     | 11 +++++++++++
 .../pathplanner/lib/path/PathPlannerPath.java    | 16 ++++++++++++++++
 .../cpp/pathplanner/lib/path/PathPlannerPath.cpp | 14 ++++++++++++++
 3 files changed, 41 insertions(+)

diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py
index 9c105d68..323cc1de 100644
--- 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
index 5ee0da4b..3b8c96ce 100644
--- 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
index 60e33b53..a3d2f1b5 100644
--- 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;
 }