Add support for choreo trajectories (#271)

* Add support for choreo trajectories Signed-off-by: Jade Turner <spacey-sooty@proton.me> * fix some bugs Signed-off-by: Jade Turner <spacey-sooty@proton.me> * review Signed-off-by: Jade Turner <spacey-sooty@proton.me> * 3d field Signed-off-by: Jade Turner <spacey-sooty@proton.me> * remove support from non trajectory keys Signed-off-by: Jade Turner <spacey-sooty@proton.me> --------- Signed-off-by: Jade Turner <spacey-sooty@proton.me>
Mechanical-Advantage · Dec 9, 2024 · 77377c4 · 77377c4
1 parent b6c0c17
commit 77377c4
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 1 deletion.
diff --git a/src/hub/controllers/OdometryController_Config.ts b/src/hub/controllers/OdometryController_Config.ts
@@ -348,7 +348,9 @@ const OdometryController_Config: SourceListConfig = {
         "Transform3d[]",
         "Translation2d[]",
         "Translation3d[]",
-        "Trajectory"
+        "Trajectory",
+        "DifferentialSample[]",
+        "SwerveSample[]"
       ],
       showDocs: true,
       options: [
@@ -501,6 +503,8 @@ const OdometryController_Config: SourceListConfig = {
         "Transform3d",
         "Transform2d[]",
         "Transform3d[]",
+        "DifferentialSample[]",
+        "SwerveSample[]",
         "Trajectory"
       ],
       showDocs: true,

diff --git a/src/hub/controllers/ThreeDimensionController_Config.ts b/src/hub/controllers/ThreeDimensionController_Config.ts
@@ -446,6 +446,8 @@ const ThreeDimensionController_Config: SourceListConfig = {
         "Transform3d[]",
         "Translation2d[]",
         "Translation3d[]",
+        "SwerveSample[]",
+        "DifferentialSample[]",
         "Trajectory"
       ],
       showDocs: true,

diff --git a/src/shared/geometry.ts b/src/shared/geometry.ts
@@ -214,6 +214,9 @@ export function grabPosesAuto(
       } else {
         return [];
       }
+    case "DifferentialSample[]":
+    case "SwerveSample[]":
+      return grabChoreoSampleArray(log, key, timestamp, uuid);
     default:
       return [];
   }
@@ -438,6 +441,21 @@ export function grabPose2d(log: Log, key: string, timestamp: number, uuid?: stri
   ];
 }
 
+export function grabChoreoSample(log: Log, key: string, timestamp: number, uuid?: string): AnnotatedPose3d[] {
+  return [
+    {
+      pose: pose2dTo3d({
+        translation: [
+          getOrDefault(log, key + "/pose/translation/x", LoggableType.Number, timestamp, 0, uuid),
+          getOrDefault(log, key + "/pose/translation/y", LoggableType.Number, timestamp, 0, uuid)
+        ],
+        rotation: getOrDefault(log, key + "/pose/rotation/value", LoggableType.Number, timestamp, 0, uuid)
+      }),
+      annotation: { is2DSource: true }
+    }
+  ];
+}
+
 export function grabPose3d(log: Log, key: string, timestamp: number, uuid?: string): AnnotatedPose3d[] {
   return [
     {
@@ -466,6 +484,13 @@ export function grabPose2dArray(log: Log, key: string, timestamp: number, uuid?:
   );
 }
 
+export function grabChoreoSampleArray(log: Log, key: string, timestamp: number, uuid?: string): AnnotatedPose3d[] {
+  return indexArray(getOrDefault(log, key + "/length", LoggableType.Number, timestamp, 0, uuid)).reduce(
+    (array, index) => array.concat(grabChoreoSample(log, key + "/" + index.toString(), timestamp)),
+    [] as AnnotatedPose3d[]
+  );
+}
+
 export function grabPose3dArray(log: Log, key: string, timestamp: number, uuid?: string): AnnotatedPose3d[] {
   return indexArray(getOrDefault(log, key + "/length", LoggableType.Number, timestamp, 0, uuid)).reduce(
     (array, index) => array.concat(grabPose3d(log, key + "/" + index.toString(), timestamp)),